[发明专利]一种自适应惯导和视觉组合导航方法

说明书

本发明公开了一种自适应惯导和视觉组合导航方法在现有惯导和视觉组合导航方法的基础上，设计一种自适应无迹卡尔曼滤波对惯导和视觉进行组合；该方法考虑了车辆行驶时路面颠簸导致相机振动引起的噪声，设计了反映相机振动噪声水平的自适应因子，并根据不同的噪声水平自适应的调整组合导航算法中的卡尔曼滤波增益和误差协方差矩阵，确保组合导航系统定位结果的可靠性，是一种鲁棒的定位方法；由于本发明的方法无需事先对振动噪声引起的误差建模补偿或者轨迹闭环校正，因此对环境的适应性较强；本发明在惯导15维误差量的基础上，设计了添加相机位姿的滤波器状态量，实现了惯导和视觉的紧组合，是一种精度较高的定位方法。

技术领域

本发明属于自动控制技术领域，具体涉及一种自适应惯导和视觉组合导航方法。

背景技术

与传统车辆相比，无人驾驶汽车作为一种由环境感知、导航定位、运动控制和路径规划等技术组成的智能车辆，近些年取得了很快的发展，受到了社会的广泛关注。在无人驾驶汽车涉及的技术中，导航定位是整个系统可以正常工作的关键和基础。

目前无人驾驶汽车上广泛使用的卫星导航和惯导的组合导航系统在开阔环境下可以得到准确的车辆定位，但在卫星信号拒止环境如城市峡谷中，由于信号遮挡和多路径效应的影响，卫星导航系统无法对惯导的累积误差进行有效校正，组合导航系统的定位精度快速下降，甚至无法定位。因此可以采用惯导和视觉组合的方案，利用两种传感器特性的互补性，实现在卫星信号拒止环境下的导航定位。然而目前主流的方法没有考虑路面颠簸可能导致的测量噪声，容易导致导航系统的定位误差发散。

发明内容

有鉴于此，本发明的目的是提供一种自适应惯导和视觉组合导航方法，可保证组合导航系统定位结果的可靠性，具有与更好的鲁棒性。

一种自适应惯导和视觉组合导航方法，包括如下步骤：

步骤0、初始化系统标称状态矩阵系统误差状态矩阵以及初始误差协方差矩阵P_0|0；

步骤1、捷联惯性导航系统解算，得到系统标称状态矩阵同时，根据运动动力学关系计算系统转移矩阵F_d和系统过程噪声方差矩阵Q_d；

步骤2、利用系统矩阵F_d和系统过程噪声方差矩阵Q_d对系统的误差状态进行一步预测；

步骤3、对相机采集的视觉图像进行特征提取与特征匹配，使用单点RANSAC法对误匹配点移除，得到连续三帧图像之间的特征匹配对；

步骤4、计算得到2n+1个sigma点；n表示系统状态维数；

步骤5、用2n+1个sigma点校正系统标称状态结合特征匹配结果预测观测值：

步骤6、更新当前第i次导航解算的滤波器观测量预测协方差矩阵以及观测量和状态量的协方差矩阵

步骤7、获取第i次导航解算的观测量的事实协方差矩阵P_f,i和预测协方差矩阵P_c,i，其中事实协方差矩阵P_f,i通过滤波器新息Δ_i得到，预测协方差矩阵P_c,i为步骤6中更新滤波器观测量所的预测协方差矩阵：

然后通过对角算子获取上述两个矩阵的对角矩阵：

D_f,i＝diag(P_f,i),D_c,i＝diag(P_c,i)

将D_f,i和D_c,i两个矩阵对应的对角线元素相除后取均值，得到第i次导航解算自适应因子α_i：