CN109724595B - 一种基于序贯检测的惯性视觉位置组合导航方法 - Google Patents

Abstract

本发明涉及惯性及视觉组合导航技术领域，具体公开了一种基于序贯检测的惯性视觉位置组合导航方法。该方法包括步骤1、进行视觉位置匹配定位；步骤1.1、获取视觉图像，将图像进行分块，并将具有经纬度信息的图作为基准图；步骤1.2、利用归一化积相关算法进行视觉位置匹配；步骤2、建立惯性及视觉位置匹配***状态模型；步骤3、建立量测方程及量测值；步骤4、进行卡尔曼滤波；步骤5、获得视觉位置匹配与惯导***误差最优估计值，并进行误差补偿，获得输出组合导航结果。本发明所述的一种基于序贯检测的惯性视觉位置组合导航方法，可有效利用多个位置信息，大幅度提升组合导航的计算速度，实现高精度的惯性视觉位置组合导航。

Description

一种基于序贯检测的惯性视觉位置组合导航方法

技术领域

本发明属于惯性及视觉组合导航技术领域，具体涉及一种基于序贯检测的惯性视觉位置组合导航方法。

背景技术

传统的惯性/视觉组合导航技术在进行卡尔曼滤波估计时，视觉导航输出的位置信息是将多个特征点的位置信息经过一系列的融合、剔除算法得到的单一位置信息，这将导致丢失单个特征点的位置信息，不利于提高组合导航精度。

发明内容

本发明的目的在于提供一种基于序贯检测的惯性视觉位置组合导航方法，其可以充分利用单个特征点的位置信息，进一步提高惯性、视觉组合导航精度及速度。

本发明的技术方案如下：一种基于序贯检测的惯性视觉位置组合导航方法，该方法具体包括如下步骤：

步骤1、进行视觉位置匹配定位；

步骤1.1、获取视觉图像，将图像进行分块，并将具有经纬度信息的图作为基准图；

步骤1.2、利用归一化积相关算法进行视觉位置匹配；

步骤2、建立惯性及视觉位置匹配***状态模型；

步骤3、建立量测方程及量测值；

步骤4、进行卡尔曼滤波；

步骤5、获得视觉位置匹配与惯导***误差最优估计值，并进行误差补偿，获得输出组合导航结果。

所述的步骤1.2中利用归一化积相关算法进行视觉位置匹配的具体步骤为：

实时图像在基准图像中从起始位置逐行扫过整个基准图；在每个位置将模板图像中每个像素点与它所对应基准图区域像素点进行相关性运算，将结果记录在相应的位置处；选取相关性较高的若干个匹配峰值点，作为候选匹配像素点，从而根据匹配结果即可换算出对应的位置信息；

利用下式进行相关性计算为：

其中，(x,y)为模板图像中的坐标点；(u,v)为基准图像中的坐标点；

T(x,y)为模板图像在点(x,y)处的灰度值，范围为0到255；

为模板图像在基准图像(u,v)处的均值；

I(x+u,y+v)为基准图像点(u,v)处，与模板图像相对应位置处的灰度值；

为基准图像(u,v)处与模板图像相同区域的均值；

由于每个图像块都能进行一次匹配来获得定位信息，因此，多个图像块可获得多个匹配定位结果。

所述的步骤2中建立惯性及视觉位置匹配***状态模型的具体步骤为：

建立惯性及视觉位置匹配***状态方程为：

其中，X(t)为上述***状态方程的17个状态，分别为δV_n、δV_u、δV_e、δL、δh、δλ、φ_n、φ_u、φ_e、▽_x、▽_y、▽_z、ε_x、ε_y、ε_z、δL_dsmac以及δλ_dsmac，其中，δV_n、δV_u、δV_e分别表示捷联惯导***北向、天向、东向的速度误差，单位为米每秒；δL、δh、δλ分别表示捷联惯导***的纬度误差、高度误差、经度误差，单位分别为弧度、米、弧度；φ_n、φ_u、φ_e分别表示捷联惯导***导航坐标系内北、天、东三个方向的失准角，单位为弧度；▽_x、▽_y、▽_z分别表示捷联惯导***载体坐标系内X、Y、Z三个方向的加速度计零偏，单位为米每秒平方；ε_x、ε_y、ε_z分别表示捷联惯导***载体坐标系内X、Y、Z三个方向的陀螺漂移，单位为弧度每秒；δL_dsmac、δλ_dsmac分别表示景象匹配纬度、经度误差，单位为弧度；

视觉位置匹配误差方程为：

Q(t)为***白噪声；系数矩阵F(t)根据惯性导航误差方程求取。

所述的步骤3中建立量测方程及量测值的具体步骤为：

建立基于序贯检测的卡尔曼滤波器量测方程如下：

其中，

式中，L_imu、λ_imu表示惯导输出的纬度、经度，单位为弧度；

表示第i个图像块获得的视觉位置匹配定位信息中的纬度、经度，单位为弧度。

