CN109387198B - 一种基于序贯检测的惯性/视觉里程计组合导航方法 - Google Patents

Abstract

本发明属于视觉导航技术领域，具体公开了一种基于序贯检测的惯性/视觉里程计组合导航方法。首先引入外界参考信息，即惯导的姿态信息，减少视觉里程计计算量；之后结合序贯卡尔曼滤波方法，将多个特征点估计的视觉速度与惯性导航速度进行数据融合，并利用基于χ²检测方法来对视觉故障信息进行监测；最后实现对组合导航系统测量精度的提高。一方面通过多特征点匹配，增加视觉导航信息量，降低视觉误匹配对导航精度的影响，另一方面结合序贯卡尔曼滤波方法将外界惯导导航信息与视觉导航信息融合，提高系统运动测量的精度与鲁棒性，同时进一步降低运算量。

Description

一种基于序贯检测的惯性/视觉里程计组合导航方法

技术领域

本发明属于视觉导航技术领域，具体涉及一种利用多目图像采集系统与外界辅助姿态信息进行视觉导航测速方法。

背景技术

目前，车辆、自主机器人等智能化地面交通工具对导航需求越来越高，而在城市、山谷等区域，卫星定位精度较差，甚至定位失效，因此在卫星定位不可用的环境下，自主导航系统精度是需要重点考虑的问题之一。视觉导航系统作为新兴的自主导航系统在近年来成为各国学者的研究热点。视觉里程计作为视觉导航中的重要组成部分，已应用于实际工程产品中。但是由于受图像匹配算法复杂度与特征点误匹配的存在，视觉里程计的精度仍然较低，难用于需要高精度长航时导航的智能化交通工具中。

为解决视觉里程计面临的两大难题：图像匹配算法计算复杂度与特征点误匹配。可考虑从以下两方面解决途径：第一，目前常用的图像匹配方法包括SIFT与SURF等复杂匹配算法，虽然算法的匹配度较高，但是由于为了保障匹配算法的强鲁棒性，因此牺牲了算法的实时性，于是此类算法复杂度较高，往往只能应用于对实时性要求不高的使用环境。因此为了降低图像匹配复杂度，可通过引入外界参考信息，缩小图像匹配的搜索空间，以实现缩短图像匹配算法计算时间的目的。而惯导可提供速度与航姿信息，可作为外界辅助导航信息源；第二，对于二维平移运动，利用单一特征点匹配实现对速度的测量往往会由于误匹配导致速度计算误差较大，因此，考虑引入不同景深的特征点分别进行图像匹配与运动估计。但是在引入多特征点信息后，传统滤波方法会随着特征点数量增大，因此计算机的运算量大幅增加。

发明内容

本发明的目的是提供一种基于序贯检测的惯性/视觉里程计组合导航方法，其能够提高测速精度，较大的降低计算机的计算量。

本发明的技术方案如下：

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

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)确定系统方程和量测方程分别为：

X_k+1＝Φ_k+1,kX_k+Q_k

Z_k＝H_kX_k+V_k

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)进行序贯量测更新

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

所述的步骤4)单一特征点的信息进行咔方检测，具体如下：

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

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

当

小于门限时，则按式(11)～(13)进行序贯量测更新，当

大于门限时，则直接忽略本分块观测量，进行下一分块的量测更新。

本发明的显著效果如下：

通过序贯卡尔曼滤波方法，来实现多特征点视觉导航与惯性导航的融合，在降低测速噪声，实现对视觉里程计测速精度提高的同时，并且能够较大的降低计算量。

本发明的基本原理是，首先引入外界参考信息，即惯导的姿态信息，减少视觉里程计计算量；之后结合序贯卡尔曼滤波方法，将多个特征点估计的视觉速度与惯性导航速度进行数据融合，并利用基于χ²检测方法来对视觉故障信息进行监测；最后实现对组合导航系统测量精度的提高。一方面通过多特征点匹配，增加视觉导航信息量，降低视觉误匹配对导航精度的影响，另一方面结合序贯卡尔曼滤波方法将外界惯导导航信息与视觉导航信息融合，提高系统运动测量的精度与鲁棒性，同时进一步降低运算量。

