一种基于小波神经网络的视觉惯性卫星紧耦合定位方法与流程
本发明属于无人驾驶多传感器数据融合技术领域。
背景技术：
在复杂的道路交通环境下，无人车必须具备实时、全面、准确的定位特性，其定位误差不能超过10cm，如果仍然使用单一的定位方式，无法适用于所有的场景，达到理想的定位效果。所以基于传感器融合的定位方法是现在定位技术研究的主要方向。卫星惯导组合系统是目前应用最广泛的定位系统。但是为了满足无人车厘米级的定位要求，需要使用高性能的惯性测量单元(imu)，限制了自主导航的发展。加入视觉里程计(vo)辅助定位，可以降低对imu精度的要求，适合无人车低成本的发展趋势。然而，全球导航卫星系统定位(gnss)易受到干扰和遮挡，影响gnss/imu/vo定位精度。
目前，卫星惯导组合导航系统主要采用基于位置或基于速度的松组合导航方式。在松组合方式下，卫星和惯导各自保持独立工作状态。该组合方式实现简单，应用广泛，但是无法很好地解决信号缺失的问题。当无人车经过丛林高山或者在城市环境中受到遮挡时，很容易导致有效卫星少于4颗，此时卫星导航无法提供准确的位置与速度信息，因此无法实现卫星与惯导的信息融合。为了解决这一问题，有研究将人工智能算法应用到组合导航领域，可以减小传感器的累积误差，加快解算速度，是未来组合导航发展的一个参考方向。近年来，神经网络算法已经被越来越多的应用到组合导航领域，基于神经网络的组合导航系统可以解决单传感器失效的问题，具有计算量小、实时性高、定位精度高的优点，具有很好的应用前景。但是目前的神经网络大多是基于卫星惯导的松组合系统，组合定位系统并没有充分的利用多传感器采集到的测量信息，系统在gnss长期失锁的场景中不具备很高的定位精度。
技术实现要素：
发明目的：为解决现有技术在gnss长期失锁的场景中不具备很高的定位精度的问题，本发明提供了一种基于小波神经网络的视觉惯性卫星紧耦合定位方法。
技术方案：本方提供了一种基于小波神经网络的视觉惯性卫星紧耦合定位方法，具体包括如下步骤：
步骤一：在载体上安装双目摄像机vo、惯性传感器imu和gnss接收机，组成定位系统；
步骤二：对系统进行初始化，将vo坐标系和imu坐标系均转换到gnss的世界坐标系下，并将vo、imu和gnss采集的数据进行时间同步；
步骤三：跟踪第k时刻图像帧中的fast特征点，从而得到第k+1时刻图像帧中的fast特征点；
步骤四：基于相邻两帧图像之间的fast特征点，计算该相邻两帧图像之间的vo重投影误差；
步骤五：对第k时刻与第k+1时刻之间imu测得数据进行预积分，得到imu误差传递模型，利用该模型对相邻两时刻之间imu测得数据进行噪声处理，从而计算出相邻两个时刻之间imu的惯性残差；
步骤六：根据步骤四中的重投影误差和步骤五中的惯性残差，计算得到第k+1时刻时载体位置的估计值；
步骤七：判断gnss信号是否被遮挡，若否，则根据第k时刻载体位置估计值和第k+1时刻载体位置估计值之差，设置gnss接收机搜索的频域范围，gnss接收机在设置的频域范围内对卫星信号进行三维搜索，测得第k+1时刻载体的位置信息，并且以gnss接收机测得的载体的位置信息与载体实际位置之间的残差、imu的惯性残差和vo的重投影误差的三者之和最小为目标，建立优化函数，并求解该函数得到第k+1时刻时载体位置的最优估计值；若gnss信号被遮挡，则转步骤八；
步骤八：将第k时刻和第k+1时刻之间的vo重投影误差与imu惯性残差之和输入至训练好的小波神经网络中，从而得到第k+1时刻载体位置估计值的修正量，并基于该修正量调整第k+1时刻时载体位置的估计值，从而得到第k+1时刻时载体位置的最优估计值；所述训练好的小波神经网络具体为：在gnss信号没有被遮挡的情况下，采集n组相邻两个时刻之间的vo重投影误差和imu惯性残差，将第q组重投影误差和第q组惯性残差之和作为小波神经网络的输入向量，将第q组重投影误差和第q组惯性残差对应的载体位置估计值与载体位置实际值之差作为第q组修正量，并将该修正量作为小波神经网络的输出向量，q＝1，2，…n，基于粒子群算法，对小波神经网络进行训练。
进一步的，所述步骤三中采用光流计算法跟踪第k时刻图像帧中的fast特征点。
进一步的，所述步骤四中采用多点透视法计算相邻两帧图像之间的vo重投影误差。
进一步的，所述步骤八中的粒子群算法采用改进的粒子群算法，具体为：首先让整个粒子群独立寻找最优位置，从而构造小生境环境，再将各粒子的搜索信息共享，指导每个粒子向最优位置搜索。
进一步的，所述步骤八中小波神经网络训练时激活函数选择莫雷小波函数。
进一步的，所述步骤七中的优化函数为：
其中，χ*为视觉-惯性-卫星紧耦合构成的系统状态变量的最优估计值，χ为视觉-惯性-卫星紧耦合构成的系统的状态变量，rg为gnss接收机测得的载体的位置信息与载体实际位置之间的残差，j为残差对状态变量求导的雅克比，rb(.)为imu惯性残差函数，
进一步的，所述步骤七中设置gnss接收机搜索的频域具体为：在[-10khz，10khz]的频域范围内，以500hz为频率步长，gnss接收机搜索的频域根据相邻两个时刻之间载体位置估计值之差的大小呈线性变化，差值越大则搜索的频域越大。
有益效果：本发明提出采用小波神经网络的方式解决gnss信号经常失锁的问题，小波神经网络是一种有监督学习网络，因为其引入了伸缩因子和平移因子，所以相比于一般的前向神经网络具有更好的逼近能力，收敛速度更快，小波神经网络的训练方法采用改进的小生境粒子群优化算法，可以得到最优的隐藏层神经元数量和最优的权值和阈值，从而得到观测值和优化位姿之差和修正量之间的映射关系，本发明具有很强的鲁棒性，可以在gnss信号长期失锁时仍保持较高的定位精度。
附图说明
图1是本发明的系统结构图。
图2是本发明的方法流程如。
图3是小波神经网络的示意图。
具体实施方式
构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。
如图1、2所示，本实施例提供了一种基于小波神经网络的视觉惯性卫星紧耦合定位方法，该方法采用的传感器包括双目摄像头、imu和gnss接收机三大部分。双目摄像头用于采集周围环境的图像信息并利用fast特征点和lk光流法解算相机的位姿估计。imu用于测量载体的三轴角速度和三轴加速度，由于imu会受到偏置的随机游走和高斯白噪声的影响，所以在估计系统状态时，还需要对陀螺仪和加速度计的偏置进行估计。gnss接收机用于测量载体在真实世界中的经纬度坐标，由于gnss接收机捕获需要搜索的时间较长，所以根据vo和imu得到的位置信息只对一定范围内的卫星信号进行检测，可以加快gnss接收机的捕获和跟踪过程。若gnss出现被遮挡的情况，也可以在重新接收到gnss信号的时候进行快速重捕。
下面对本实施例具体实现步骤进行说明。
步骤一：在载体上安装固定双目摄像头、imu传感器和gnss接收机，准确测量它们的安装方向和平移距离，计算出三个传感器之间的旋转外参和平移外参。根据imu与摄像头之间的旋转外参和平移外参，先将imu坐标系旋转并移动到vo坐标系下。再根据摄像头与gnss接收机之间的旋转外参和平移外参，将vo坐标系旋转并移动到gnss坐标系下。
步骤二：由于双目摄像头、imu传感器和gnss接收机具有不同的工作频率，其中，imu传感器的采集频率最高，gnss接收机的采集频率最低。为了得到时间同步的传感器数据，在采集传感器测量数据的时候，给三个传感器一个同步的基准时间，让三个传感器在采集各自的测量结果时，为测量值加上对应的时间戳。最后根据时间戳信息，得到时间同步的视觉测量值、惯性测量值和卫星测量值。
步骤三：对系统参数进行初始化，包括初始位置、初始速度、重力加速度、陀螺仪偏置。根据步骤一计算好的外参将相机测量得到的轨迹和imu测量得到的轨迹均旋转到世界坐标系下，将三个传感器的轨迹数据对齐。
步骤四：处理双目摄像头采集到的数据。首先提取第k时刻图像帧的fast特征点，然后在第k+1时刻的图像帧中根据光流计算跟踪上一帧提取到的fast特征点。其中，fast特征点的提取方法是对像素点周围像素的亮度进行比较，由于仅进行亮度比较而不用计算复杂的描述子，所以fast特征点法的速度非常快，具有较好的实时性。
在得到匹配好的fast特征点对之后，采用pnp(perspective-n-point，多点透视)算法，建立基于视觉的重投影误差模型，计算出视觉的重投影误差。
步骤五：由于imu传感器是组合定位系统所采用的三个传感器中工作频率最快的传感器，能够以200hz以上的频率实时采集载体的三轴角速度和三轴加速度，所以需要对相邻两个图像帧之间的imu测量值进行预积分。
得到imu测量值的预积分之后，推导imu的误差传递模型，计算得到基于imu预积分的惯性残差。
根据重投影误差和惯性残差，计算得到第k+1时刻时载体位置的估计值，该估计值为vo/imu组合定位结果。
步骤六：卫星信号通过天线和射频前端处理后进入gnss接收机的基带处理模块。gnns接收机对卫星信号的处理步骤是：第一步，根据相邻两个时刻之间载体的位置估计值之差，设置gnss的搜索频率范围，使用并行频率捕获算法在该范围的频域上对卫星信号进行三维搜索，得到可见卫星的prn(伪随机噪声码)码，载波频率的估计值和伪码相位的估计值；第二步，采用双频环路跟踪算法对捕获到的卫星信号进行码环跟踪和载频跟踪，得到载波频率和伪码相位的精确值；第三步，对卫星接收机跟踪环路输出的卫星信号号解调，提取位同步和帧同步信号；第四步，解码导航电文，得到系统时间、星历、历书、卫星时钟的修正参数和电离层延时模型参数；第五步，计算卫星位置，得到gnss观测的伪距和伪距率，进而得到载体的位置信息。建立gnss位置约束模型。根据vo和imu得到的初步位置信息，对一定范围内的卫星进行捕获和跟踪，从而解算gnss测量出的当前载体位置信息，并计算得到gnss位测量到的当前载体位置信息与当前时刻载体位置实际值之间的差值；
因为对所有卫星信号进行三维搜索的时间较长，所以根据vo和imu解算得到的位置信息变化量将卫星信号的搜索范围限定在一定范围内，提高gnss接收机捕获的速度。当gnss信号被遮挡时，可以通过这种方式使卫星信号再次出现时可以直接进入到跟踪模块进行跟踪。计算得到基于gnss的位置残差。
步骤七：在gnss信号能够稳定输出的情况下，采用改进的粒子群优化算法通过大量的实测数据对小波神经网络进行训练，得到vo和imu的残差之和与vo/imu系统输出定位结果调整的修正量二者之间的映射关系(所述修正量为第k+1时刻载体实际位置与vo/imu组合定位得到的第k+1时刻载体位置估计值之差)。在训练过程中，首先让整个粒子群独立寻找最优位置，以构造小生境环境，再将各粒子的搜索信息共享，指导粒子向最优位置的搜索。
本发明采用具有一个隐藏层的小波神经网络，其结构如图3所示。其中，输入向量vo和imu的残差之和，输出向量是vo/imu组合定位结果的修正量，激活函数选择morlet(莫雷)小波函数。morlet小波函数是高斯包络下的单频率副正弦函数，表示为：
其中，t为时间，ψ(t)是小波函数值；cm是重构时的归一化常数。选择使用小波神经网络，用小波函数的尺度伸缩因子和时间平移因子代替神经网络中输入层到隐藏层的权值和隐藏层的阈值，可以得到更优的收敛结果，同时可以加快网络收敛速度，避免陷入局部最优的问题。
步骤八：判断当前的gnss信号强度，若当前gnss信号可以稳定输出，则将gnss、imu和vo测量得到的残差进行联合非线性优化，待优化的状态变量为：
χ＝[x0，x1，…，xn，λ0，λ1，…，λm]
其中，χ是待优化的状态变量；x0，x1，…，xn是载体n个位姿的估计值，它是一个15维的状态向量，包括载体的位置、速度、姿态、imu陀螺仪偏置和imu加速度计偏置；λ0，λ1，…，λn为m个特征点的逆深度值。
通过非线性优化的方法寻找到一个最优的状态变量估计值使组合定位系统的残差之和最小，表示为：
其中，χ*为视觉-惯性-卫星紧耦合构成的系统状态变量的最优估计值，x为视觉-惯性-卫星紧耦合构成的系统的状态变量，rg为gnss接收机测得的载体的位置信息与载体实际位置之间的残差，j为残差对状态变量求导的雅克比，rb(.)为imu惯性残差函数，
当gnss信号受到遮挡物干扰，无法稳定输出时，通过训练好的小波神经网络得到一个修正值。通过这个修正值对vo/imu系统的输出定位结果进行调整，再输出修正后的定位结果。
本发明一种基于小波神经网络的视觉惯性卫星紧耦合定位方法分为在线训练和预测输出两种模式，当gnss信号可以稳定输出时，将gnss观测值、imu观测值和vo观测值放在一起进行联合优化，并通过gnss/imu/vo组合导航的最终输出结果对神经网络进行在线实时训练。当gnss信号失锁时，采用已经训练好的小波神经网络对vio系统的输出进行校正，从而减少组合导航系统的误差，提高系统的鲁棒性和定位精度。
本发明一种基于小波神经网络的视觉-惯性-卫星紧耦合定位系统的技术方案为：安装固定双目摄像头、惯性传感器和卫星接收机，测量并计算三个传感器之间的外参，并采集时间同步的传感器观测数据。通过采集到的时间同步数据对gnss/imu/vo组合定位系统进行初始化，得到载体的初始位置、初始速度、当地的重力加速度和陀螺仪零偏，将视觉轨迹和惯性轨迹旋转对齐。采用fast特征点法和lk光流法跟踪图像，得到视觉重投影误差模型。对imu数据进行预积分，并进行协方差传递推导，得到imu预积分残差模型。gnss接收机捕获跟踪卫星信号，解算得到载体当前的位置信息，并计算gnss的位置残差。判断能否接收到gnss信号，若可以接收到稳定的gnss信号，则将vo、imu和gnss计算到的残差相加，构建非线性优化问题，通过最小二乘法求解得到载体当前的最优位姿估计值。若当前无法接收到gnss信号，则将vo和imu的残差进行联合非线性优化，得到组合定位系统的初始位姿估计值，再根据训练好的小波神经网络对初始位姿估计值进行修正，输出校正后的定位结果。
上面结合附图对本发明的实施方式作了详细说明，但是本发明并不限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下做出各种变化。
- 还没有人留言评论。精彩留言会获得点赞！