所述的步骤4中进行卡尔曼滤波的具体步骤为：

根据惯性视觉位置组合导航***误差方程，计算卡尔曼滤波周期到来时的状态一步转移矩阵Φ_k,k-1，其计算公式如下：

Φ_k,k-1＝I+F(t)

进行时间更新，即

其中，

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

为上一时刻状态估计值；

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

依次利用单次匹配中第i＝1,…,N个定位结果，进行序贯量测更新，即

其中，

为单次匹配中第i次量测更新的量测矩阵，

为单次匹配中第i次量测更新的量测量，

为单次匹配中第i次量测更新的滤波增益矩阵，

为单次匹配中第i次量测更新的观测噪声阵，

为单次匹配中第i次量测更新的估计误差方差阵。

所述的步骤5的具体步骤为：

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

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

所述的步骤1.1中获取视觉图像，将图像进行分块，并将具有经纬度信息的图作为基准图的具体步骤为：

利用相机获得飞行载体下方图像，将该图像进行分块并获得多个图像块后，将每个图像块作为一个单独实时图，并将事先置备的具有经纬度信息的图作为基准图。

本发明的显著效果在于：本发明所述的一种基于序贯检测的惯性视觉位置组合导航方法，可有效利用多个位置信息，大幅度提升组合导航的计算速度，实现高精度的惯性视觉位置组合导航。

附图说明

图1为本发明所述的一种基于序贯检测的惯性视觉位置组合导航方法流程图。

具体实施方式

下面结合附图及具体实施例对本发明作进一步详细说明。

如图1所示，一种基于序贯检测的惯性视觉位置组合导航方法，该方法具体包括如下步骤：

步骤1、进行视觉位置匹配定位；

步骤1.1、获取视觉图像，将图像进行分块，并将具有经纬度信息的图作为基准图；

利用相机获得飞行载体下方图像，将该图像进行分块并获得多个图像块后，将每个图像块作为一个单独实时图，并将事先置备的具有经纬度信息的图作为基准图；

步骤1.2、利用归一化积相关算法进行视觉位置匹配；

实时图像在基准图像中从起始位置逐行扫过整个基准图；在每个位置将模板图像中每个像素点与它所对应基准图区域像素点进行相关性运算，将结果记录在相应的位置处；选取相关性较高的若干个匹配峰值点，作为候选匹配像素点，从而根据匹配结果即可换算出对应的位置信息；

利用下式进行相关性计算为：

其中，(x,y)为模板图像中的坐标点；(u,v)为基准图像中的坐标点；

T(x,y)为模板图像在点(x,y)处的灰度值，范围为0到255；

为模板图像在基准图像(u,v)处的均值；

I(x+u,y+v)为基准图像点(u,v)处，与模板图像相对应位置处的灰度值；

为基准图像(u,v)处与模板图像相同区域的均值；

由于每个图像块都能进行一次匹配来获得定位信息，因此，多个图像块可获得多个匹配定位结果；

步骤2、建立惯性及视觉位置匹配***状态模型；

建立惯性及视觉位置匹配***状态方程为：

其中，X(t)为上述***状态方程的17个状态，分别为δV_n、δV_u、δV_e、δL、δh、δλ、φ_n、φ_u、φ_e、▽_x、▽_y、▽_z、ε_x、ε_y、ε_z、δL_dsmac以及δλ_dsmac，其中，δV_n、δV_u、δV_e分别表示捷联惯导***北向、天向、东向的速度误差，单位为米每秒；δL、δh、δλ分别表示捷联惯导***的纬度误差、高度误差、经度误差，单位分别为弧度、米、弧度；φ_n、φ_u、φ_e分别表示捷联惯导***导航坐标系内北、天、东三个方向的失准角，单位为弧度；▽_x、▽_y、▽_z分别表示捷联惯导***载体坐标系内X、Y、Z三个方向的加速度计零偏，单位为米每秒平方；ε_x、ε_y、ε_z分别表示捷联惯导***载体坐标系内X、Y、Z三个方向的陀螺漂移，单位为弧度每秒；δL_dsmac、δλ_dsmac分别表示景象匹配纬度、经度误差，单位为弧度；

视觉位置匹配误差方程为：

Q(t)为***白噪声；系数矩阵F(t)根据惯性导航误差方程求取；

步骤3、建立量测方程及量测值；

建立基于序贯检测的卡尔曼滤波器量测方程如下：

其中，

式中，L_imu、λ_imu表示惯导输出的纬度、经度，单位为弧度；

表示第i个图像块获得的视觉位置匹配定位信息中的纬度、经度，单位为弧度；

步骤4、进行卡尔曼滤波

根据惯性视觉位置组合导航***误差方程，计算卡尔曼滤波周期到来时的状态一步转移矩阵Φ_k,k-1，其计算公式如下：

Φ_k,k-1＝I+F(t)

进行时间更新，即

其中，

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

为上一时刻状态估计值；

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

依次利用单次匹配中第i＝1,…,N个定位结果，进行序贯量测更新，即

