本发明属于组合导航领域，具体涉及一种捷联惯性导航系统/视觉里程计组合导航方法。
背景技术：
近年来，导航领域中单独的导航子设备或者子系统不断的发展，其性能日趋完善，各自的优势和局限性日趋明显。随着科学技术的飞速发展，而任何一种单独的导航子设备或者子系统不可能完全实现日益增长的导航需求的目的，因此可以实现优势互补的组合导航技术的应用正在不断扩展，并受到了越来越广泛的重视。
由于捷联惯性导航系统(strapdowninertialnavigationsystem，sins)具有低成本、小体积、全自主、隐蔽性好、采样频率高等优势，被广泛应用于多种领域；而全球导航卫星系统具有单点定位精度高、误差不随时间发散等优点，因此将捷联惯性导航系统与全球导航卫星系统进行组合可以实现优势互补，从而提高整个导航系统的精度。目前捷联惯性导航系统/全球导航卫星系统组合导航是应用最为广泛的一种组合导航形式。然而捷联惯性导航系统/全球导航卫星系统组合导航系统对全球导航卫星系统信息的依赖程度过大，而全球导航卫星系统信息又绝对依赖于外部卫星信号。因此在森林覆盖区、建筑物密集区、室内、水下等环境中时，全球导航卫星系统信号就很容易受到干扰而产生信号拥堵或堵塞，从而造成全球导航卫星系统定位精度严重下降，进而大幅度降低整个组合导航系统的定位精度，因此需要寻求一种新的组合导航方式。
基于视觉传感器的视觉里程计(visualodometer，vo)是目前新兴的一种导航设备，具有价格低、耗能少、信息量丰富等优势，因此视觉里程计迅速得到了广泛的关注与应用，基于捷联惯性导航系统/视觉里程计的组合导航技术也称为导航领域内的一个十分重要的研究方向。
视觉里程计是利用视觉传感器捕获周围环境的图像信息，然后通过计算机视觉处理算法对图像或者图像序列进行处理，从而实现对周围环境的理解，实现对运载体的导航与定位。因此如何高效的从图像序列中提取出有效的周围环境信息，即特征提取与匹配问题，成为视觉辅助导航技术研究中极为重要的内容之一。为提高视觉里程计的计算效率，特征提取与匹配方法需要具有高效性，而加速分割检测特征(featurefromacceleratedsegmenttest，fast)方法由于具有简单有效、计算速度快等优势，被应用于视觉特征与匹配过程中。但是fast仅仅对一小部分像素进行了处理，所以其对噪声特别敏感，极易受到周围环境的影响。针对这一问题，本发明提出了一种改进的fast特征提取方法，通过降低误匹配率来抑制环境噪声对视觉里程计算法精度的影响，利用非线性滤波器容积卡尔曼滤波(cubaturekalmanfilter，ckf)完成对捷联惯性导航系统/视觉里程计组合导航系统状态的估计，从而提高捷联惯性导航系统/视觉里程计组合导航系统的定位精度和鲁棒性。
技术实现要素：
本发明的目的是提供一种能够同时定位精度更高和鲁棒性更好的捷联惯性导航系统/视觉里程计组合导航方法。
本发明的目的是通过以下步骤来实现的：
步骤1：在运载体上安装双目视觉里程计、光纤陀螺惯性导航系统，对捷联惯性导航系统/视觉里程计组合导航系统进行预热，并采集各个传感器的数据；
步骤2：根据视觉里程计中的双目摄像头采集到的图像序列，利用fast方法对图像序列中的特征进行提取，并利用基于随机抽样一致的特征匹配方法完成特征匹配，并根据视觉里程计原理计算出运载体的运动信息；
步骤3：建立捷联惯性导航系统/视觉里程计组合导航系统的非线性状态方程；
步骤4：以捷联惯性导航系统与视觉里程计测得的速度误差为观测量，建立捷联惯性导航系统/视觉里程计组合导航系统的量测方程；
步骤5：利用非线性滤波器容积卡尔曼滤波完成捷联惯性导航系统/视觉里程计组合导航系统的时间更新和量测更新，对系统状态进行估计，实现捷联惯性导航系统/视觉里程计组合导航系统的导航定位。
进一步地，在步骤2中，利用fast方法对图像序列中的特征进行提取，并利用基于随机抽样一致的特征匹配方法完成特征匹配，并根据视觉里程计原理计算出运载体的运动信息，其具体方法为：
1)通过视觉里程计的双目视觉里程计获取运载体周围环境的图像对，并对图像序列进行校正；
2)对校正后的图像序列利用fast进行图像特征提取，并生成特征向量；
3)对从图像对中所提取的特征点，利用最近邻和次最近邻的距离比方法进行图像特征粗匹配；
4)利用随机抽样一致方法对完成粗匹配后的特征点对进一步提纯，完成图像特征的精匹配；
5)根据三角测量原理和左右两张图像中成功匹配的特征点对信息，计算得到每个匹配特征点的3d坐标；
6)对当前帧和下一帧两相邻时刻的左侧图像中的3d特征点进行匹配，计算出双目视觉的运动信息，包括旋转矩阵和平移矩阵；
7)根据运载体与视觉里程计之间的坐标系变换，计算出运载体相对初始时刻的位移和姿态变化，得到运载体的运动信息。
本发明的优势在于：(1)由于利用改进的特征匹配方法，对特征点进行了粗匹配和精匹配两次匹配过程，有效剔除了误匹配点对，减小了由外界干扰引起的误匹配点对对视觉里程计定位精度的影响，从而提高了捷联惯性导航系统/视觉里程计组合导航系统的定位精度和鲁棒性；(2)利用非线性滤波器容积卡尔曼滤波完成对捷联惯性导航系统/视觉里程计组合导航系统状态的估计，进一步提高了组合导航系统的精度。
附图说明
图1为本发明的流程图；
图2为本发明中改进的双目视觉里程计算法的流程图；
图3运载体的真实运动轨迹；
图4为利用本发明方法和传统方法时运载体的运动轨迹对比图；
图5为利用本发明方法和传统方法时运载体位置误差对比曲线。
具体实施方式
以下结合具体实施案例，对本发明进行详细说明。
本发明是一种基于随机抽样一致的改进的捷联惯性导航系统/视觉里程计组合导航方法，结合图1～2所示的算法流程框图，其具体实施方式为：
步骤1：在运载体上安装双目视觉里程计、光纤陀螺惯性导航系统以及全球定位系统，其中捷联惯性导航系统与全球定位系统工作于组合导航模式并作为数据基准。对运载体上的各传感器和设备进行预热，并采集各个传感器和设备的数据；
步骤2：利用视觉里程计中的双目摄像头采集到运载体周围环境的图像序列，并完成对图像序列的校正；对校正后的图像序列利用fast方法对图像进行分割测试提取候选特征点，其中每个像素p点，其周围以3个像素为半径的圆上的像素点可以分为三类：
其中ip为中心采样点p的像素值，ip→x为p点周围圆形模板上的点x的像素值，t是比较阈值。随后，利用迭代决策算法来选择具有最大信息增益的像素点作为特征点。
设kp为一参数，当p为特征点，kp为true，否则为false。而参数kp的熵可以用公式(3-9)来表示：
其中c＝|{p|kpistrue}|表示特征点的数目，
那么，图像中某一个特定像素的信息增益的计算公式为：
h(p)-h(pd)-h(ps)-h(pb)
对上述三类像素依次进行反复计算，直到熵为0，即可提取出候选特征点。
随后，利用非极大值抑制方法精确确定特征点，定义一个特征点响应函数
根据定义的特征点响应函数计算每个候选特征点的响应函数值，然后利用非极大值抑制方法对非特征点进行排除，筛选出真正的特征点。
利用小波响应分配特征点方向并生成特征描述向量，计算相应特征点的欧式距离，设两幅图中的特征点对的坐标分别为a(xa,ya)、b(xb,yb)，则两点之间的欧氏距离为：
分别计算两幅图中特征点之间的欧氏距离的最小值与次最小值，即最近邻dest和次最近邻dnd，设定的阈值为r，则
若dest/dnd＜r，则最近邻距离对应的两个特征点相互匹配；
若dest/dnd＞r，则最近邻距离对应的两个特征点相互不匹配。
从而完成特征点对的粗匹配，在此基础上，利用随机抽样一致算法对进行粗匹配后的特征点对进一步提纯，剔除误匹配点对，减少误匹配点对的干扰和影响，使得特征点的匹配更加精确，这样就可以减小误匹配特征点，提高匹配精度，从而减小噪声或者图片质量降低对特征提取与匹配的影响，从而完成特征点对的精确匹配。
然后根据三角测量原理计算成功匹配的特征点的3d坐标(xi,yi,zi)，对当前帧和下一帧两相邻时刻的左侧图像中的3d特征点进行匹配，同样是先进行粗匹配再进行精匹配。最后，计算出双目视觉传感器的旋转矩阵r(k→k+1)和平移矩阵t(k→k+1)，根据运载体与视觉里程计之间的坐标系变换，计算出运载体相对初始时刻的位移和姿态变化，从而得到运载体的运动信息，包括位置(xi,yi,zi)和姿态
步骤3：充分考虑实际系统的非线性特征，以捷联惯性导航系统为主系统，捷联惯性导航系统的速度误差、姿态误差、位置误差以及器件误差微分方程为：
其中，
考虑运载体的经纬度误差
则系统的非线性状态方程为：
xk+1＝f(xk,wk)
其中wk为系统的过程噪声矩阵：w＝[02×1waxwaywgxwgywgz05×1]t。
步骤4：根据捷联惯性导航系统和视觉里程计测量的运载体的运动信息，以捷联惯性导航系统与视觉里程计测得的速度误差为观测量，建立捷联惯性导航系统/视觉里程计组合导航系统的量测方程
其中量测噪声为η＝[ηxηy]t。
步骤5：利用非线性滤波器容积卡尔曼滤波完成捷联惯性导航系统/视觉里程计组合导航系统的时间更新和量测更新，对系统状态进行估计，从而完成捷联惯性导航系统/视觉里程计组合导航系统的导航过程。
0
本发明的效果通过如下方法得到验证：
利用德国卡尔斯鲁厄理工学院和丰田技术研究所(karlsruheinstituteoftechnologyandtoyotatechnologicalinstitute，kitti)的开源数据对本发明的性能进行验证。kitti研究所的车载实验是以一辆名为annieway的自主车为平台，搭载有多种传感器来采集周围环境中的数据，传感器设备如表1所示：
表1kitti实验室的车载实验设备
本发明仅采用imu/全球定位系统组合导航系统以及灰度摄像机的数据即可，其中pointgreyflea2摄像机是由加拿大pointgrey公司生产的单目摄像机，因此需要两个摄像机组成摄像机对来代替双目视觉系统。摄像机对均安装在自主车的顶部，安装平面与地面近似水平，两灰度相机之间的安装基线近似为54cm，相机的采样频率为10帧/秒，图像的大小约为1382*512像素。oxtsrt3003系统是imu/gps组合导航系统，它被安装在自主车的后部，其采样频率为0.1hz，其输出的位置和姿态可以作为自主车的真实值。
使用的数据采集环境为住宅区，图像序列共1106对，试验时间约为110.6s左右，试验自主车的真实运行轨迹如图3所示。试验环境中树木和房屋比较多，所获取的图像中光照变化比较大，图像质量会受到影响，为验证改进特征匹配算法的性能，从图像序列中任选一对图像，传统最近邻匹配方法作为对照方法，则fast特征提取与匹配结果如表1所示。利用基于未改进特征匹配方法的传统捷联惯性导航系统/视觉里程计组合导航方法作为参考对照，采用本发明对该次试验数据进行分析，试验结果如图4和图5所示。
表1两种特征提取算法与匹配结果对比
从表1可以看出，采用本发明所提改进的fast特征提取与匹配方法对图像特征进行提取与匹配，误匹配率从11.3％下降到了4.7％，因此，本发明所提匹配算法可以有效提高特征匹配的匹配精度，抑制环境对视觉里程计定位的影响。从图4和图5可以看出，与传统方法相比，利用本发明方法所取得到的运动轨迹更接近运载体的真实运动轨迹，捷联惯性导航系统/视觉里程计组合导航系统的定位误差最大值约为12m，而传统方法的定位误差最大值约为17m左右。综上所述，本发明提供的方法具有更加精确的估计精度和鲁棒性，可以有效地提高捷联惯性导航系统/视觉里程计组合导航系统的导航定位能力。