CN109724595A - 一种基于序贯检测的惯性视觉位置组合导航方法 - Google Patents
一种基于序贯检测的惯性视觉位置组合导航方法 Download PDFInfo
- Publication number
- CN109724595A CN109724595A CN201711047547.7A CN201711047547A CN109724595A CN 109724595 A CN109724595 A CN 109724595A CN 201711047547 A CN201711047547 A CN 201711047547A CN 109724595 A CN109724595 A CN 109724595A
- Authority
- CN
- China
- Prior art keywords
- visual position
- inertia
- error
- visual
- follows
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
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个状态,分别为δ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)为系统白噪声;系数矩阵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、获得视觉位置匹配与惯导系统误差最优估计值,并进行误差补偿,获得输出组合导航结果;
根据卡尔曼滤波最优估计结果可得到视觉位置匹配与惯导系统误差的最优估计值,利用估计出的误差进行补偿,最终输出组合导航结果。
Claims (7)
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分别表示捷联惯导系统载体坐标系内X、Y、Z三个方向的加速度计零偏,单位为米每秒平方;εx、εy、εz分别表示捷联惯导系统载体坐标系内X、Y、Z三个方向的陀螺漂移,单位为弧度每秒;δLdsmac、δλdsmac分别表示景象匹配纬度、经度误差,单位为弧度;
视觉位置匹配误差方程为:
Q(t)为系统白噪声;系数矩阵F(t)根据惯性导航误差方程求取。
4.根据权利要求3所述的一种基于序贯检测的惯性视觉位置组合导航方法,其特征在于:所述的步骤3中建立量测方程及量测值的具体步骤为:
建立基于序贯检测的卡尔曼滤波器量测方程如下:
其中,
式中,Limu、λimu表示惯导输出的纬度、经度,单位为弧度;表示第i个图像块获得的视觉位置匹配定位信息中的纬度、经度,单位为弧度。
5.根据权利要求4所述的一种基于序贯检测的惯性视觉位置组合导航方法,其特征在于:所述的步骤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次量测更新的估计误差方差阵。
6.根据权利要求5所述的一种基于序贯检测的惯性视觉位置组合导航方法,其特征在于:所述的步骤5的具体步骤为:
根据卡尔曼滤波最优估计结果可得到视觉位置匹配与惯导系统误差的最优估计值,利用估计出的误差进行补偿,最终输出组合导航结果。
7.根据权利要求1所述的一种基于序贯检测的惯性视觉位置组合导航方法,其特征在于:所述的步骤1.1中获取视觉图像,将图像进行分块,并将具有经纬度信息的图作为基准图的具体步骤为:
利用相机获得飞行载体下方图像,将该图像进行分块并获得多个图像块后,将每个图像块作为一个单独实时图,并将事先置备的具有经纬度信息的图作为基准图。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711047547.7A CN109724595B (zh) | 2017-10-31 | 2017-10-31 | 一种基于序贯检测的惯性视觉位置组合导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711047547.7A CN109724595B (zh) | 2017-10-31 | 2017-10-31 | 一种基于序贯检测的惯性视觉位置组合导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109724595A true CN109724595A (zh) | 2019-05-07 |
CN109724595B CN109724595B (zh) | 2021-04-02 |
Family
ID=66293450
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201711047547.7A Active CN109724595B (zh) | 2017-10-31 | 2017-10-31 | 一种基于序贯检测的惯性视觉位置组合导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109724595B (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115127554A (zh) * | 2022-08-31 | 2022-09-30 | 中国人民解放军国防科技大学 | 一种基于多源视觉辅助的无人机自主导航方法与系统 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101046387A (zh) * | 2006-08-07 | 2007-10-03 | 南京航空航天大学 | 利用景象匹配提高导航系统精度的方法及组合导航仿真系统 |
CN101598556A (zh) * | 2009-07-15 | 2009-12-09 | 北京航空航天大学 | 一种未知环境下无人机视觉/惯性组合导航方法 |
CN102506867A (zh) * | 2011-11-21 | 2012-06-20 | 清华大学 | 基于Harris角点匹配的SINS/SMANS组合导航方法及系统 |
US9026263B2 (en) * | 2011-11-30 | 2015-05-05 | Alpine Electronics, Inc. | Automotive navigation system and method to utilize internal geometry of sensor position with respect to rear wheel axis |
EP2894602A1 (en) * | 2014-01-13 | 2015-07-15 | Honeywell International Inc. | A method of using image warping for geo-registration feature matching in vision-aided positioning |
CN107063246A (zh) * | 2017-04-24 | 2017-08-18 | 齐鲁工业大学 | 一种视觉导航/惯性导航的松散组合导航方法 |
-
2017
- 2017-10-31 CN CN201711047547.7A patent/CN109724595B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101046387A (zh) * | 2006-08-07 | 2007-10-03 | 南京航空航天大学 | 利用景象匹配提高导航系统精度的方法及组合导航仿真系统 |
CN101598556A (zh) * | 2009-07-15 | 2009-12-09 | 北京航空航天大学 | 一种未知环境下无人机视觉/惯性组合导航方法 |
CN102506867A (zh) * | 2011-11-21 | 2012-06-20 | 清华大学 | 基于Harris角点匹配的SINS/SMANS组合导航方法及系统 |
US9026263B2 (en) * | 2011-11-30 | 2015-05-05 | Alpine Electronics, Inc. | Automotive navigation system and method to utilize internal geometry of sensor position with respect to rear wheel axis |
EP2894602A1 (en) * | 2014-01-13 | 2015-07-15 | Honeywell International Inc. | A method of using image warping for geo-registration feature matching in vision-aided positioning |
CN107063246A (zh) * | 2017-04-24 | 2017-08-18 | 齐鲁工业大学 | 一种视觉导航/惯性导航的松散组合导航方法 |
Non-Patent Citations (3)
Title |
---|
刘畅等: "一种基于惯性/视觉信息融合的无人机自主着陆导航算法", 《导航定位与授时》 * |
朱遵尚: "基于三维地形重建与匹配的飞行器视觉导航方法研究", 《中国博士学位论文全文数据库》 * |
高伟等: "SINS/视觉组合导航系统融合算法", 《压电与声光》 * |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115127554A (zh) * | 2022-08-31 | 2022-09-30 | 中国人民解放军国防科技大学 | 一种基于多源视觉辅助的无人机自主导航方法与系统 |
Also Published As
Publication number | Publication date |
---|---|
CN109724595B (zh) | 2021-04-02 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN104729506B (zh) | 一种视觉信息辅助的无人机自主导航定位方法 | |
CN103697889B (zh) | 一种基于多模型分布式滤波的无人机自主导航与定位方法 | |
Zhuang et al. | Tightly-coupled integration of WiFi and MEMS sensors on handheld devices for indoor pedestrian navigation | |
CN106482736B (zh) | 一种基于平方根容积卡尔曼滤波的多机器人协同定位算法 | |
US9420275B2 (en) | Visual positioning system that utilizes images of a working environment to determine position | |
JP5639874B2 (ja) | 運転支援装置 | |
CN114216454B (zh) | 一种gps拒止环境下基于异源图像匹配的无人机自主导航定位方法 | |
US20100176987A1 (en) | Method and apparatus to estimate vehicle position and recognized landmark positions using GPS and camera | |
US20070282565A1 (en) | Object locating in restricted environments using personal navigation | |
CN107796391A (zh) | 一种捷联惯性导航系统/视觉里程计组合导航方法 | |
CN109059907B (zh) | 轨迹数据处理方法、装置、计算机设备和存储介质 | |
CN106708066A (zh) | 基于视觉/惯导的无人机自主着陆方法 | |
CN110274588A (zh) | 基于无人机集群信息的双层嵌套因子图多源融合导航方法 | |
CN108871336A (zh) | 一种车辆位置估算系统及方法 | |
CN108592914A (zh) | 无gps情况下的复杂区域巡视机器人定位、导航及授时方法 | |
EP2175237B1 (en) | System and methods for image-based navigation using line features matching | |
US9977113B2 (en) | Method of determining position and cooperative positioning system using same | |
CN111649737B (zh) | 一种面向飞机精密进近着陆的视觉-惯性组合导航方法 | |
CN106767791A (zh) | 一种采用基于粒子群优化的ckf的惯性/视觉组合导航方法 | |
CN109579825A (zh) | 基于双目视觉和卷积神经网络的机器人定位系统及方法 | |
CN111025366B (zh) | 基于ins及gnss的网格slam的导航系统及方法 | |
CN110849360B (zh) | 面向多机协同编队飞行的分布式相对导航方法 | |
CN113447949B (zh) | 一种基于激光雷达和先验地图的实时定位系统及方法 | |
CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
CN111238469A (zh) | 一种基于惯性/数据链的无人机编队相对导航方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |