CN109387198B - 一种基于序贯检测的惯性/视觉里程计组合导航方法 - Google Patents
一种基于序贯检测的惯性/视觉里程计组合导航方法 Download PDFInfo
- Publication number
- CN109387198B CN109387198B CN201710655918.3A CN201710655918A CN109387198B CN 109387198 B CN109387198 B CN 109387198B CN 201710655918 A CN201710655918 A CN 201710655918A CN 109387198 B CN109387198 B CN 109387198B
- Authority
- CN
- China
- Prior art keywords
- visual
- navigation
- speed
- information
- inertial navigation
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 25
- 238000001514 detection method Methods 0.000 title claims abstract description 19
- 230000000007 visual effect Effects 0.000 claims abstract description 41
- 238000001914 filtration Methods 0.000 claims abstract description 22
- 238000005259 measurement Methods 0.000 claims abstract description 21
- 239000011159 matrix material Substances 0.000 claims description 22
- 238000012883 sequential measurement Methods 0.000 claims description 8
- 238000013519 translation Methods 0.000 claims description 5
- 230000007704 transition Effects 0.000 claims description 4
- 230000004927 fusion Effects 0.000 abstract description 4
- 238000012544 monitoring process Methods 0.000 abstract description 2
- 238000004364 calculation method Methods 0.000 description 6
- 238000006073 displacement reaction Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000004904 shortening Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
- G01C11/36—Videogrammetry, i.e. electronic processing of video signals from a single source or from different sources to give parallax or range information
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Multimedia (AREA)
- Signal Processing (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
本发明属于视觉导航技术领域,具体公开了一种基于序贯检测的惯性/视觉里程计组合导航方法。首先引入外界参考信息,即惯导的姿态信息,减少视觉里程计计算量;之后结合序贯卡尔曼滤波方法,将多个特征点估计的视觉速度与惯性导航速度进行数据融合,并利用基于χ2检测方法来对视觉故障信息进行监测;最后实现对组合导航***测量精度的提高。一方面通过多特征点匹配,增加视觉导航信息量,降低视觉误匹配对导航精度的影响,另一方面结合序贯卡尔曼滤波方法将外界惯导导航信息与视觉导航信息融合,提高***运动测量的精度与鲁棒性,同时进一步降低运算量。
Description
技术领域
本发明属于视觉导航技术领域,具体涉及一种利用多目图像采集***与外界辅助姿态信息进行视觉导航测速方法。
背景技术
目前,车辆、自主机器人等智能化地面交通工具对导航需求越来越高,而在城市、山谷等区域,卫星定位精度较差,甚至定位失效,因此在卫星定位不可用的环境下,自主导航***精度是需要重点考虑的问题之一。视觉导航***作为新兴的自主导航***在近年来成为各国学者的研究热点。视觉里程计作为视觉导航中的重要组成部分,已应用于实际工程产品中。但是由于受图像匹配算法复杂度与特征点误匹配的存在,视觉里程计的精度仍然较低,难用于需要高精度长航时导航的智能化交通工具中。
为解决视觉里程计面临的两大难题:图像匹配算法计算复杂度与特征点误匹配。可考虑从以下两方面解决途径:第一,目前常用的图像匹配方法包括SIFT与SURF等复杂匹配算法,虽然算法的匹配度较高,但是由于为了保障匹配算法的强鲁棒性,因此牺牲了算法的实时性,于是此类算法复杂度较高,往往只能应用于对实时性要求不高的使用环境。因此为了降低图像匹配复杂度,可通过引入外界参考信息,缩小图像匹配的搜索空间,以实现缩短图像匹配算法计算时间的目的。而惯导可提供速度与航姿信息,可作为外界辅助导航信息源;第二,对于二维平移运动,利用单一特征点匹配实现对速度的测量往往会由于误匹配导致速度计算误差较大,因此,考虑引入不同景深的特征点分别进行图像匹配与运动估计。但是在引入多特征点信息后,传统滤波方法会随着特征点数量增大,因此计算机的运算量大幅增加。
发明内容
本发明的目的是提供一种基于序贯检测的惯性/视觉里程计组合导航方法,其能够提高测速精度,较大的降低计算机的计算量。
本发明的技术方案如下:
一种基于序贯检测的惯性/视觉里程计组合导航方法,包括如下步骤:
1)单特征点的线运动测量;
2)构建滤波状态方程与量测方程;
3)确定融合后的视觉导航速度信息;
4)对单一特征点的信息进行咔方检测。
所述的步骤1)单特征点的线运动测量是指确定特征点A的平移向量T,具体如下:
其中,载体在t1时刻观察到的A的坐标为(x1 y1 z1)T,t2时刻观察到的坐标为(x2 y2z2)T,γ、ψ和θ分别为惯导***在相邻两帧时刻的滚动角、航向角和俯仰角的差值。
所述的步骤2)构建滤波状态方程与量测方程具体为:
δVn,δVu,δVe分别表示惯导***北向、天向、东向的速度误差;
φn,φu,φe分别表示惯导***导航坐标系内北、天、东三个方向的失准角;
εx,εy,εz分别表示惯导***载体坐标系内X、Y、Z三个方向的陀螺漂移;
2.2)确定***方程和量测方程分别为:
Xk+1=Φk+1,kXk+Qk
Zk=HkXk+Vk
Qk、Vk都是零均值的白噪声;Vk的方差阵R为分块对角阵;
2.3)确定单一特征点的观测量
所述的步骤3)确定融合后的视觉导航速度信息,具体如下:
3.1)在每一滤波周期内,首先进行时间更新
3.2)进行序贯量测更新
根据卡尔曼滤波最优估计结果得到视觉位置匹配与惯导***误差的最优估计值,利用估计出的误差进行补偿,最终输出组合导航结果。
所述的步骤4)单一特征点的信息进行咔方检测,具体如下:
4.1)确定当前特征点速度的信息及其方差。
4.2)确定第i个特征点速度的故障检测函数
本发明的显著效果如下:
通过序贯卡尔曼滤波方法,来实现多特征点视觉导航与惯性导航的融合,在降低测速噪声,实现对视觉里程计测速精度提高的同时,并且能够较大的降低计算量。
本发明的基本原理是,首先引入外界参考信息,即惯导的姿态信息,减少视觉里程计计算量;之后结合序贯卡尔曼滤波方法,将多个特征点估计的视觉速度与惯性导航速度进行数据融合,并利用基于χ2检测方法来对视觉故障信息进行监测;最后实现对组合导航***测量精度的提高。一方面通过多特征点匹配,增加视觉导航信息量,降低视觉误匹配对导航精度的影响,另一方面结合序贯卡尔曼滤波方法将外界惯导导航信息与视觉导航信息融合,提高***运动测量的精度与鲁棒性,同时进一步降低运算量。
通过惯性导航***姿态信息,以辅助视觉里程计进行特征点匹配,提高图像处理的效率;单帧图像中存在大量特征点信息,利用不同景深的特征点分别在前后帧图像中进行匹配,以计算相邻时刻特征点的运动情况。利用序贯卡尔曼滤波方法来将惯性与视觉导航信息融合,相比传统方法,在提高视觉里程计的测速精度与可靠性的同时,可大幅降低计算量。
具体实施方式
下面通过具体实施方式对本发明作进一步说明。
步骤1)单特征点的线运动测量,即确定特征点A的位移(平移向量)T;
立体视觉里程计是通过对全局坐标系中特征点A进行跟踪。当载体在t1时刻观察到的A的坐标为(x1 y1 z1)T,t2时刻观察到的坐标为(x2 y2 z2)T。两时刻间,载体姿态发生的变化表示为旋转矩阵R,位移发生的变化表示为平移向量T。通过惯导***提供的姿态信息,求解旋转矩阵R,因此只需求解平移向量T中的三个未知量。
其中,γ、ψ和θ分别为惯导***在相邻两帧时刻的滚动角、航向角和俯仰角的差值。
通过式(2)可得位移发生变化,进而通过对采样周期进行微分,即可得当前时刻针对某个特征点的视觉里程计速度。
步骤2)构建滤波状态方程与量测方程
由于视觉误匹配将极大影响视觉导航精度,因此利用r个不同特征点计算出r个北向速度和东向速度后再进行组合滤波。但是由于随着特征点的增加,观测量的维数会随着成倍的增加,而如果观测量Z的维数很大,即量测噪声方差阵R的阶数很高,则求取最佳增益阵K时的矩阵求逆的阶数就很高。而求逆运算的计算量与矩阵阶数的三次方近似成正比,这将大幅增加卡尔曼滤波的计算量。因此考虑基于序贯检测方法,一方面独立检测不同特征点,降低误匹配对信息融合滤波器的影响,另一方面也可降低计算量。
在滤波***中,观测噪声阵为(分块)对角阵,误差主要包括2个方面:一是惯导的导航参数解算误差;二是惯导的惯性器件自身误差和其受环境影响而引起的误差。***状态方程就是各误差方程,共选取12个***状态:
其中:
δVn,δVu,δVe分别表示惯导***北向、天向、东向的速度误差;
φn,φu,φe分别表示惯导***导航坐标系内北、天、东三个方向的失准角;
εx,εy,εz分别表示惯导***载体坐标系内X、Y、Z三个方向的陀螺漂移。
***方程和量测方程分别为:
状态一步转移矩阵Φk+1,k根据惯导误差方程求取,惯性导航误差方程可参见文献《惯性导航》(第一版,秦永元等编著)中的形式;Qk、Vk都是零均值的白噪声;Vk的方差阵R为分块对角阵。
m1+m2+…+mr=dz,其中dz表示观测量的维数。
将观测方程改写成
在组合滤波过程中,我们一般是针对某单一特征点的观测量进行量测更新,单一特征点的观测量为该特征点计算的北速、东速与惯导的误差,即
因此将观测量按每两维分成一块,最终分块数与参与组合的特征点数量相同。
步骤3)利用序贯卡尔曼滤波,将惯导与多组特征点对应的视觉速度进行融合,确定融合后的视觉导航速度信息;
步骤3.1)在每一滤波周期内,首先进行时间更新
步骤3.2)根据时间更新结果,进行序贯量测更新。
具体算法为:
步骤4)在序贯滤波器中,针对单一特征点的信息进行咔方检测;
为了有效地利用每个特征点的观测信息,并及时隔离故障特征点对有效速度信息的污染,有必要对每个特征点速度信息进行故障检测及隔离。
本实施例中采用的是序贯滤波χ2检测方法。
步骤4.1)确定当前特征点速度的信息及其方差。
步骤4.2)确定第i个特征点速度的故障检测函数为:
Claims (2)
1.一种基于序贯检测的惯性/视觉里程计组合导航方法,其特征在于,其包括如下步骤:
1)单特征点的线运动测量;
2)构建滤波状态方程与量测方程;
3)确定融合后的视觉导航速度信息;
4)对单一特征点的信息进行咔方检测;
所述的步骤1)单特征点的线运动测量是指确定特征点A的平移向量T,具体如下:
其中,载体在t1时刻观察到的A的坐标为(x1 y1 z1)T,t2时刻观察到的坐标为(x2 y2 z2)T,γ、ψ和θ分别为惯导***在相邻两帧时刻的滚动角、航向角和俯仰角的差值;
所述的步骤2)构建滤波状态方程与量测方程具体为:
δVn,δVu,δVe分别表示惯导***北向、天向、东向的速度误差;
φn,φu,φe分别表示惯导***导航坐标系内北、天、东三个方向的失准角;
εx,εy,εz分别表示惯导***载体坐标系内X、Y、Z三个方向的陀螺漂移;
2.2)确定***方程和量测方程分别为:
Qk、Vk都是零均值的白噪声;Vk的方差阵R为分块对角阵;
2.3)确定单一特征点的观测量
将观测量按每两维分成一块,最终分块数与参与组合的特征点数量相同;
所述的步骤3)确定融合后的视觉导航速度信息,具体如下:
3.1)在每一滤波周期内,首先进行时间更新
3.2)进行序贯量测更新
根据卡尔曼滤波最优估计结果得到视觉位置匹配与惯导***误差的最优估计值,利用估计出的误差进行补偿,最终输出组合导航结果。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710655918.3A CN109387198B (zh) | 2017-08-03 | 2017-08-03 | 一种基于序贯检测的惯性/视觉里程计组合导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710655918.3A CN109387198B (zh) | 2017-08-03 | 2017-08-03 | 一种基于序贯检测的惯性/视觉里程计组合导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109387198A CN109387198A (zh) | 2019-02-26 |
CN109387198B true CN109387198B (zh) | 2022-07-15 |
Family
ID=65412250
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710655918.3A Active CN109387198B (zh) | 2017-08-03 | 2017-08-03 | 一种基于序贯检测的惯性/视觉里程计组合导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109387198B (zh) |
Families Citing this family (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110207714B (zh) * | 2019-06-28 | 2021-01-19 | 广州小鹏自动驾驶科技有限公司 | 一种确定车辆位姿的方法、车载***及车辆 |
CN112179347B (zh) * | 2020-09-18 | 2022-10-18 | 西北工业大学 | 一种基于光谱红移误差观测方程的组合导航方法 |
CN112539746B (zh) * | 2020-10-21 | 2023-05-26 | 济南大学 | 基于多频卡尔曼滤波的机器人视觉/ins组合定位方法及*** |
CN113608169B (zh) * | 2021-05-20 | 2023-08-25 | 济南大学 | 一种基于序贯融合算法的多auv协同定位方法 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103217157A (zh) * | 2012-01-18 | 2013-07-24 | 北京自动化控制设备研究所 | 一种惯导/里程计自主组合导航方法 |
CN105371840A (zh) * | 2015-10-30 | 2016-03-02 | 北京自动化控制设备研究所 | 一种惯性/视觉里程计/激光雷达的组合导航方法 |
CN105509738A (zh) * | 2015-12-07 | 2016-04-20 | 西北工业大学 | 基于惯导/多普勒雷达组合的车载定位定向方法 |
-
2017
- 2017-08-03 CN CN201710655918.3A patent/CN109387198B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103217157A (zh) * | 2012-01-18 | 2013-07-24 | 北京自动化控制设备研究所 | 一种惯导/里程计自主组合导航方法 |
CN105371840A (zh) * | 2015-10-30 | 2016-03-02 | 北京自动化控制设备研究所 | 一种惯性/视觉里程计/激光雷达的组合导航方法 |
CN105509738A (zh) * | 2015-12-07 | 2016-04-20 | 西北工业大学 | 基于惯导/多普勒雷达组合的车载定位定向方法 |
Non-Patent Citations (1)
Title |
---|
一种序贯Unscented卡尔曼...微小型无人直升机导航***中的应用;柏猛等;《机器人 ROBOT》;20080531;第37卷(第3期);205-209 * |
Also Published As
Publication number | Publication date |
---|---|
CN109387198A (zh) | 2019-02-26 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
WO2021232470A1 (zh) | 基于多传感器融合的slam制图方法、*** | |
CN109387198B (zh) | 一种基于序贯检测的惯性/视觉里程计组合导航方法 | |
US11940277B2 (en) | Vision-aided inertial navigation system for ground vehicle localization | |
CN112268559B (zh) | 复杂环境下融合slam技术的移动测量方法 | |
CN112649016B (zh) | 一种基于点线初始化的视觉惯性里程计方法 | |
Liu et al. | Stereo visual-inertial odometry with multiple Kalman filters ensemble | |
CN101949703B (zh) | 一种捷联惯性/卫星组合导航滤波方法 | |
CN110986988B (zh) | 融合多传感器数据的轨迹推算方法、介质、终端和装置 | |
CN107909614B (zh) | 一种gps失效环境下巡检机器人定位方法 | |
CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
CN112683267B (zh) | 一种附有gnss速度矢量辅助的车载姿态估计方法 | |
CN110186461B (zh) | 一种基于重力梯度信息测距的协同导航方法 | |
CN109959374B (zh) | 一种行人惯性导航全时全程逆向平滑滤波方法 | |
CN113376669B (zh) | 一种基于点线特征的单目vio-gnss融合定位算法 | |
CN108759822B (zh) | 一种移动机器人3d定位*** | |
Jin et al. | Fast and accurate initialization for monocular vision/INS/GNSS integrated system on land vehicle | |
Tang et al. | LE-VINS: A robust solid-state-LiDAR-enhanced visual-inertial navigation system for low-speed robots | |
CN112729283A (zh) | 一种基于深度相机/mems惯导/里程计组合的导航方法 | |
Xiao et al. | LIO-vehicle: A tightly-coupled vehicle dynamics extension of LiDAR inertial odometry | |
CN113503872B (zh) | 一种基于相机与消费级imu融合的低速无人车定位方法 | |
CN113008229B (zh) | 一种基于低成本车载传感器的分布式自主组合导航方法 | |
CN111145267A (zh) | 基于imu辅助的360度全景视图多相机标定方法 | |
CN114993338B (zh) | 基于多段独立地图序列的一致性高效视觉惯性里程计算法 | |
CN115540854A (zh) | 一种基于uwb辅助的主动定位方法、设备和介质 | |
CN115077517A (zh) | 一种基于立体相机与imu融合的低速无人车定位方法及*** |
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 |