一种适用于GNSS拒止环境下的鲁棒自适应VIO导航定位方法

本发明涉及gnss拒止环境下的车载导航定位，具体为一种适用于gnss拒止环境下的鲁棒自适应vio导航定位方法，用于提高车辆在无法接受到gnss信号情况下的导航定位精度。

背景技术：

1、近年来，无人车(unmanned ground vehicles， ugvs)作为焦点被大量研究，其最常用的导航定位方式是将全球卫星导航系统(global navigation satellite system，gnss)和惯性导航系统(inertial navigation system， ins)联合成gnss/ins组合导航系统。伴随着多个卫星系统的出现，如我国北斗bds、美国gps等全球卫星导航系统，在空旷环境下，车载gnss/ins组合导航已经可以提供较好的定位精度。然而，在gnss拒止环境下，ugvs的导航定位精度将会受到严重干扰。因此，目前ugvs研究领域中最大的挑战即为在gnss拒止环境下的自主导航定位问题。

2、在gnss拒止环境下的车载gnss/ins组合导航定位方法通常可以分为两大类：第一，先进的融合算法，如神经网络算法等，被用来模拟gnss的量测信息，从而可以一定程度上减小定位误差。但是，这些算法在长时间gnss信号丢失情况下的导航精度并不能满足实际应用的需求。第二，寻求一种其它传感器进行组合导航定位，其一般与惯性测量单元(inertial measurement unit， imu)具有良好的优势互补关系。而随着视觉技术的快速发展，近年来，视觉导航已经成为ugvs导航领域中不可或缺的一部分。

3、视觉里程计(visual odometry， vo)作为视觉导航中的一个重要分支，是一个重要的研究热点，它能够利用相机，如单目、双目和多目相机等，获取周围环境的图像序列，然后基于图像间的几何关系估计出相机的位姿运动，其中，单目相机由于低的功耗、成本、体积和图像处理消耗等被广泛的应用。但是，单目相机在一些挑战环境如快速运动、无纹理环境和快速光照变化的场景是不足以胜任的，并且具有尺度不确定性的问题，难以恢复环境的尺度信息。单目相机和ins之间具有良好的优势互补关系，两者联立可以组成vio(visual-inertial odometry)导航定位系统。单目相机可以减少imu零偏引起的误差，与此同时，imu可以帮助单目相机消除尺度不确定性，并且在视觉跟踪丢失的时候提供观测量。鉴于vio组合导航系统由于低功耗、低成本、高度自主等优势，已经逐渐成为gnss拒止环境下提高ugvs导航精度的研究热点和重要的发展方向。

4、目前，多种针对vio组合导航系统的滤波器结构已经被研究，其中比较流行的是多状态约束卡尔曼滤波(multi-state constraint kalman filter，msckf)。为了避免对特征点三维空间位置的估计和简化msckf的量测更新过程，减小vio组合导航算法的计算复杂度，可利用对极几何和三焦点张量几何约束模型建立vio的量测方程，且其不需要计算特征点的三维空间位置坐标和环境重构。更进一步的，vio组合导航系统是一个高维的非线性系统，有学者提出利用非线性性能较好的容积卡尔曼滤波器和msckf结合构成一种基于容积的msckf(cubature msckf，cmsckf)算法，并获得了较好的导航定位性能。

5、但是此类算法都属于标准卡尔曼滤波范畴，在系统噪声统计特性精确已知、系统模型准确的时候，可以取得较好的滤波精度。而在这些假设不成立的时候，如系统噪声为有色噪声、统计特性不精确已知以及系统模型不准确等情况，会产生较大的滤波误差。在实际ugvs运动过程中，vio导航系统受其本身和外部环境的不确定性因素的影响，这些情况是不可避免的，其导航定位结果的鲁棒性和可靠性将会受到严重挑战。因此，如何提高vio组合导航在复杂环境中的鲁棒自适应能力，是ugvs在gnss拒止条件下提高导航定位精度亟需解决的核心难题。

