[发明专利]一种基于序贯检测的惯性/视觉里程计组合导航方法

权利要求书

1.一种基于序贯检测的惯性/视觉里程计组合导航方法，其特征在于，其包括如下步骤：

1)单特征点的线运动测量；

2)构建滤波状态方程与量测方程；

3)确定融合后的视觉导航速度信息；

4)对单一特征点的信息进行咔方检测；

所述的步骤1)单特征点的线运动测量是指确定特征点A的平移向量T，具体如下：

其中，载体在t₁时刻观察到的A的坐标为(x₁ y₁ z₁)^T，t₂时刻观察到的坐标为(x₂ y₂ z₂)^T，γ、ψ和θ分别为惯导系统在相邻两帧时刻的滚动角、航向角和俯仰角的差值；

所述的步骤2)构建滤波状态方程与量测方程具体为：

2.1)选取12个系统状态δV_n,δV_u,δV_e,φ_n,φ_u,φ_e,ε_x,ε_y,ε_z；

δV_n,δV_u,δV_e分别表示惯导系统北向、天向、东向的速度误差；

φ_n,φ_u,φ_e分别表示惯导系统导航坐标系内北、天、东三个方向的失准角；

分别表示惯导系统载体坐标系内X、Y、Z三个方向的加速度计零偏；

ε_x,ε_y,ε_z分别表示惯导系统载体坐标系内X、Y、Z三个方向的陀螺漂移；

2.2)确定系统方程和量测方程分别为：

Q_k、V_k都是零均值的白噪声；V_k的方差阵R为分块对角阵；

其中，的阶数分别是m₁,m₂,…m_r；m₁+m₂+…+m_r＝dz，其中dz表示观测量的维数，r为不同特征点个数；

2.3)确定单一特征点的观测量

其中，V_{n_ins},V_{e_ins}分别为惯导北向速度和东向速度；分别为第i个特征点计算的北向速度和东向速度；

将观测量按每两维分成一块，最终分块数与参与组合的特征点数量相同；

所述的步骤3)确定融合后的视觉导航速度信息，具体如下：

3.1)在每一滤波周期内，首先进行时间更新

其中，为一步状态预测值，Φ_k,k-1为状态一步转移矩阵，为上一时刻状态估计值；为一步预测估计误差方差阵，P_k-1为上一时刻的估计误差方差阵，Q_k-1为上一时刻系统噪声阵；

3.2)进行序贯量测更新

根据卡尔曼滤波最优估计结果得到视觉位置匹配与惯导系统误差的最优估计值，利用估计出的误差进行补偿，最终输出组合导航结果。

2.如权利要求1所述的一种基于序贯检测的惯性/视觉里程计组合导航方法，其特征在于，所述的步骤4)单一特征点的信息进行咔方检测，具体如下：

4.1)确定当前特征点速度的信息及其方差

4.2)确定第i个特征点速度的故障检测函数

当小于门限时，则按步骤3.2)进行序贯量测更新，当大于门限时，则直接忽略本分块观测量，进行下一分块的量测更新。