[发明专利]一种视觉导航/惯性导航的全组合方法

说明书

技术领域

本发明涉及一种视觉导航系统和惯导系统的组合方式，尤其是涉及一种视觉导航/惯性导航的全组合方法。

背景技术

视觉导航，又称为视觉测程法(Visual Odometry，VO)，基本原理是：载体前进过程中，利用立体相机获取周围场景的影像，依靠特征点在左右影像间的匹配，以及前后帧影像间的追踪，以共线方程为基础列立观测方程，根据最小二乘原理平差求取相机的空间位置和姿态。视觉导航是一种高精度的自主导航方式，但是作为一种航迹推算方式，其导航误差随时间积累，而且算法的稳定性一直是个难题，有时候影像视场中的背景单一或过于相似，或者载体车的动态性过大，例如转弯角度过大，使得相邻影像之间的重叠度太小，这些都会导致视觉导航解算失败，因此要借助其它手段来增强算法的稳定性。

惯导系统(Inertial Navigation System，INS)以牛顿力学定律为工作原理，利用安装在载体上的惯性测量元件(Inertial Measurement Unit，IMU)来敏感载体的角速度和加速度信息，通过积分运算得到载体的位置、速度和姿态等导航参数。惯性导航具有完全的自主性和抗干扰性，短时间内具有高精度，但是由于积分原理，其误差随时间累积，因此长时间工作的精度较差。为了减少误差累积，要引入外部观测值来对惯导系统进行修正。

因此，视觉导航与惯性导航作为两种自主导航方式，具有很好的互补性。两者的组合可以解决无GPS环境下的导航问题，对于室内导航、机器人路径规划、无人机自主着陆，自动行驶车的导航、地下交通导航定位、矿井作业安全等应用需求具有重要意义。

目前，视觉导航与惯导系统的组合方式一般有三类：(1)结合型，一般依靠视觉系统提供载体的位置信息，惯导系统提供载体的姿态信息；(2)修正型，这种组合方式是以一种导航系统去修正另一种导航系统；(3)融合型，一般采用卡尔曼滤波来实现两者的数据融合。以上三类组合方式中，结合型属于低层次的组合，常用于精度要求不高的场合，而在高精度的场合中通常采用修正型或融合型。

在修正型组合方式中，一般是利用惯导系统来修正视觉系统，实现过程一般是这样的：(1)相机获取当前时刻的影像，(2)对当前时刻的左右影像进行特征点提取和匹配，并与前一时刻的影像进行匹配，然后通过后方交会计算相机的外方位元素(位置和姿态)，(3)利用IMU的输出数据计算载体的俯仰角和翻滚角，以及航向角增量，(4)利用载体的俯仰角和翻滚角以及航向角增量，通过卡尔曼滤波对相机位置和姿态进行修正，(5)利用修正后的相机位置和姿态，通过前方交会求特征点的物方坐标，(6)重复以上步骤。

融合型组合方式的实现过程一般是这样的：(1)相机获取当前时刻的影像，(2)对当前时刻的左右影像进行特征点提取和匹配，并与前一时刻的影像进行匹配，然后通过后方交会计算相机的外方位元素(位置和姿态)，(3)利用相机的位置和姿态作为量测值，采用卡尔曼滤波估计IMU的位置和姿态，(4)将滤波后得到的IMU位置和姿态传递给相机，然后通过前方交会求特征点的物方坐标，(5)重复以上步骤。

从以上可以看出，修正型的组合方式中，仅仅利用IMU的输出数据提供姿态，而没提供位置信息，因此这种方式没能最大程度上的利用惯导系统所提供的导航信息。融合型的组合方式能较好的利用视觉系统和惯导系统的信息，使得两者都可以得到修正，但是这种方式没有考虑到影像特征点物方坐标的先验信息，组合效果不能达到最佳。实际上在视觉导航中，特征点在不同影像之间的传递是关键，当前影像中提取的特征点，大部分在上一帧影像中已经通过最小二乘平差获得了其物方坐标，且坐标精度也已知，因此必须对精度信息加以考虑。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种理论严密、稳定、高效的视觉导航/惯性导航的全组合方法。

本发明的目的可以通过以下技术方案来实现：

一种视觉导航/惯性导航的全组合方法，该方法包括以下步骤：

1)视觉导航解算：相机在载体运动过程中获取影像，以共线方程为基础列立观测方程，采用最小二乘原理平差求取载体位置和姿态参数X_vo，并计算参数之间的方差-协方差阵P(X_vo)；

2)惯性导航计算：在当地水平坐标系中做导航计算，得到每个时刻载体的位置、速度和姿态参数X_ins，同时根据惯导系统的误差方程，按协方差传播律，计算参数之间的方差-协方差阵P(X_ins)；