技术实现思路

1、为提高复杂环境下无gnss信号时ugvs的导航定位精度，本发明提出一种适用于gnss拒止环境下的鲁棒自适应vio导航定位方法。

2、该方法首先利用imu测量单元捕获ugvs的加速度和角速度信息，同时利用相机捕获其周围的环境信息。然后，利用imu测量数据和相机捕获图像之间的内在联系，基于多幅图像之间的对极几何和三焦点张量几何关系建立vio的量测约束模型。接着，在滤波算法部分，将h∞准则引入到cmsckf中以提高vio滤波算法的鲁棒性，并进一步设计一种基于限定记忆指数加权法优化的系统不确定性程度表征方法，在此基础上，设计了一种三段式鲁棒自适应vio滤波算法。最终，综合利用标准卡尔曼滤波算法和h∞滤波算法，将两者进行优势互补，实现滤波算法整体性能的提升，使得vio组合导航不但具有较高的滤波精度，还具备较好的鲁棒性，从而提高ugvs在复杂环境下的导航定位性能。

3、本发明的技术方案为：

4、所述一种适用于gnss拒止环境下的鲁棒自适应vio导航定位方法，包括以下步骤：

5、步骤1：利用惯性测量单元(inertial measurement unit， imu)和相机获取ugvs的加速度、角速度信息和周围环境的图像信息。

6、在gnss拒止环境中，由于无法接收到gnss信号，ugvs中常用的gnss/ins组合导航的定位精度将受到严重影响。而相机是一个功耗低、体积小、质量轻并且可提供丰富的周围环境信息的传感器，因此为ugvs安装一个前向相机，用于捕获车辆前方的环境信息，同时利用imu获取ugvs的三轴加速度和三轴角速度信息，将imu和相机组成vio(visual-inertialodometry)组合导航系统。

7、步骤2：构建vio组合导航系统的状态模型和量测模型。

8、首先，基于惯性导航系统(inertial navigation system， ins)的15维误差状态模型，并借鉴当前比较流行且性能优越的多状态约束卡尔曼滤波(multi-stateconstraintkalman filter， msckf)架构的思想，建立vio的状态方程。其中，状态变量的选取如下所示：

9、

10、

11、其中，i表示imu坐标系，在vio中，imu与车体固联，原则上希望imu在安装过程中与载体坐标系完全重合，因此，在不考虑安装误差角或者在已经经过补偿后，可以认为imu坐标系就是“右前上”载体坐标系；g是全局坐标系，在本发明中，选择相机第一帧图像所对应的imu坐标系为全局坐标系g系；和分别为ins的姿态、速度和位置误差，bg和ba分别为陀螺仪和加速度计零偏；和分别表示当前ins误差状态对应的前两帧的姿态误差和位置误差。

12、然后，根据ins和视觉相机量测信息之间的内在联系，基于多个图像之间的对极几何和三角点张量几何关系，构建vio的量测约束方程。

13、步骤3：根据步骤2中构建的vio组合导航系统的状态模型和量测模型，设计合适的滤波算法进行滤波估计。

14、步骤2中构建的vio模型为非线性的，基于容积卡尔曼滤波器和msckf算法构成基于容积的msckf(cmskcf)滤波器，以获得更好的非线性滤波性能。同时，为提高算法的鲁棒性，保证vio组合导航系统在复杂环境下的导航性能，进一步将h∞准则引入到cmsckf中，构成一种hcmsckf滤波算法。

