CN108801248B - 基于ukf的平面视觉惯性导航方法 - Google Patents

基于ukf的平面视觉惯性导航方法 Download PDF

Info

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
Application number
CN201810666461.0A
Other languages
English (en)
Other versions
CN108801248A (zh
Inventor
刘雅芳
王博
张冬冬
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen Beidou Industry Internet Research Institute
Original Assignee
Shenzhen Beidou Industry Internet Research Institute
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Shenzhen Beidou Industry Internet Research Institute filed Critical Shenzhen Beidou Industry Internet Research Institute
Priority to CN201810666461.0A priority Critical patent/CN108801248B/zh
Publication of CN108801248A publication Critical patent/CN108801248A/zh
Application granted granted Critical
Publication of CN108801248B publication Critical patent/CN108801248B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

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的平面视觉惯性导航方法，其能解决现有技术存在的精度低、存在误差等的问题。
本发明的目的采用以下技术方案实现：
基于UKF的平面视觉惯性导航方法，包括如下步骤：
S1：计算惯性导航系统的非线性惯导误差传播模型；
S2：根据状态变量以及所需导航平面上的三维位置计算出摄像机的投影模型；
S3：根据所述投影模型计算得到任意一个测量特征点相对于两个不同位置的摄像机的位置模型；
S4：根据所述投影模型和位置模型计算得到同一平面上的多个测量特征点匹配构成的非线性测量方程；
S5：根据所述非线性测量方法获取UKF线性误差，根据该UKF线性误差对非线性惯导误差传播模型进行修正。
优选的，步骤S1具体包括如下步骤：
S10：定义惯性导航系统相对于参考坐标系的姿态由全局坐标系{G}表示，以向量形式定义惯性导航系统的状态变量为
Figure GDA0002547992320000021
其中，CθG∈R3×1表示的是全局坐标系{G}在摄像机参考坐标系{C}中的方向，GvC为摄像机在全局坐标系{G}中的速度，GpC为摄像机在全局坐标系{G}中的位置，ba为加速度计的偏差矢量，bg为陀螺仪的偏差矢量；
S11：通过公式
Figure GDA0002547992320000022
Figure GDA0002547992320000023
Figure GDA0002547992320000024
获取惯性导航系统的状态向量的时间更新；其中，ω(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)；
S13：定义惯性导航系统的误差状态向量为：
Figure GDA0002547992320000031
从而建立非线性惯导误差传播模型为
Figure GDA0002547992320000032
其中，
Figure GDA0002547992320000033
和
Figure GDA0002547992320000034
是分别是离散时间状态和系统噪声的传播矩阵。
优选的，步骤S2包括如下子步骤：
S20：设定摄像机观察的任意一个特征点f，设定相对于摄像机中心{C}的位置为Cpf，该特征点在摄像机的图像平面上的均匀和归一化像素坐标分别为
Figure GDA0002547992320000035
和Cz，
Figure GDA0002547992320000036
Figure GDA0002547992320000037
Cpf＝C(CqG)(Gpf-GpC)；
S21：定义一个虚拟坐标系，使虚拟坐标系{V}的中心与摄像机坐标系的中心重合，将摄像机坐标系与虚拟坐标系通过公式
Figure GDA0002547992320000038
进行转换，使得特征点f、pf分别在两个坐标系中的位置通过公式Cpf＝CCV Vpf得到关联；
S22：将Vpf在虚拟影像平面(9)中的投影替代Cpf＝CCV Vpf中的Vpf得到
Figure GDA0002547992320000041
S23：根据公式
Figure GDA0002547992320000042
将特征点f在虚像影像平面中的投影可以被映射到真实影像平面，从而计算得到
Figure GDA0002547992320000043
也就是
Figure GDA0002547992320000044
S24:设定虚拟摄像机的光轴与所需的影像平面正交，则
Figure GDA0002547992320000045
S25：设定一局部坐标系{L}，假设该局部坐标系{L}在所需导航平面和水平面的交点上，其{z}轴是平面的法线，则可计算得到
Figure GDA0002547992320000046
为摄像机的投影模型。
优选的，步骤S3具体包括如下子步骤：
S30：设定两个不同位置的摄像机，该两个摄像机分别对应的参考坐标系为{C}和
Figure GDA00025479923200000412
则特征点f对于这两个参考坐标系的位置为
Figure GDA0002547992320000049
其中，
Figure GDA0002547992320000047
Figure GDA00025479923200000410
为相应的旋转矩阵，
Figure GDA00025479923200000411
为平移矢量；
S31：根据公式
Figure GDA0002547992320000048
得到任意一个测量特征点相对于两个不同位置的摄像机的位置模型
Figure GDA0002547992320000051
即
Figure GDA0002547992320000052
优选的，步骤S4具体包括如下子步骤：
S40：设定当前的特征点fi与前一帧的参考坐标系
Figure GDA0002547992320000058
观察的特征点匹配，则
Figure GDA0002547992320000053
ni为零均值噪声；
S41：建立同一个平面上的M个特征点匹配后构成的非线性测量方法为
Figure GDA0002547992320000054
其中，
Figure GDA0002547992320000056
Figure GDA0002547992320000057
为摄像机在
Figure GDA0002547992320000059
坐标系下的位置误差，
Figure GDA00025479923200000510
为摄像机在
Figure GDA00025479923200000511
坐标系下的姿态角误差，
Figure GDA0002547992320000055
是参考坐标系
Figure GDA00025479923200000512
中匹配特征的归一化像素坐标。
相比现有技术，本发明的有益效果在于：
本发明通过对状态空间进行推导，结合UKF框架进行误差估计和修正，降低视觉惯性导航的误差，适用于任意平面模型。
附图说明
图1为本发明的基于UKF的平面视觉惯性导航方法的流程图；
图2为本发明的摄像机坐标系与虚拟坐标系的结构示意图。
具体实施方式
下面，结合附图以及具体实施方式，对本发明做进一步描述：
如图1所示，本发明提供基于UKF的平面视觉惯性导航方法，本发明的目标是估计移动视觉惯性导航系统相对于参考坐标系的姿态，由{G}表示，硬件结构由安装在惯性测量单元的摄像头组成，嘉定惯性测量单元和摄像机的参考系一直，则{Ipc＝03×1,C(Iqc)＝I3}，包括如下步骤：
S1：计算惯性导航系统的的非线性惯导传播模型；
S2：根据状态变量以及所需导航平面上的三维位置计算出摄像机的投影模型；
S3：根据所述投影模型计算得到任意一个测量特征点相对于两个不同位置的摄像机的位置模型；
S4：根据所述投影模型和位置模型计算得到同一平面上的多个测量特征点匹配构成的非线性测量方程；
S5：根据所述非线性测量方法获取UKF线性误差，根据该UKF线性误差对非线性惯导传播模型进行修正。
S1具体包括如下步骤：
S10：定义惯性导航系统相对于参考坐标系的姿态由全局坐标系{G}表示，以向量形式定义惯性导航系统的状态变量为
Figure GDA0002547992320000061
其中，CθG∈R3×1表示的是全局坐标系{G}在摄像机参考坐标系{C}中的方向，GvC为摄像机在全局坐标系{G}中的速度，GpC为摄像机在全局坐标系{G}中的位置，ba为加速度计的偏差矢量，bg为陀螺仪的偏差矢量；
S11：通过公式
Figure GDA0002547992320000062
Figure GDA0002547992320000063
Figure GDA0002547992320000071
获取惯性导航系统的状态向量的时间更新，加点表示微分；其中，ω(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：定义惯性导航系统的误差状态向量为：
Figure GDA0002547992320000072
从而建立非线性惯导误差传播模型为
Figure GDA0002547992320000073
其中，
Figure GDA0002547992320000074
和
Figure GDA0002547992320000075
是分别是离散时间状态和系统噪声的传播矩阵，
Figure GDA0002547992320000076
Figure GDA0002547992320000077
是过程噪声(假设为零均值广义平稳)与对应的对角协方差矩阵Q∈R15×15，
Figure GDA0002547992320000078
是估计的旋转矩阵，并且
Figure GDA0002547992320000081
步骤S2包括如下子步骤：
S20：设定摄像机观察的任意一个特征点f，设定相对于摄像机中心{C}的位置为Cpf，该特征点在摄像机的图像平面上的均匀和归一化像素坐标分别为
Figure GDA0002547992320000082
和Cz，
Figure GDA0002547992320000083
Figure GDA0002547992320000084
Cpf＝C(CqG)(Gpf-GpC)；I2表示二阶单位阵；
S21：定义一个虚拟坐标系，使虚拟坐标系{V}的中心与摄像机坐标系的中心重合，将摄像机坐标系与虚拟坐标系通过公式
Figure GDA0002547992320000085
进行转换，使得特征点f、pf分别在两个坐标系中的位置通过公式Cpf＝CCV Vpf得到关联；cCv代表摄像机中心和虚拟坐标系之间的转换矩阵。
具体结合图2，摄像机和虚拟相机的坐标系分别由{C}和{V}表示，虚拟摄像机的光轴被选择为始终与所需平面正交。全局坐标系{G}位于水平面上，局部坐标系{L}位于全局坐标系与所需导航坐标系的交点处，使得
Figure GDA0002547992320000086
其中z轴与所需平面正交。
S22：将Vpf在虚拟影像平面
Figure GDA0002547992320000087
中的投影替代Cpf＝CCV Vpf中的Vpf得到
Figure GDA0002547992320000088
S23：根据公式
Figure GDA0002547992320000089
将特征点f在虚像影像平面中的投影可以被映射到真实影像平面，从而计算得到
Figure GDA00025479923200000810
也就是
Figure GDA0002547992320000091
S24:设定虚拟摄像机的光轴与所需的影像平面正交，则
Figure GDA0002547992320000092
S25：设定一局部坐标系{L}，假设该局部坐标系{L}在所需导航平面和水平面的交点上，其{z}轴是平面的法线，参考图2，可得到虚拟摄像机坐标系相对于局部坐标系的方向为C(VqL)＝diag(1,-1,-1)，
Figure GDA0002547992320000093
从图2可以看出LpC为LpC＝C(LqG)GpC+GpL，上式两边同时乘以
Figure GDA0002547992320000094
则可以得到
Figure GDA0002547992320000095
Figure GDA0002547992320000096
将
Figure GDA0002547992320000097
代入
Figure GDA0002547992320000098
则可计算得到
Figure GDA0002547992320000099
为摄像机的投影模型。为了测量这个值，可以使用用于测量斜率相对于重力角度的测斜仪。
步骤S3具体包括如下子步骤：
S30：设定两个不同位置的摄像机，该两个摄像机分别对应的参考坐标系为{C}和
Figure GDA00025479923200000910
则特征点f对于这两个参考坐标系的位置为
Figure GDA0002547992320000101
Figure GDA0002547992320000109
其中，
Figure GDA0002547992320000102
Figure GDA00025479923200001010
为相应的旋转矩阵，
Figure GDA00025479923200001011
为平移矢量；
Figure GDA00025479923200001012
将两个摄像机视图中的特征f的观察结果与其相应的平移关联起来，由旋转矩阵
Figure GDA00025479923200001013
和平移矢量
Figure GDA00025479923200001014
进行表示，因此，Cpf的投影与系统运动(状态变量)相关，可以对累积的惯性导航系统误差施加约束。惯性测量单元的摄像机及在当前{C}时间下的坐标系并且以
Figure GDA00025479923200001015
滞后于当前的
Figure GDA00025479923200001016
不同时刻之间的相对旋转和平移由
Figure GDA00025479923200001017
和
Figure GDA00025479923200001018
表示。假定全局参考系G位于水平面上，其中z轴垂直于水平面。位于所需平面上的样本特征点f被认为是在相机的视野中；
S31：由于该点属于目标平面，
Figure GDA00025479923200001019
可以用相机姿态
Figure GDA00025479923200001020
Figure GDA00025479923200001021
以及观测值
Figure GDA0002547992320000103
表示，根据公式
Figure GDA0002547992320000104
得到任意一个测量特征点相对于两个不同位置的摄像机的位置模型
Figure GDA0002547992320000105
即
Figure GDA0002547992320000106
步骤S4具体包括如下子步骤：
S40：设定当前的特征点fi与前一帧的参考坐标系
Figure GDA00025479923200001022
观察的特征点匹配，则
Figure GDA0002547992320000107
ni为零均值噪声；具有协方差矩阵
Figure GDA0002547992320000108
S41：建立同一个平面上的M个特征点匹配后构成的非线性测量方法为
Figure GDA0002547992320000111
其中，
Figure GDA00025479923200001119
Figure GDA00025479923200001120
为摄像机在
Figure GDA00025479923200001121
坐标系下的位置误差，
Figure GDA00025479923200001122
为摄像机在
Figure GDA00025479923200001123
坐标系下的姿态角误差，
Figure GDA0002547992320000112
是参考坐标系
Figure GDA00025479923200001124
中匹配特征的归一化像素坐标，相应的测量协方差矩阵为
Figure GDA0002547992320000113
在本发明中，对于线性过程模型，协方差矩阵传播方程为
Figure GDA0002547992320000114
其中，
Figure GDA0002547992320000115
并且
Figure GDA0002547992320000116
当记录新图像时，误差状态
Figure GDA00025479923200001125
和
Figure GDA00025479923200001126
更新为δxcam，误差协方差矩阵更新为
Figure GDA0002547992320000117
其中，
Figure GDA0002547992320000118
当定义的平面上的特征被检测和匹配时，UKF被设置为执行测量更新。基于
Figure GDA0002547992320000119
的状态空间模型，UKF线性估计误差则为
Figure GDA00025479923200001110
其中
Figure GDA00025479923200001111
是测量预测，Kk是卡尔曼增益矩阵。δx和
Figure GDA00025479923200001112
的联合统计量作为2N+1个sigma点传播，其中N是状态维数，N＝15+6+2M。设联合状态向量表示为
Figure GDA00025479923200001113
Figure GDA00025479923200001114
是误差协方差矩阵
Figure GDA00025479923200001115
的预测，其中
Figure GDA00025479923200001116
表示矩阵的直接和。然后使用联合误差协方差矩阵的矩阵平方根生成sigma点
Figure GDA00025479923200001117
通过公式
Figure GDA00025479923200001118
中传播sigma点，可以近似估计误差状态δx和观测值Cy之间的相关性，这相关性构成了卡尔曼增益的基础。
UKF中的权重设置为
Figure GDA0002547992320000121
当l＝0时，
Figure GDA0002547992320000122
且
Figure GDA0002547992320000123
这里λ＝α2(N+κ)-N，其中参数设为α＝0.1，β＝2和κ＝0，这也决定了sigam点通过权重
Figure GDA0002547992320000124
进行传播。
基于上述的N＝15+6+2M时，基于UKF的运动估计算法流程如下：
Figure GDA0002547992320000125
Figure GDA0002547992320000131
对本领域的技术人员来说，可根据以上描述的技术方案以及构思，做出其它各种相应的改变以及形变，而所有的这些改变以及形变都应该属于本发明权利要求的保护范围之内。

Claims (1)

1.基于UKF的平面视觉惯性导航方法，其特征在于，包括如下步骤：
S1：计算惯性导航系统的非线性惯导误差传播模型；
S2：根据状态变量以及所需导航平面上的三维位置计算出摄像机的投影模型；
S3：根据所述投影模型计算得到任意一个测量特征点相对于两个不同位置的摄像机的位置模型；
S4：根据所述投影模型和位置模型计算得到同一平面上的多个测量特征点匹配构成的非线性测量方程；
S5：根据所述非线性测量方法获取UKF线性误差，根据该UKF线性误差对非线性惯导误差传播模型进行修正；
其中，步骤S1具体包括如下步骤：
S10：定义惯性导航系统相对于参考坐标系的姿态由全局坐标系{G}表示，以向量形式定义惯性导航系统的状态变量为
Figure FDA0002547992310000011
其中，CθG∈R3×1表示的是全局坐标系{G}在摄像机参考坐标系{C}中的方向，GvC为摄像机在全局坐标系{G}中的速度，GpC为摄像机在全局坐标系{G}中的位置，ba为加速度计的偏差矢量，bg为陀螺仪的偏差矢量；
S11：通过公式
Figure FDA0002547992310000012
Figure FDA0002547992310000013
Figure FDA0002547992310000021
获取惯性导航系统的状态向量的时间更新；其中，ω(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)；
S13：定义惯性导航系统的误差状态向量为：
Figure FDA0002547992310000022
从而建立非线性惯导误差传播模型为
Figure FDA0002547992310000023
其中，
Figure FDA0002547992310000024
和
Figure FDA0002547992310000025
是分别是离散时间状态和系统噪声的传播矩阵；
步骤S2包括如下子步骤：
S20：设定摄像机观察的任意一个特征点f，设定相对于摄像机中心{C}的位置为Cpf，该特征点在摄像机的图像平面上的均匀和归一化像素坐标分别为
Figure FDA0002547992310000026
和Cz，
Figure FDA0002547992310000027
Figure FDA0002547992310000028
Cpf＝C(CqG)(Gpf-GpC)；
S21：定义一个虚拟坐标系，使虚拟坐标系{V}的中心与摄像机坐标系的中心重合，将摄像机坐标系与虚拟坐标系通过公式
Figure FDA0002547992310000029
进行转换，使得特征点f、pf分别在两个坐标系中的位置通过公式Cpf＝CCV Vpf得到关联；
S22：将Vpf在虚拟影像平面
Figure FDA0002547992310000031
中的投影替代Cpf＝CCV Vpf中的Vpf得到
Figure FDA0002547992310000032
S23：根据公式
Figure FDA0002547992310000033
将特征点f在虚像影像平面中的投影可以被映射到真实影像平面，从而计算得到
Figure FDA0002547992310000034
也就是
Figure FDA0002547992310000035
S24:设定虚拟摄像机的光轴与所需的影像平面正交，则
Figure FDA0002547992310000036
S25：设定一局部坐标系{L}，假设该局部坐标系{L}在所需导航平面和水平面的交点上，其{z}轴是平面的法线，则可计算得到
Figure FDA0002547992310000037
为摄像机的投影模型；
步骤S3具体包括如下子步骤：
S30：设定两个不同位置的摄像机，该两个摄像机分别对应的参考坐标系为{C}和{Cl}，则特征点f对于这两个参考坐标系的位置为
Figure FDA0002547992310000038
其中，
Figure FDA0002547992310000039
Figure FDA00025479923100000310
为相应的旋转矩阵，
Figure FDA00025479923100000311
为平移矢量；
S31：根据公式
Figure FDA0002547992310000041
得到任意一个测量特征点相对于两个不同位置的摄像机的位置模型
Figure FDA0002547992310000042
即
Figure FDA0002547992310000043
步骤S4具体包括如下子步骤：
S40：设定当前的特征点fi与前一帧的参考坐标系{Cl}观察的特征点匹配，则
Figure FDA0002547992310000044
ni为零均值噪声；
S41：建立同一个平面上的M个特征点匹配后构成的非线性测量方法为
Figure FDA0002547992310000045
其中，
Figure FDA0002547992310000046
Figure FDA0002547992310000047
为摄像机在Cl坐标系下的位置误差，
Figure FDA0002547992310000048
为摄像机在Cl坐标系下的姿态角误差，
Figure FDA0002547992310000049
是参考坐标系Cl中匹配特征的归一化像素坐标。
CN201810666461.0A 2018-06-22 2018-06-22 基于ukf的平面视觉惯性导航方法 Expired - Fee Related CN108801248B (zh)

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)

* Cited by examiner, † Cited by third party
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方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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
US11940277B2 (en) Vision-aided inertial navigation system for ground vehicle localization
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) 一种基于单目视觉惯导融合的无人机自主定位方法与系统
CN104848861A (zh) 一种基于图像消失点识别技术的移动设备姿态测量方法
Jin et al. Fast and accurate initialization for monocular vision/INS/GNSS integrated system on land vehicle
CN105324792A (zh) 用于估计移动元件相对于参考方向的角偏差的方法
CN113516692A (zh) 一种多传感器融合的slam方法和装置
JP2017524932A (ja) ビデオ支援着艦誘導システム及び方法
CN113267794A (zh) 一种基线长度约束的天线相位中心校正方法及装置
CN112129263A (zh) 一种分离移动式立体测距相机及其设计方法
Karam et al. Integrating a low-cost mems imu into a laser-based slam for indoor mobile mapping
CN115560760A (zh) 一种面向无人机的视觉/激光测距高空导航方法
CN114690229A (zh) 一种融合gps的移动机器人视觉惯性导航方法
CN114812601A (zh) 视觉惯性里程计的状态估计方法、装置、电子设备
CN108801248B (zh) 基于ukf的平面视觉惯性导航方法

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