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

摘要

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

主权项

1.一种自适应惯导和视觉组合导航方法，包括如下步骤：步骤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：其中，α表示sigma点的散布程度，β用来描述状态量的分布信息；步骤8、获得自适应因子的均值m_α，并判断：当α_i≤m_α时，计算滤波器卡尔曼滤波增益K_k并更新误差协方差矩阵P_k|k：其中，P_k|k‑1表示步骤2得到的误差状态中的误差协方差矩阵；当m_α<α_i≤m_α+σ_α时，对误差协方差矩阵进行调整，即：当m_α+σ_α<α_i≤m_α+σ_α²时，对误差协方差矩阵和卡尔曼滤波增益均进行调整，即：当α_i>m_α+σ_α²时，对误差协方差矩阵进行快速调整，即：最后对系统误差状态进行估计：步骤9、使用步骤8得到的系统误差状态对系统标称状态校正，得到定位结果；重复步骤1到步骤9，获得连续的车辆导航定位信息。