15、为了合理利用设计的cmsckf算法和hcmsckf算法，将两者进行优势互补，本发明采用三段式鲁棒自适应vio滤波算法。在vio导航系统正常工作时，利用cmsckf滤波算法进行滤波估计，从而获得高精度的滤波结果；而当vio系统具有不确定性时，引入hcmsckf算法以提高系统的鲁棒性，此时分为两类情形：一种是当不确定度较低的时候，综合利用cmsckf和hcmsckf同时进行滤波，并进行加权优化；另外一种是当不确定度较高的时候，只利用hcmsckf进行滤波估计。最终，实现vio滤波算法整体性能的提升，使得其在复杂环境下具有较高滤波精度的同时，还具备较好的鲁棒自适应能力，从而提高ugvs的导航定位性能。

16、进一步的，步骤2中建立的vio状态方程为：

17、

18、其中，为扣除陀螺仪零漂后的角速度的反对称矩阵，表示扣除加速度零偏后的加速度的反对称矩阵，表示i系到g系的旋转矩阵的四元数表达式，表示的方向余弦矩阵，nbg和nba为陀螺仪和加速度计的高斯白噪声ng和na的一阶导数。

19、进一步的，步骤2中建立的vio量测约束方程

20、

21、

22、其中，zi(1)和zi(2)是zi的第一行和第二行元素，具体表示为连续三幅图像中第一和第二幅图像、第二和第三幅图像间的匹配特征点对应的对极几何关系；zi(3)是zi的第三行元素，具体表示连续三幅图像中三幅图像间匹配特征点对应的三焦点张量几何关系；pj表示特征点在三幅图像中所对应的像素坐标系下的像素位置；k为相机的内参矩阵，其参数值相机由生产厂商告知，或者通过相机标定技术获得；ti为张量描述；r12、t12和r23、t23分别表示第1幅图像对应的相机坐标系相对第2幅图像对应的相机坐标系，以及第2幅图像对应的相机坐标系相对第3幅图像对应的相机坐标系之间的旋转和位移矩阵旋转和位移矩阵。

23、进一步的，对于vio系统不确定度，借鉴滑动窗口的思想，并强调新近数据的作用，逐渐遗忘陈旧数据的作用，这里基于限定记忆指数加权法设计vio导航系统不确定度的表征方法：

24、

25、

26、其中，zj为实际观测值，为预测值，为预测残差向量，s为调节因子，满足s≥1，其具体取值取决于实际系统，在本专利中选择s＝1.5，tr[·]表示求括号中矩阵的迹，h(·)为步骤1中构建的vio的量测方程，ξi，j/j-1为利用容积卡尔曼滤波算法进行计算是的采样点，rj为量测噪声的协方差矩阵，ωj为不同时刻的加权因子，并满足下式条件：

27、

28、其中，b为遗忘因子，在本发明中选择b＝0.9，m为滑动窗口的宽度，本发明中取m＝10。

29、进一步的，综合利用cmsckf和hcmsckf同时进行滤波，并进行加权优化时，cmsckf和hcmskf对应的加权因子wcmsckf和whcmsckf分别为：

30、

31、whcmsckf＝1-wcmsckf

32、其中，ρ1和ρ2表示系统不确定度的两个阈值。

33、有益效果

34、本发明立足于gnss拒止环境下ugvs的导航定位问题，针对目前vio组合导航算法存在的不足，提出了一种适用于gnss拒止环境下的鲁棒自适应vio导航定位方法。该方法首先利用imu和相机图像之间的内在联系，基于对极几何和三焦点张量几何关系建立了vio的量测约束模型，从而避免了求取图像特征点三维空间位置的问题，大大减小了算法的计算量。并将h∞准则引入到现有cmsckf滤波算法中，达到了提高了vio算法鲁棒性的目的。同时，针对h∞滤波算法牺牲精度以换取鲁棒性的不足，进一步设计了一种基于限定记忆指数加权法优化的系统不确定性程度表征方法，并借鉴igg iii模型的思想提出了三段式鲁棒自适应vio滤波算法，使得vio导航系统在复杂环境下，具备高定位精度的同时还具备高鲁棒性，提高了ugvs在gnss拒止环境下的导航定位性能。

35、本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。