通过惯性导航系统姿态信息，以辅助视觉里程计进行特征点匹配，提高图像处理的效率；单帧图像中存在大量特征点信息，利用不同景深的特征点分别在前后帧图像中进行匹配，以计算相邻时刻特征点的运动情况。利用序贯卡尔曼滤波方法来将惯性与视觉导航信息融合，相比传统方法，在提高视觉里程计的测速精度与可靠性的同时，可大幅降低计算量。

具体实施方式

下面通过具体实施方式对本发明作进一步说明。

步骤1)单特征点的线运动测量，即确定特征点A的位移(平移向量)T；

立体视觉里程计是通过对全局坐标系中特征点A进行跟踪。当载体在t₁时刻观察到的A的坐标为(x₁ y₁ z₁)^T，t₂时刻观察到的坐标为(x₂ y₂ z₂)^T。两时刻间，载体姿态发生的变化表示为旋转矩阵R，位移发生的变化表示为平移向量T。通过惯导系统提供的姿态信息，求解旋转矩阵R，因此只需求解平移向量T中的三个未知量。

其中，γ、ψ和θ分别为惯导系统在相邻两帧时刻的滚动角、航向角和俯仰角的差值。

通过式(2)可得位移发生变化，进而通过对采样周期进行微分，即可得当前时刻针对某个特征点的视觉里程计速度。

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

由于视觉误匹配将极大影响视觉导航精度，因此利用r个不同特征点计算出r个北向速度

和东向速度

后再进行组合滤波。但是由于随着特征点的增加，观测量的维数会随着成倍的增加，而如果观测量Z的维数很大，即量测噪声方差阵R的阶数很高，则求取最佳增益阵K时的矩阵求逆的阶数就很高。而求逆运算的计算量与矩阵阶数的三次方近似成正比，这将大幅增加卡尔曼滤波的计算量。因此考虑基于序贯检测方法，一方面独立检测不同特征点，降低误匹配对信息融合滤波器的影响，另一方面也可降低计算量。

在滤波系统中，观测噪声阵为(分块)对角阵，误差主要包括2个方面：一是惯导的导航参数解算误差；二是惯导的惯性器件自身误差和其受环境影响而引起的误差。系统状态方程就是各误差方程，共选取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三个方向的陀螺漂移。

系统方程和量测方程分别为：

状态一步转移矩阵Φ_k+1,k根据惯导误差方程求取,惯性导航误差方程可参见文献《惯性导航》(第一版，秦永元等编著)中的形式；Q_k、V_k都是零均值的白噪声；V_k的方差阵R为分块对角阵。

式中，

的阶数分别是m₁,m₂,…m_r；

m₁+m₂+…+m_r＝dz，其中dz表示观测量的维数。

将观测方程改写成

在组合滤波过程中，我们一般是针对某单一特征点的观测量进行量测更新，单一特征点的观测量为该特征点计算的北速、东速与惯导的误差，即

式中，V_{n_ins},V_{e_ins}分别为惯导北速和东速；

分别为第k个特征点计算的北速和东速。

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

步骤3)利用序贯卡尔曼滤波，将惯导与多组特征点对应的视觉速度进行融合，确定融合后的视觉导航速度信息；

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

其中，

为一步状态预测值，Φ_k,k-1为状态一步转移矩阵，

为上一时刻状态估计值；

为一步预测估计误差方差阵，P_k-1为上一时刻的估计误差方差阵，Q_k-1为上一时刻系统噪声阵。

步骤3.2)根据时间更新结果，进行序贯量测更新。

具体算法为：

然后根据卡尔曼滤波最优估计结果

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

步骤4)在序贯滤波器中，针对单一特征点的信息进行咔方检测；

为了有效地利用每个特征点的观测信息，并及时隔离故障特征点对有效速度信息的污染，有必要对每个特征点速度信息进行故障检测及隔离。

本实施例中采用的是序贯滤波χ²检测方法。

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

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

当

小于门限时，则按式(11)～(13)进行序贯量测更新，当

大于门限时，则直接忽略本分块观测量，进行下一分块的量测更新。

Claims (2)

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)进行序贯量测更新，当

大于门限时，则直接忽略本分块观测量，进行下一分块的量测更新。