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

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

An Inertial Vision Position Integrated Navigation Method Based on Sequential Detection

The invention relates to the technical field of inertial and visual integrated navigation, and specifically discloses an inertial visual position integrated navigation method based on sequential detection. The method includes step 1, visual position matching and positioning; step 1.1, acquiring visual image, dividing the image into blocks and taking the map with latitude and longitude information as the reference map; step 1.2, using normalized product correlation algorithm to match visual position; step 2, establishing the state model of inertia and visual position matching system; step 3, establishing measurement equation and measurement value; Fifth, the optimal estimation of visual position matching and inertial navigation system error is obtained, and the error compensation is carried out to obtain the output integrated navigation results. The inertial visual position integrated navigation method based on sequential detection can effectively utilize multiple position information, greatly improve the calculation speed of integrated navigation, and realize high precision inertial visual position integrated navigation.

【技术实现步骤摘要】
一种基于序贯检测的惯性视觉位置组合导航方法
本专利技术属于惯性及视觉组合导航
，具体涉及一种基于序贯检测的惯性视觉位置组合导航方法。
技术介绍
传统的惯性/视觉组合导航技术在进行卡尔曼滤波估计时，视觉导航输出的位置信息是将多个特征点的位置信息经过一系列的融合、剔除算法得到的单一位置信息，这将导致丢失单个特征点的位置信息，不利于提高组合导航精度。
技术实现思路
本专利技术的目的在于提供一种基于序贯检测的惯性视觉位置组合导航方法，其可以充分利用单个特征点的位置信息，进一步提高惯性、视觉组合导航精度及速度。本专利技术的技术方案如下：一种基于序贯检测的惯性视觉位置组合导航方法，该方法具体包括如下步骤：步骤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个状态，分别为δVn、δVu、δVe、δL、δh、δλ、φn、φu、φe、▽x、▽y、▽z、εx、εy、εz、δLdsmac以及δλdsmac，其中，δVn、δVu、δVe分别表示捷联惯导系统北向、天向、东向的速度误差，单位为米每秒；δL、δh、δλ分别表示捷联惯导系统的纬度误差、高度误差、经度误差，单位分别为弧度、米、弧度；φn、φu、φe分别表示捷联惯导系统导航坐标系内北、天、东三个方向的失准角，单位为弧度；▽x、▽y、▽z分别表示捷联惯导系统载体坐标系内X、Y、Z三个方向的加速度计零偏，单位为米每秒平方；εx、εy、εz分别表示捷联惯导系统载体坐标系内X、Y、Z三个方向的陀螺漂移，单位为弧度每秒；δLdsmac、δλdsmac分别表示景象匹配纬度、经度误差，单位为弧度；视觉位置匹配误差方程为：Q(t)为系统白噪声；系数矩阵F(t)根据惯性导航误差方程求取。所述的步骤3中建立量测方程及量测值的具体步骤为：建立基于序贯检测的卡尔曼滤波器量测方程如下：其中，式中，Limu、λimu表示惯导输出的纬度、经度，单位为弧度；表示第i个图像块获得的视觉位置匹配定位信息中的纬度、经度，单位为弧度。所述的步骤4中进行卡尔曼滤波的具体步骤为：根据惯性视觉位置组合导航系统误差方程，计算卡尔曼滤波周期到来时的状态一步转移矩阵Φk,k-1，其计算公式如下：Φk,k-1＝I+F(t)进行时间更新，即其中，为一步状态预测值，Φk,k-1为状态一步转移矩阵，为上一时刻状态估计值；为一步预测估计误差方差阵，Pk-1为上一时刻的估计误差方差阵，Qk-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个状态，分别为δVn、δVu、δVe、δL、δh、δλ、φn、φu、φe、▽x、▽y、▽z、εx、εy、εz、δLdsmac以及δλdsmac，其中，δVn、δVu、δVe分别表示捷联惯导系统北向、天向、东向的速度误差，单位为米每秒；δL、δh、δλ分别表示捷联惯导系统的纬度误差、高度误差、经度误差，单位分别为弧度、米、弧度；φn、φu、φe分别表示捷联惯导系统导航坐标系内北、天、东三个方向的失准角，单位为弧度；▽x、▽y、▽z分别表示捷联惯导系统载体坐标系内X、Y、Z三个方向的加速度计零偏，单位为米每秒平方；εx、εy、εz分别表示捷联惯导系统载体坐标系内X、Y、Z三个方向的陀螺漂移，单位为弧度每秒；δLdsmac、δλdsmac分别表示景象匹配纬度、经度误差，单位为弧度；视觉位置匹配误差方程为：Q(t)为系本文档来自技高网...

【技术保护点】
1.一种基于序贯检测的惯性视觉位置组合导航方法，其特征在于：该方法具体包括如下步骤：步骤1、进行视觉位置匹配定位；步骤1.1、获取视觉图像，将图像进行分块，并将具有经纬度信息的图作为基准图；步骤1.2、利用归一化积相关算法进行视觉位置匹配；步骤2、建立惯性及视觉位置匹配系统状态模型；步骤3、建立量测方程及量测值；步骤4、进行卡尔曼滤波；步骤5、获得视觉位置匹配与惯导系统误差最优估计值，并进行误差补偿，获得输出组合导航结果。

【技术特征摘要】
1.一种基于序贯检测的惯性视觉位置组合导航方法，其特征在于：该方法具体包括如下步骤：步骤1、进行视觉位置匹配定位；步骤1.1、获取视觉图像，将图像进行分块，并将具有经纬度信息的图作为基准图；步骤1.2、利用归一化积相关算法进行视觉位置匹配；步骤2、建立惯性及视觉位置匹配系统状态模型；步骤3、建立量测方程及量测值；步骤4、进行卡尔曼滤波；步骤5、获得视觉位置匹配与惯导系统误差最优估计值，并进行误差补偿，获得输出组合导航结果。2.根据权利要求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)处与模板图像相同区域的均值；由于每个图像块都能进行一次匹配来获得定位信息，因此，多个图像块可获得多个匹配定位结果。3.根据权利要求1所述的一种基于序贯检测的惯性视觉位置组合导航方法，其特征在于：所述的步骤2中建立惯性及视觉位置匹配系统状态模型的具体步骤为：建立惯性及视觉位置匹配系统状态方程为：其中，X(t)为上述系统状态方程的17个状态，分别为δVn、δVu、δVe、δL、δh、δλ、φn、φu、φe、▽x、▽y、▽z、εx、εy、εz、δLdsmac以及δλdsmac，其中，δVn、δVu、δVe分别表示捷联惯导系统北向、天向、东向的速度误差，单位为米每秒；δL、δh、δλ分别表示捷联惯导系统的纬度误差、高度误差、经度误差，单位分别为弧度、米、弧度；φn、φu、φe分别表示捷联惯导系统导航坐标系内北、天、东三个方向的失准角，单位为弧度；▽x、▽y、▽z分别表示捷联...

【专利技术属性】
技术研发人员：孙伟，郭元江，李海军，徐海刚，李群，蔡善军，刘冲，郑辛，裴玉峰，原润，钟润伍，
申请(专利权)人：北京自动化控制设备研究所，
类型：发明
国别省市：北京,11