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

权利要求书

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_f,i通过滤波器新息Δ_i得到，预测协方差矩阵为步骤6中更新滤波器观测量所得预测协方差矩阵：

然后通过对角算子获取上述事实协方差矩阵P_f,i和预测协方差矩阵的对角矩阵：

D_f，i＝diag(P_f，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_α+σ_α²时，对误差协方差矩阵进行调整，即：

η_i＝α_i-m_α,

其中，σ_α²表示自适应因子的方差；

当m_α+σ_αα_i≤m_α+σ_α²时，对误差协方差矩阵和卡尔曼滤波增益均进行调整，即：

η_i＝α_i-m_α,

当α_i＞m_α+σ_α²时，对误差协方差矩阵进行快速调整，即：

η_i＝α_i-m_α,

最后对系统误差状态进行估计：

步骤9、使用步骤8得到的系统误差状态对系统标称状态矩阵校正，得到定位结果；重复步骤1到步骤9，获得连续的车辆导航定位信息。

2.如权利要求1所述的一种自适应惯导和视觉组合导航方法，其特征在于，所述步骤0中，在所述系统误差状态矩阵中，添加最近两个时刻的相机位姿；在所述步骤9中，得到定位结果后，用最近两个时刻的相机位姿更新系统误差状态矩阵中历史相机位姿，继而更新误差协方差矩阵；然后再返回步骤1，进入下一次定位解算。