CN108801248B - 基于ukf的平面视觉惯性导航方法 - Google Patents
基于ukf的平面视觉惯性导航方法 Download PDFInfo
- Publication number
- CN108801248B CN108801248B CN201810666461.0A CN201810666461A CN108801248B CN 108801248 B CN108801248 B CN 108801248B CN 201810666461 A CN201810666461 A CN 201810666461A CN 108801248 B CN108801248 B CN 108801248B
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- camera
- inertial navigation
- model
- error
- 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.)
- Expired - Fee Related
Links
Images
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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明公开了基于UKF的平面视觉惯性导航方法,包括如下步骤:计算惯性导航***的非线性惯导误差传播模型;根据状态变量以及所需导航平面上的三维位置计算出摄像机的投影模型;根据所述投影模型计算得到任意一个测量特征点相对于两个不同位置的摄像机的位置模型;根据所述投影模型和位置模型计算得到同一平面上的多个测量特征点匹配构成的非线性测量方程;根据所述非线性测量方法获取UKF线性误差,根据该UKF线性误差对非线性惯导误差传播模型进行修正。本发明通过对状态空间进行推导,结合UKF框架进行误差估计和修正,降低视觉惯性导航的误差,适用于任意平面模型。
Description
技术领域
本发明涉及导航技术,尤其涉及基于UKF的平面视觉惯性导航方法。
背景技术
将航行载体从起始点引导到目的地的过程称为导航。导航有多种技术途径,如无线电导航、天文导航、卫星导航、红外导航、惯性导航、视觉导航等。
其中,惯性导航是利用加速度计与陀螺仪计算航程,推知当前位置和下一步目的,自主性强,不易受干扰,是目前的主要导航方法、但惯性导航***由于其固有的导航误差积累,导航精度随时间增长而降低,并且设备成本高,不能完全满足实际应用的需求。
而视觉导航采用成像设备拍摄图像,运用机器视觉等相关技术识别路径,实现自动导航。视觉导航因其应用范围广,在理论上具有最佳指导柔性,近年来发展十分迅速。但是视觉导航主要存在图像匹配的精度差、目标图像中的像点定位有误差、测量***的标点误差以及成像***的空间分辨率有限的缺点,导致其应用也受到限制。
发明内容
为了克服现有技术的不足,本发明的目的在于提供基于UKF的平面视觉惯性导航方法,其能解决现有技术存在的精度低、存在误差等的问题。
本发明的目的采用以下技术方案实现:
基于UKF的平面视觉惯性导航方法,包括如下步骤:
S1:计算惯性导航***的非线性惯导误差传播模型;
S2:根据状态变量以及所需导航平面上的三维位置计算出摄像机的投影模型;
S3:根据所述投影模型计算得到任意一个测量特征点相对于两个不同位置的摄像机的位置模型;
S4:根据所述投影模型和位置模型计算得到同一平面上的多个测量特征点匹配构成的非线性测量方程;
S5:根据所述非线性测量方法获取UKF线性误差,根据该UKF线性误差对非线性惯导误差传播模型进行修正。
优选的,步骤S1具体包括如下步骤:
S10:定义惯性导航***相对于参考坐标系的姿态由全局坐标系{G}表示,以向量形式定义惯性导航***的状态变量为其中,CθG∈R3×1表示的是全局坐标系{G}在摄像机参考坐标系{C}中的方向,GvC为摄像机在全局坐标系{G}中的速度,GpC为摄像机在全局坐标系{G}中的位置,ba为加速度计的偏差矢量,bg为陀螺仪的偏差矢量;
S11:通过公式 获取惯性导航***的状态向量的时间更新;其中,ω(t)=[ω1 ω2 ω3]T为惯性测量单元的角速率,a(t)=[a1 a2 a3]T为惯性测量单元的线性加速度,Gg=[0 0 g]T为惯性测量单元的重力加速度,C(q)为惯性测量单元的旋转矩阵对应的四元数,q∈R4×1,nδa为加速度计误差造成的高斯白噪声,nδg为陀螺仪误差造成的高斯白噪声;
S12:建立陀螺仪的输出测量信号模型为:ωm(t)=ω(t)+bg(t)+ng(t),以及建立加速度计的输出测量信号模型为:am(t)=C(CqG(t))(Ga(t)-Gg)+ba(t)+na(t);
优选的,步骤S2包括如下子步骤:
S21:定义一个虚拟坐标系,使虚拟坐标系{V}的中心与摄像机坐标系的中心重合,将摄像机坐标系与虚拟坐标系通过公式进行转换,使得特征点f、pf分别在两个坐标系中的位置通过公式Cpf=CCV Vpf得到关联;
优选的,步骤S3具体包括如下子步骤:
优选的,步骤S4具体包括如下子步骤:
相比现有技术,本发明的有益效果在于:
本发明通过对状态空间进行推导,结合UKF框架进行误差估计和修正,降低视觉惯性导航的误差,适用于任意平面模型。
附图说明
图1为本发明的基于UKF的平面视觉惯性导航方法的流程图;
图2为本发明的摄像机坐标系与虚拟坐标系的结构示意图。
具体实施方式
下面,结合附图以及具体实施方式,对本发明做进一步描述:
如图1所示,本发明提供基于UKF的平面视觉惯性导航方法,本发明的目标是估计移动视觉惯性导航***相对于参考坐标系的姿态,由{G}表示,硬件结构由安装在惯性测量单元的摄像头组成,嘉定惯性测量单元和摄像机的参考系一直,则{Ipc=03×1,C(Iqc)=I3},包括如下步骤:
S1:计算惯性导航***的的非线性惯导传播模型;
S2:根据状态变量以及所需导航平面上的三维位置计算出摄像机的投影模型;
S3:根据所述投影模型计算得到任意一个测量特征点相对于两个不同位置的摄像机的位置模型;
S4:根据所述投影模型和位置模型计算得到同一平面上的多个测量特征点匹配构成的非线性测量方程;
S5:根据所述非线性测量方法获取UKF线性误差,根据该UKF线性误差对非线性惯导传播模型进行修正。
S1具体包括如下步骤:
S10:定义惯性导航***相对于参考坐标系的姿态由全局坐标系{G}表示,以向量形式定义惯性导航***的状态变量为其中,CθG∈R3×1表示的是全局坐标系{G}在摄像机参考坐标系{C}中的方向,GvC为摄像机在全局坐标系{G}中的速度,GpC为摄像机在全局坐标系{G}中的位置,ba为加速度计的偏差矢量,bg为陀螺仪的偏差矢量;
S11:通过公式 获取惯性导航***的状态向量的时间更新,加点表示微分;其中,ω(t)=[ω1 ω2 ω3]T为惯性测量单元的角速率,a(t)=[a1 a2 a3]T为惯性测量单元的线性加速度,Gg=[0 0 g]T为惯性测量单元的重力加速度,C(q)为惯性测量单元的旋转矩阵对应的四元数,q∈R4×1,nδa为加速度计误差造成的高斯白噪声,nδa为陀螺仪误差造成的高斯白噪声;
S12:建立陀螺仪的输出测量信号模型为:ωm(t)=ω(t)+bg(t)+ng(t),以及建立加速度计的输出测量信号模型为:am(t)=C(CqG(t))(Ga(t)-Gg)+ba(t)+na(t);
S13:定义惯性导航***的误差状态向量为:从而建立非线性惯导误差传播模型为其中,和是分别是离散时间状态和***噪声的传播矩阵, 是过程噪声(假设为零均值广义平稳)与对应的对角协方差矩阵Q∈R15×15,是估计的旋转矩阵,并且
步骤S2包括如下子步骤:
S20:设定摄像机观察的任意一个特征点f,设定相对于摄像机中心{C}的位置为Cpf,该特征点在摄像机的图像平面上的均匀和归一化像素坐标分别为和Cz, Cpf=C(CqG)(Gpf-GpC);I2表示二阶单位阵;
S21:定义一个虚拟坐标系,使虚拟坐标系{V}的中心与摄像机坐标系的中心重合,将摄像机坐标系与虚拟坐标系通过公式进行转换,使得特征点f、pf分别在两个坐标系中的位置通过公式Cpf=CCV Vpf得到关联;cCv代表摄像机中心和虚拟坐标系之间的转换矩阵。
具体结合图2,摄像机和虚拟相机的坐标系分别由{C}和{V}表示,虚拟摄像机的光轴被选择为始终与所需平面正交。全局坐标系{G}位于水平面上,局部坐标系{L}位于全局坐标系与所需导航坐标系的交点处,使得其中z轴与所需平面正交。
S25:设定一局部坐标系{L},假设该局部坐标系{L}在所需导航平面和水平面的交点上,其{z}轴是平面的法线,参考图2,可得到虚拟摄像机坐标系相对于局部坐标系的方向为C(VqL)=diag(1,-1,-1),从图2可以看出LpC为LpC=C(LqG)GpC+GpL,上式两边同时乘以则可以得到 将代入则可计算得到为摄像机的投影模型。为了测量这个值,可以使用用于测量斜率相对于重力角度的测斜仪。
步骤S3具体包括如下子步骤:
将两个摄像机视图中的特征f的观察结果与其相应的平移关联起来,由旋转矩阵和平移矢量进行表示,因此,Cpf的投影与***运动(状态变量)相关,可以对累积的惯性导航***误差施加约束。惯性测量单元的摄像机及在当前{C}时间下的坐标系并且以滞后于当前的不同时刻之间的相对旋转和平移由和表示。假定全局参考系G位于水平面上,其中z轴垂直于水平面。位于所需平面上的样本特征点f被认为是在相机的视野中;
步骤S4具体包括如下子步骤:
在本发明中,对于线性过程模型,协方差矩阵传播方程为其中,并且当记录新图像时,误差状态和更新为δxcam,误差协方差矩阵更新为其中,当定义的平面上的特征被检测和匹配时,UKF被设置为执行测量更新。基于的状态空间模型,UKF线性估计误差则为其中是测量预测,Kk是卡尔曼增益矩阵。δx和的联合统计量作为2N+1个sigma点传播,其中N是状态维数,N=15+6+2M。设联合状态向量表示为 是误差协方差矩阵的预测,其中表示矩阵的直接和。然后使用联合误差协方差矩阵的矩阵平方根生成sigma点通过公式中传播sigma点,可以近似估计误差状态δx和观测值Cy之间的相关性,这相关性构成了卡尔曼增益的基础。
基于上述的N=15+6+2M时,基于UKF的运动估计算法流程如下:
对本领域的技术人员来说,可根据以上描述的技术方案以及构思,做出其它各种相应的改变以及形变,而所有的这些改变以及形变都应该属于本发明权利要求的保护范围之内。
Claims (1)
1.基于UKF的平面视觉惯性导航方法,其特征在于,包括如下步骤:
S1:计算惯性导航***的非线性惯导误差传播模型;
S2:根据状态变量以及所需导航平面上的三维位置计算出摄像机的投影模型;
S3:根据所述投影模型计算得到任意一个测量特征点相对于两个不同位置的摄像机的位置模型;
S4:根据所述投影模型和位置模型计算得到同一平面上的多个测量特征点匹配构成的非线性测量方程;
S5:根据所述非线性测量方法获取UKF线性误差,根据该UKF线性误差对非线性惯导误差传播模型进行修正;
其中,步骤S1具体包括如下步骤:
S10:定义惯性导航***相对于参考坐标系的姿态由全局坐标系{G}表示,以向量形式定义惯性导航***的状态变量为其中,CθG∈R3×1表示的是全局坐标系{G}在摄像机参考坐标系{C}中的方向,GvC为摄像机在全局坐标系{G}中的速度,GpC为摄像机在全局坐标系{G}中的位置,ba为加速度计的偏差矢量,bg为陀螺仪的偏差矢量;
S11:通过公式 获取惯性导航***的状态向量的时间更新;其中,ω(t)=[ω1 ω2 ω3]T为惯性测量单元的角速率,a(t)=[a1 a2 a3]T为惯性测量单元的线性加速度,Gg=[0 0 g]T为惯性测量单元的重力加速度,C(q)为惯性测量单元的旋转矩阵对应的四元数,q∈R4×1,nδa为加速度计误差造成的高斯白噪声,nδg为陀螺仪误差造成的高斯白噪声;
S12:建立陀螺仪的输出测量信号模型为:ωm(t)=ω(t)+bg(t)+ng(t),以及建立加速度计的输出测量信号模型为:am(t)=C(CqG(t))(Ga(t)-Gg)+ba(t)+na(t);
步骤S2包括如下子步骤:
S21:定义一个虚拟坐标系,使虚拟坐标系{V}的中心与摄像机坐标系的中心重合,将摄像机坐标系与虚拟坐标系通过公式进行转换,使得特征点f、pf分别在两个坐标系中的位置通过公式Cpf=CCV Vpf得到关联;
步骤S3具体包括如下子步骤:
步骤S4具体包括如下子步骤:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810666461.0A CN108801248B (zh) | 2018-06-22 | 2018-06-22 | 基于ukf的平面视觉惯性导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810666461.0A CN108801248B (zh) | 2018-06-22 | 2018-06-22 | 基于ukf的平面视觉惯性导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108801248A CN108801248A (zh) | 2018-11-13 |
CN108801248B true CN108801248B (zh) | 2020-09-15 |
Family
ID=64071617
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810666461.0A Expired - Fee Related CN108801248B (zh) | 2018-06-22 | 2018-06-22 | 基于ukf的平面视觉惯性导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108801248B (zh) |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102636081A (zh) * | 2011-12-29 | 2012-08-15 | 南京航空航天大学 | 一种基于视觉运动建模的传递对准方法及装置 |
CN103424114A (zh) * | 2012-05-22 | 2013-12-04 | 同济大学 | 一种视觉导航/惯性导航的全组合方法 |
CN104833352A (zh) * | 2015-01-29 | 2015-08-12 | 西北工业大学 | 多介质复杂环境下高精度视觉/惯性组合导航方法 |
CN106052683A (zh) * | 2016-05-25 | 2016-10-26 | 速感科技(北京)有限公司 | 机器人运动姿态估计方法 |
CN106091980A (zh) * | 2016-06-13 | 2016-11-09 | 天津大学 | 一种自主流动式三维形貌测量精度控制方法 |
CN106679648A (zh) * | 2016-12-08 | 2017-05-17 | 东南大学 | 一种基于遗传算法的视觉惯性组合的slam方法 |
-
2018
- 2018-06-22 CN CN201810666461.0A patent/CN108801248B/zh not_active Expired - Fee Related
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102636081A (zh) * | 2011-12-29 | 2012-08-15 | 南京航空航天大学 | 一种基于视觉运动建模的传递对准方法及装置 |
CN103424114A (zh) * | 2012-05-22 | 2013-12-04 | 同济大学 | 一种视觉导航/惯性导航的全组合方法 |
CN104833352A (zh) * | 2015-01-29 | 2015-08-12 | 西北工业大学 | 多介质复杂环境下高精度视觉/惯性组合导航方法 |
CN106052683A (zh) * | 2016-05-25 | 2016-10-26 | 速感科技(北京)有限公司 | 机器人运动姿态估计方法 |
CN106091980A (zh) * | 2016-06-13 | 2016-11-09 | 天津大学 | 一种自主流动式三维形貌测量精度控制方法 |
CN106679648A (zh) * | 2016-12-08 | 2017-05-17 | 东南大学 | 一种基于遗传算法的视觉惯性组合的slam方法 |
Non-Patent Citations (1)
Title |
---|
Developing A Cubature Multi-State Constraint Kalman Filter for Visual-Inertial Navigation System;Trung Nguyen, et al;《canadian conference on computer and robot vision,IEEE computer society》;20170414;第1-9页 * |
Also Published As
Publication number | Publication date |
---|---|
CN108801248A (zh) | 2018-11-13 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111156998B (zh) | 一种基于rgb-d相机与imu信息融合的移动机器人定位方法 | |
US8761439B1 (en) | Method and apparatus for generating three-dimensional pose using monocular visual sensor and inertial measurement unit | |
US10306206B2 (en) | 3-D motion estimation and online temporal calibration for camera-IMU systems | |
US8320616B2 (en) | Image-based system and methods for vehicle guidance and navigation | |
US7313252B2 (en) | Method and system for improving video metadata through the use of frame-to-frame correspondences | |
WO2020253260A1 (zh) | 时间同步处理方法、电子设备及存储介质 | |
IL214151A (en) | Method and device for restoring 3D character | |
CN113551665B (zh) | 一种用于运动载体的高动态运动状态感知***及感知方法 | |
GB2498177A (en) | Apparatus for determining a floor plan of a building | |
TW201711011A (zh) | 定位定向資料分析之系統及其方法 | |
CN110824453A (zh) | 一种基于图像跟踪与激光测距的无人机目标运动估计方法 | |
CN114693754B (zh) | 一种基于单目视觉惯导融合的无人机自主定位方法与*** | |
Jin et al. | Fast and accurate initialization for monocular vision/INS/GNSS integrated system on land vehicle | |
CN113516692A (zh) | 一种多传感器融合的slam方法和装置 | |
CN104848861A (zh) | 一种基于图像消失点识别技术的移动设备姿态测量方法 | |
CN105324792A (zh) | 用于估计移动元件相对于参考方向的角偏差的方法 | |
JP2017524932A (ja) | ビデオ支援着艦誘導システム及び方法 | |
CN113267794A (zh) | 一种基线长度约束的天线相位中心校正方法及装置 | |
CN115560760A (zh) | 一种面向无人机的视觉/激光测距高空导航方法 | |
CN114964276A (zh) | 一种融合惯导的动态视觉slam方法 | |
CN112129263A (zh) | 一种分离移动式立体测距相机及其设计方法 | |
Karam et al. | Integrating a low-cost mems imu into a laser-based slam for indoor mobile mapping | |
CN114690229A (zh) | 一种融合gps的移动机器人视觉惯性导航方法 | |
CN114812601A (zh) | 视觉惯性里程计的状态估计方法、装置、电子设备 | |
CN117073720A (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 | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20200915 Termination date: 20210622 |
|
CF01 | Termination of patent right due to non-payment of annual fee |