其中，

为单次匹配中第i次量测更新的量测矩阵，

为单次匹配中第i次量测更新的量测量，

为单次匹配中第i次量测更新的滤波增益矩阵，

为单次匹配中第i次量测更新的观测噪声阵，

为单次匹配中第i次量测更新的估计误差方差阵；

步骤5、获得视觉位置匹配与惯导***误差最优估计值，并进行误差补偿，获得输出组合导航结果；

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

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

Claims (4)

1.一种基于序贯检测的惯性视觉位置组合导航方法，其特征在于：该方法具体包括如下步骤：

步骤1、进行视觉位置匹配定位；

步骤1.1、获取视觉图像，将图像进行分块，并将具有经纬度信息的图作为基准图；

步骤1.2、利用归一化积相关算法进行视觉位置匹配；

实时图像在基准图像中从起始位置逐行扫过整个基准图；在每个位置将模板图像中每个像素点与它所对应基准图区域像素点进行相关性运算，将结果记录在相应的位置处；选取相关性高的若干个匹配峰值点，作为候选匹配像素点，从而根据匹配结果即可换算出对应的位置信息；

利用下式进行相关性计算为：

其中，(x,y)为模板图像中的坐标点；(u,v)为基准图像中的坐标点；

T(x,y)为模板图像在点(x,y)处的灰度值，范围为0到255；

为模板图像在基准图像(u,v)处的均值；

I(x+u,y+v)为基准图像点(u,v)处，与模板图像相对应位置处的灰度值；

为基准图像(u,v)处与模板图像相同区域的均值；

由于每个图像块都能进行一次匹配来获得定位信息，因此，多个图像块可获得多个匹配定位结果；

步骤2、建立惯性及视觉位置匹配***状态模型；

步骤3、建立量测方程及量测值；

所述的步骤3中建立量测方程及量测值的具体步骤为：

建立基于序贯检测的卡尔曼滤波器量测方程如下：

其中，

式中，L_imu、λ_imu表示惯导输出的纬度、经度，单位为弧度；

表示第i个图像块获得的视觉位置匹配定位信息中的纬度、经度，单位为弧度；

步骤4、进行卡尔曼滤波；

根据惯性视觉位置组合导航***误差方程，计算卡尔曼滤波周期到来时的状态一步转移矩阵Φ_k,k-1，其计算公式如下：

Φ_k,k-1＝I+F(t)

进行时间更新，即

其中，

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

为上一时刻状态估计值；

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

依次利用单次匹配中第i＝1,…,N个定位结果，进行序贯量测更新，即

其中，

为单次匹配中第i次量测更新的量测矩阵，

为单次匹配中第i次量测更新的量测量，

为单次匹配中第i次量测更新的滤波增益矩阵，

为单次匹配中第i次量测更新的观测噪声阵，

为单次匹配中第i次量测更新的估计误差方差阵；

步骤5、获得视觉位置匹配与惯导***误差最优估计值，并进行误差补偿，获得输出组合导航结果。

2.根据权利要求1所述的一种基于序贯检测的惯性视觉位置组合导航方法，其特征在于：所述的步骤2中建立惯性及视觉位置匹配***状态模型的具体步骤为：

建立惯性及视觉位置匹配***状态方程为：

其中，X(t)为上述***状态方程的17个状态，分别为δV_n、δV_u、δV_e、δL、δh、δλ、φ_n、φ_u、φ_e、

ε_x、ε_y、ε_z、δL_dsmac以及δλ_dsmac，其中，δV_n、δV_u、δV_e分别表示捷联惯导***北向、天向、东向的速度误差，单位为米每秒；δL、δh、δλ分别表示捷联惯导***的纬度误差、高度误差、经度误差，单位分别为弧度、米、弧度；φ_n、φ_u、φ_e分别表示捷联惯导***导航坐标系内北、天、东三个方向的失准角，单位为弧度；

分别表示捷联惯导***载体坐标系内X、Y、Z三个方向的加速度计零偏，单位为米每秒平方；ε_x、ε_y、ε_z分别表示捷联惯导***载体坐标系内X、Y、Z三个方向的陀螺漂移，单位为弧度每秒；δL_dsmac、δλ_dsmac分别表示景象匹配纬度、经度误差，单位为弧度；

视觉位置匹配误差方程为：

Q(t)为***白噪声；系数矩阵F(t)根据惯性导航误差方程求取。

3.根据权利要求1所述的一种基于序贯检测的惯性视觉位置组合导航方法，其特征在于：所述的步骤5的具体步骤为：

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

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

4.根据权利要求1所述的一种基于序贯检测的惯性视觉位置组合导航方法，其特征在于：所述的步骤1.1中获取视觉图像，将图像进行分块，并将具有经纬度信息的图作为基准图的具体步骤为：

利用相机获得飞行载体下方图像，将该图像进行分块并获得多个图像块后，将每个图像块作为一个单独实时图，并将事先置备的具有经纬度信息的图作为基准图。

Images