CN111156997A - 一种基于相机内参在线标定的视觉/惯性组合导航方法 - Google Patents

一种基于相机内参在线标定的视觉/惯性组合导航方法 Download PDF

Info

Publication number
CN111156997A
CN111156997A CN202010135460.0A CN202010135460A CN111156997A CN 111156997 A CN111156997 A CN 111156997A CN 202010135460 A CN202010135460 A CN 202010135460A CN 111156997 A CN111156997 A CN 111156997A
Authority
CN
China
Prior art keywords
time
inertial
camera
visual
integration
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
Application number
CN202010135460.0A
Other languages
English (en)
Other versions
CN111156997B (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.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
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 Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN202010135460.0A priority Critical patent/CN111156997B/zh
Publication of CN111156997A publication Critical patent/CN111156997A/zh
Application granted granted Critical
Publication of CN111156997B publication Critical patent/CN111156997B/zh
Active 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
    • 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

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

本发明公开了一种基于相机内参在线标定的视觉/惯性组合导航方法，步骤：采集k时刻视觉传感器数据S(k)、加速度计数据
Figure DDA0002397140380000011
和陀螺仪数据
Figure DDA0002397140380000012
利用视觉传感器数据S(k)进行相邻两图像帧之间的特征匹配与检测；利用惯性传感器数据
Figure DDA0002397140380000013
和
Figure DDA0002397140380000014
进行相邻两图像帧之间的预积分；联合视觉重投影误差和惯性预积分误差优化求解载体导航信息和相机内参；输出载体导航信息和相机内参。本发明可以在视觉/惯性导航框架下完成相机内参的标定，并且能够有效解决因相机内参变化而导致的导航系统精度降低的问题。

Description

一种基于相机内参在线标定的视觉/惯性组合导航方法
技术领域
本发明属于机器人导航领域，特别涉及了一种视觉/惯性组合导航方法。
背景技术
视觉/惯性组合导航系统因其具有良好的鲁棒性而成为机器人自主导航领域中的一大研究热点。视觉传感器可以抑制惯性传感器的漂移问题，惯性传感器可以弥补纹理少、光照不充足环境下视觉传感器无法工作的问题，因此视觉/惯性组合导航系统具有广阔的发展前景。
目前大多数视觉/惯性组合导航方法在运行过程中将相机内参作为固定参数，并通过传统的离线标定方法得到相机的内参。但实际中，相机内参可能会因为机械冲击或其它因素而发生变化，进而降低了组合导航系统的精度和可靠性。
发明内容
为了解决上述背景技术提到的技术问题，本发明提出了一种基于相机内参在线标定的视觉/惯性组合导航方法，解决因相机内参变化而导致的导航系统精度降低的问题。
为了实现上述技术目的，本发明的技术方案为：
一种基于相机内参在线标定的视觉/惯性组合导航方法，包括以下步骤：
(1)采集k时刻视觉传感器数据S(k)、加速度计数据
Figure BDA0002397140360000011
和陀螺仪数据
Figure BDA0002397140360000012
(2)利用视觉传感器数据S(k)进行相邻两图像帧之间的特征匹配与检测；
(3)利用惯性传感器数据
Figure BDA0002397140360000013
和
Figure BDA0002397140360000014
进行相邻两图像帧之间的预积分；
(4)联合视觉重投影误差和惯性预积分误差优化求解载体导航信息和相机内参；
(5)输出载体导航信息和相机内参，并返回步骤(1)。
进一步地，在步骤(2)中，利用视觉传感器数据S(k)进行相邻两图像帧之间的特征匹配与检测的方法如下：
所述视觉传感器数据S(k)包括第k帧的图像信息和第k-1帧图像的FAST特征点；使用KLT稀疏光流法对第k-1帧图像的FAST特征点进行跟踪，完成前后两帧图像特征点的匹配，同时对第k帧图像进行新的FAST特征点检测，使每张图像中的特征点数目保持在一定值。
进一步地，在步骤(3)中，利用惯性传感器数据
Figure BDA0002397140360000021
和
Figure BDA0002397140360000022
进行相邻两图像帧之间的预积分的方法如下：
所述k时刻得到的惯性传感器数据
Figure BDA0002397140360000023
和
Figure BDA0002397140360000024
包括从k-1时刻到k时刻的加速度计数据
Figure BDA0002397140360000025
和陀螺仪数据
Figure BDA0002397140360000026
其中i＝0,1,2,···,(t(k)-t(k-1))/Δt，t(k)为k 时刻对应的采样时间，t(k-1)为k-1时刻对应的采样时间，Δt为惯性传感器的采样周期，惯性传感器测量模型：
Figure BDA0002397140360000027
Figure BDA0002397140360000028
上式中，na和nω分别为加速度计和陀螺仪白噪声；
Figure BDA0002397140360000029
和
Figure BDA00023971403600000210
为加速度计和陀螺仪的随机游走，其导数为白噪声；
Figure BDA00023971403600000211
为加速度计量测的理想值，
Figure BDA00023971403600000212
为陀螺仪量测的理想值；gW为导航系下的重力；
Figure BDA00023971403600000213
为采样时刻导航坐标系到机体坐标系的旋转矩阵；
相邻两惯性帧之间的预积分过程：
Figure BDA0002397140360000031
Figure BDA0002397140360000032
Figure BDA0002397140360000033
上式中，
Figure BDA0002397140360000034
为位置预积分，
Figure BDA0002397140360000035
为速度预积分，旋转用四元数γ表示，
Figure BDA0002397140360000036
为旋转预积分；初始时，
Figure BDA0002397140360000037
和
Figure BDA0002397140360000038
为0，
Figure BDA0002397140360000039
为单位四元数，R(γ)表示将四元数转化为旋转矩阵，
Figure BDA00023971403600000310
表示四元数的乘法运算；在相邻两图像帧之间有若干惯性传感器数据，通过上式，采用迭代的方式对相邻两图像帧之间的所有惯性传感器数据进行预积分，从而使视觉数据的频率与惯性预积分频率保持一致，得到相邻两图像帧之间的惯性预积分
Figure BDA00023971403600000311
和
Figure BDA00023971403600000312
进一步地，在步骤(4)中，联合视觉重投影误差和惯性预积分误差优化求解载体导航信息和相机内参的具体过程如下：
(a)建立优化变量X：
X＝[x0,x1,···,xn,xc,P0,P1,···,Pm]
上式中，
Figure BDA00023971403600000313
n为最后一帧的序号，
Figure BDA00023971403600000314
Figure BDA00023971403600000315
和
Figure BDA00023971403600000316
分别表示第k帧的载体位置、速度、四元数、加速度计和陀螺仪的随机游走；xc＝[fx,fy,cx,cy,k1,k2]，fx和fy为相机的焦距，cx和cy为相机的光心位置， k1和k2为相机的径向畸变参数；Pl,l∈[0,m]为特征点坐标，m为最后一个特征点的序号；
(b)建立优化函数：
Figure BDA00023971403600000317
其中，||ep-HpX||2为边缘化约束，{ep,Hp}为边缘化的先验信息；
Figure BDA00023971403600000318
为IMU预积分误差，
Figure BDA0002397140360000041
表示该IMU预积分误差项是第k-1帧图像与第k帧图像之间的IMU预积分误差，B是所有IMU测量的集合，
Figure BDA0002397140360000042
为IMU预积分误差的协方差；
Figure BDA0002397140360000043
为相机内参为变量的视觉重投影误差，
Figure BDA0002397140360000044
表示该视觉重投影误差项是第k帧图像上关于第l特征点的视觉重投影误差，C是所有图像帧中至少有两次观测的特征集，
Figure BDA0002397140360000045
为视觉重投影误差的协方差；
Figure BDA0002397140360000046
对于k时刻的第l个特征点Pl，将相机内参建模到视觉重投影误差
Figure BDA0002397140360000047
中：
Figure BDA0002397140360000048
上式中，Zl为在k时刻相机系下Z轴方向的值，K为相机的投影矩阵，
Figure BDA0002397140360000049
表示消除径向畸变后的特征点位置：
Figure BDA00023971403600000410
Figure BDA00023971403600000411
其中，
Figure BDA00023971403600000412
为特征点Pl在像素平面上的实际成像位置；
Figure BDA00023971403600000413
为消除径向畸变后特征点Pl在像素平面上的位置；
Figure BDA00023971403600000414
xd、yd分别为相机图像坐标系下特征点Pl在x和y方向的值；
Figure BDA00023971403600000415
为k时刻机体系在导航系下的位姿，
Figure BDA00023971403600000416
为相机和惯性传感器之间的旋转和平移；
计算视觉重投影误差
Figure BDA00023971403600000417
关于其相关变量的雅可比矩阵J[0]～J[7]：
Figure BDA00023971403600000418
Figure BDA00023971403600000419
Figure BDA0002397140360000051
Figure BDA0002397140360000052
Figure BDA0002397140360000053
Figure BDA0002397140360000054
Figure BDA0002397140360000055
Figure BDA0002397140360000056
上式中，Xl、Yl、Zl分别为特征点Pl在k时刻相机系下X、Y、Z轴方向的值；
Figure BDA0002397140360000057
为k时刻机体系在导航系下姿态的四元数表示；K1的形式如下：
Figure BDA0002397140360000058
[a]^表示把三维实向量映射为3×3反对称阵；
相邻两图像帧之间的惯性预积分误差由预积分预测值与预积分值之差得到：
Figure BDA0002397140360000059
上式中，
Figure BDA0002397140360000061
为k-1时刻导航坐标系到机体坐标系的旋转矩阵；
Figure BDA0002397140360000062
分别为 k时刻和k-1时刻机体坐标系在导航坐标系下的位置；
Figure BDA0002397140360000063
分别为k时刻和 k-1时刻机体坐标系在导航坐标系下的速度；Δtk为相邻两图像帧之间的时间间隔；
Figure BDA0002397140360000064
分别为k时刻和k-1时刻机体坐标系在导航坐标系下旋转的四元数；
Figure BDA0002397140360000065
Figure BDA0002397140360000066
分别为k时刻和k-1时刻机体坐标系下的加速度计随机游走；
Figure BDA0002397140360000067
分别为k时刻和k-1时刻机体坐标系下的陀螺仪随机游走，[γ]xyz表示取四元数γ的 x、y、z分量；
(c)使用高斯-牛顿法对优化函数进行迭代求解，当达到误差收敛或者设定的最大迭代次数时停止迭代，求得载体导航信息和相机内参。
进一步地，在步骤(c)中，所述设定的最大迭代次数为30。
采用上述技术方案带来的有益效果：
本发明将相机内参作为优化变量建模到视觉重投影误差中，在视觉/惯性导航系统的运行中联合视觉传感器和惯性传感器数据对相机内参和载体导航信息实时优化求解，可以在相机内参因为机械冲击或其它因素而发生变化的情况下及时修正，因而能够有效解决由于相机内参变化而导致的视觉/惯性导航系统精度降低的问题，可以获得高精度的导航信息。
附图说明
图1是本发明的基本流程图。
具体实施方式
以下将结合附图，对本发明的技术方案进行详细说明。
本发明设计了一种基于相机内参在线标定的视觉/惯性组合导航方法，如图1 所示，步骤如下：
步骤1：采集k时刻视觉传感器数据S(k)、加速度计数据
Figure BDA0002397140360000071
和陀螺仪数据
Figure BDA0002397140360000072
步骤2：利用视觉传感器数据S(k)进行相邻两图像帧之间的特征匹配与检测；
步骤3：利用惯性传感器数据
Figure BDA0002397140360000073
和
Figure BDA0002397140360000074
进行相邻两图像帧之间的预积分；
步骤4：联合视觉重投影误差和惯性预积分误差优化求解载体导航信息和相机内参；
步骤5：输出载体导航信息和相机内参，并返回步骤1。
在本实施例中，上述步骤2可以采用如下优选方案实现：
利用视觉传感器数据S(k)进行相邻两图像帧之间的特征匹配与检测的方法如下：
所述视觉传感器数据S(k)包括第k帧的图像信息和第k-1帧图像的FAST特征点；使用KLT稀疏光流法对第k-1帧图像的FAST特征点进行跟踪，完成前后两帧图像特征点的匹配，同时对第k帧图像进行新的FAST特征点检测，使每张图像中的特征点数目保持在一定值。
在本实施例中，上述步骤3可以采用如下优选方案实现：
利用惯性传感器数据
Figure BDA0002397140360000075
和
Figure BDA0002397140360000076
进行相邻两图像帧之间的预积分的方法如下：
所述k时刻得到的惯性传感器数据
Figure BDA0002397140360000077
和
Figure BDA0002397140360000078
包括从k-1时刻到k时刻的加速度计数据
Figure BDA0002397140360000079
和陀螺仪数据
Figure BDA00023971403600000710
其中i＝0,1,2,···,(t(k)-t(k-1))/Δt，t(k)为k 时刻对应的采样时间，t(k-1)为k-1时刻对应的采样时间，Δt为惯性传感器的采样周期，惯性传感器测量模型：
Figure BDA00023971403600000711
Figure BDA00023971403600000712
上式中，na和nω分别为加速度计和陀螺仪白噪声；
Figure BDA0002397140360000081
和
Figure BDA0002397140360000082
为加速度计和陀螺仪的随机游走，其导数为白噪声；
Figure BDA0002397140360000083
为加速度计量测的理想值，
Figure BDA0002397140360000084
为陀螺仪量测的理想值；gW为导航系下的重力；
Figure BDA0002397140360000085
为采样时刻导航坐标系到机体坐标系的旋转矩阵；
相邻两惯性帧之间的预积分过程：
Figure BDA0002397140360000086
Figure BDA0002397140360000087
Figure BDA0002397140360000088
上式中，
Figure BDA0002397140360000089
为位置预积分，
Figure BDA00023971403600000810
为速度预积分，旋转用四元数γ表示，
Figure BDA00023971403600000811
为旋转预积分；初始时，
Figure BDA00023971403600000812
和
Figure BDA00023971403600000813
为0，
Figure BDA00023971403600000814
为单位四元数，R(γ)表示将四元数转化为旋转矩阵，
Figure BDA00023971403600000815
表示四元数的乘法运算；在相邻两图像帧之间有若干惯性传感器数据，通过上式，采用迭代的方式对相邻两图像帧之间的所有惯性传感器数据进行预积分，从而使视觉数据的频率与惯性预积分频率保持一致，得到相邻两图像帧之间的惯性预积分
Figure BDA00023971403600000816
和
Figure BDA00023971403600000817
在本实施例中，上述步骤4可以采用如下优选方案实现：
联合视觉重投影误差和惯性预积分误差优化求解载体导航信息和相机内参的具体过程如下：
(a)建立优化变量X：
X＝[x0,x1,···,xn,xc,P0,P1,···,Pm]
上式中，
Figure BDA00023971403600000818
n为最后一帧的序号，
Figure BDA00023971403600000819
Figure BDA00023971403600000820
和
Figure BDA00023971403600000821
分别表示第k帧的载体位置、速度、四元数、加速度计和陀螺仪的随机游走；xc＝[fx,fy,cx,cy,k1,k2]，fx和fy为相机的焦距，cx和cy为相机的光心位置，k1和k2为相机的径向畸变参数；Pl,l∈[0,m]为特征点坐标，m为最后一个特征点的序号；
(b)建立优化函数：
Figure BDA0002397140360000091
其中，||ep-HpX||2为边缘化约束，{ep,Hp}为边缘化的先验信息；
Figure BDA0002397140360000092
为 IMU预积分误差，
Figure BDA0002397140360000093
表示该IMU预积分误差项是第k-1帧图像与第k帧图像之间的IMU预积分误差，B是所有IMU测量的集合，
Figure BDA0002397140360000094
为IMU预积分误差的协方差；
Figure BDA0002397140360000095
为相机内参为变量的视觉重投影误差，
Figure BDA0002397140360000096
表示该视觉重投影误差项是第k帧图像上关于第l特征点的视觉重投影误差，C是所有图像帧中至少有两次观测的特征集，
Figure BDA0002397140360000097
为视觉重投影误差的协方差；
Figure BDA0002397140360000098
对于k时刻的第l个特征点Pl，将相机内参建模到视觉重投影误差
Figure BDA0002397140360000099
中：
Figure BDA00023971403600000910
上式中，Zl为在k时刻相机系下Z轴方向的值，K为相机的投影矩阵，
Figure BDA00023971403600000911
表示消除径向畸变后的特征点位置：
Figure BDA00023971403600000912
Figure BDA00023971403600000913
其中，
Figure BDA00023971403600000914
为特征点Pl在像素平面上的实际成像位置；
Figure BDA00023971403600000915
为消除径向畸变后特征点Pl在像素平面上的位置；
Figure BDA00023971403600000916
xd、yd分别为相机图像坐标系下特征点Pl在x和y方向的值；
Figure BDA00023971403600000917
为k时刻机体系在导航系下的位姿，
Figure BDA00023971403600000918
为相机和惯性传感器之间的旋转和平移；
计算视觉重投影误差
Figure BDA00023971403600000919
关于其相关变量的雅可比矩阵J[0]～J[7]：
Figure BDA0002397140360000101
Figure BDA0002397140360000102
Figure BDA0002397140360000103
Figure BDA0002397140360000104
Figure BDA0002397140360000105
Figure BDA0002397140360000106
Figure BDA0002397140360000107
Figure BDA0002397140360000108
上式中，Xl、Yl、Zl分别为特征点Pl在k时刻相机系下X、Y、Z轴方向的值；
Figure BDA0002397140360000109
为k时刻机体系在导航系下姿态的四元数表示；K1的形式如下：
Figure BDA00023971403600001010
[a]^表示把三维实向量映射为3×3反对称阵，即：
Figure BDA0002397140360000111
相邻两图像帧之间的惯性预积分误差由预积分预测值与预积分值之差得到：
Figure BDA0002397140360000112
上式中，
Figure BDA0002397140360000113
为k-1时刻导航坐标系到机体坐标系的旋转矩阵；
Figure BDA0002397140360000114
分别为 k时刻和k-1时刻机体坐标系在导航坐标系下的位置；
Figure BDA0002397140360000115
分别为k时刻和 k-1时刻机体坐标系在导航坐标系下的速度；Δtk为相邻两图像帧之间的时间间隔；
Figure BDA0002397140360000116
分别为k时刻和k-1时刻机体坐标系在导航坐标系下旋转的四元数；
Figure BDA0002397140360000117
Figure BDA0002397140360000118
分别为k时刻和k-1时刻机体坐标系下的加速度计随机游走；
Figure BDA0002397140360000119
分别为k时刻和k-1时刻机体坐标系下的陀螺仪随机游走，[γ]xyz表示取四元数γ的 x、y、z分量；
(c)使用高斯-牛顿法对优化函数进行迭代求解，当达到误差收敛或者设定的最大迭代次数时停止迭代，求得载体导航信息和相机内参。
以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (5)

1.一种基于相机内参在线标定的视觉/惯性组合导航方法，其特征在于，包括以下步骤：
(1)采集k时刻视觉传感器数据S(k)、加速度计数据
Figure FDA0002397140350000011
和陀螺仪数据
Figure FDA0002397140350000012
(2)利用视觉传感器数据S(k)进行相邻两图像帧之间的特征匹配与检测；
(3)利用惯性传感器数据
Figure FDA0002397140350000013
和
Figure FDA0002397140350000014
进行相邻两图像帧之间的预积分；
(4)联合视觉重投影误差和惯性预积分误差优化求解载体导航信息和相机内参；
(5)输出载体导航信息和相机内参，并返回步骤(1)。
2.根据权利要求1所述基于相机内参在线标定的视觉/惯性组合导航方法，其特征在于，在步骤(2)中，利用视觉传感器数据S(k)进行相邻两图像帧之间的特征匹配与检测的方法如下：
所述视觉传感器数据S(k)包括第k帧的图像信息和第k-1帧图像的FAST特征点；使用KLT稀疏光流法对第k-1帧图像的FAST特征点进行跟踪，完成前后两帧图像特征点的匹配，同时对第k帧图像进行新的FAST特征点检测，使每张图像中的特征点数目保持在一定值。
3.根据权利要求1所述基于相机内参在线标定的视觉/惯性组合导航方法，其特征在于，在步骤(3)中，利用惯性传感器数据
Figure FDA0002397140350000015
和
Figure FDA0002397140350000016
进行相邻两图像帧之间的预积分的方法如下：
所述k时刻得到的惯性传感器数据
Figure FDA0002397140350000017
和
Figure FDA0002397140350000018
包括从k-1时刻到k时刻的加速度计数据
Figure FDA0002397140350000019
和陀螺仪数据
Figure FDA00023971403500000110
其中i＝0,1,2,···,(t(k)-t(k-1))/Δt，t(k)为k时刻对应的采样时间，t(k-1)为k-1时刻对应的采样时间，Δt为惯性传感器的采样周期，惯性传感器测量模型：
Figure FDA0002397140350000021
Figure FDA0002397140350000022
上式中，na和nω分别为加速度计和陀螺仪白噪声；
Figure FDA0002397140350000023
和
Figure FDA0002397140350000024
为加速度计和陀螺仪的随机游走，其导数为白噪声；
Figure FDA0002397140350000025
为加速度计量测的理想值，
Figure FDA0002397140350000026
为陀螺仪量测的理想值；gW为导航系下的重力；
Figure FDA0002397140350000027
为采样时刻导航坐标系到机体坐标系的旋转矩阵；
相邻两惯性帧之间的预积分过程：
Figure FDA0002397140350000028
Figure FDA0002397140350000029
Figure FDA00023971403500000210
上式中，为位置预积分，
Figure FDA00023971403500000212
为速度预积分，旋转用四元数γ表示，
Figure FDA00023971403500000213
为旋转预积分；初始时，
Figure FDA00023971403500000214
和
Figure FDA00023971403500000215
为0，
Figure FDA00023971403500000216
为单位四元数，R(γ)表示将四元数转化为旋转矩阵，
Figure FDA00023971403500000217
表示四元数的乘法运算；在相邻两图像帧之间有若干惯性传感器数据，通过上式，采用迭代的方式对相邻两图像帧之间的所有惯性传感器数据进行预积分，从而使视觉数据的频率与惯性预积分频率保持一致，得到相邻两图像帧之间的惯性预积分
Figure FDA00023971403500000218
和
Figure FDA00023971403500000219
4.根据权利要求3所述基于相机内参在线标定的视觉/惯性组合导航方法，其特征在于，在步骤(4)中，联合视觉重投影误差和惯性预积分误差优化求解载体导航信息和相机内参的具体过程如下：
(a)建立优化变量X：
X＝[x0,x1,···,xn,xc,P0,P1,···,Pm]
上式中，
Figure FDA00023971403500000220
n为最后一帧的序号，
Figure FDA00023971403500000221
Figure FDA0002397140350000031
和
Figure FDA0002397140350000032
分别表示第k帧的载体位置、速度、四元数、加速度计和陀螺仪的随机游走；xc＝[fx,fy,cx,cy,k1,k2]，fx和fy为相机的焦距，cx和cy为相机的光心位置，k1和k2为相机的径向畸变参数；Pl,l∈[0,m]为特征点坐标，m为最后一个特征点的序号；
(b)建立优化函数：
Figure FDA0002397140350000033
其中，||ep-HpX||2为边缘化约束，{ep,Hp}为边缘化的先验信息；
Figure FDA0002397140350000034
为IMU预积分误差，
Figure FDA0002397140350000035
表示该IMU预积分误差项是第k-1帧图像与第k帧图像之间的IMU预积分误差，B是所有IMU测量的集合，
Figure FDA0002397140350000036
为IMU预积分误差的协方差；
Figure FDA0002397140350000037
为相机内参为变量的视觉重投影误差，
Figure FDA0002397140350000038
表示该视觉重投影误差项是第k帧图像上关于第l特征点的视觉重投影误差，C是所有图像帧中至少有两次观测的特征集，
Figure FDA0002397140350000039
为视觉重投影误差的协方差；
Figure FDA00023971403500000310
对于k时刻的第l个特征点Pl，将相机内参建模到视觉重投影误差
Figure FDA00023971403500000311
中：
Figure FDA00023971403500000312
上式中，Zl为在k时刻相机系下Z轴方向的值，K为相机的投影矩阵，
Figure FDA00023971403500000313
表示消除径向畸变后的特征点位置：
Figure FDA00023971403500000314
Figure FDA00023971403500000315
其中，
Figure FDA00023971403500000316
为特征点Pl在像素平面上的实际成像位置；
Figure FDA00023971403500000317
为消除径向畸变后特征点Pl在像素平面上的位置；
Figure FDA00023971403500000318
xd、yd分别为相机图像坐标系下特征点Pl在x和y方向的值；
Figure FDA00023971403500000319
为k时刻机体系在导航系下的位姿，
Figure FDA0002397140350000041
为相机和惯性传感器之间的旋转和平移；
计算视觉重投影误差
Figure FDA0002397140350000042
关于其相关变量的雅可比矩阵J[0]～J[7]：
Figure FDA0002397140350000043
Figure FDA0002397140350000044
Figure FDA0002397140350000045
Figure FDA0002397140350000046
Figure FDA0002397140350000047
Figure FDA0002397140350000048
Figure FDA0002397140350000049
Figure FDA00023971403500000410
上式中，Xl、Yl、Zl分别为特征点Pl在k时刻相机系下X、Y、Z轴方向的值；
Figure FDA00023971403500000411
为k时刻机体系在导航系下姿态的四元数表示；K1的形式如下：
Figure FDA00023971403500000412
[a]^表示把三维实向量映射为3×3反对称阵；
相邻两图像帧之间的惯性预积分误差由预积分预测值与预积分值之差得到：
Figure FDA0002397140350000051
上式中，
Figure FDA0002397140350000052
为k-1时刻导航坐标系到机体坐标系的旋转矩阵；
Figure FDA0002397140350000053
分别为k时刻和k-1时刻机体坐标系在导航坐标系下的位置；
Figure FDA0002397140350000054
分别为k时刻和k-1时刻机体坐标系在导航坐标系下的速度；Δtk为相邻两图像帧之间的时间间隔；
Figure FDA0002397140350000055
分别为k时刻和k-1时刻机体坐标系在导航坐标系下旋转的四元数；
Figure FDA0002397140350000056
Figure FDA0002397140350000057
分别为k时刻和k-1时刻机体坐标系下的加速度计随机游走；
Figure FDA0002397140350000058
分别为k时刻和k-1时刻机体坐标系下的陀螺仪随机游走，[γ]xyz表示取四元数γ的x、y、z分量；
(c)使用高斯-牛顿法对优化函数进行迭代求解，当达到误差收敛或者设定的最大迭代次数时停止迭代，求得载体导航信息和相机内参。
5.根据权利要求4所述基于相机内参在线标定的视觉/惯性组合导航方法，其特征在于，在步骤(c)中，所述设定的最大迭代次数为30。
CN202010135460.0A 2020-03-02 2020-03-02 一种基于相机内参在线标定的视觉/惯性组合导航方法 Active CN111156997B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010135460.0A CN111156997B (zh) 2020-03-02 2020-03-02 一种基于相机内参在线标定的视觉/惯性组合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010135460.0A CN111156997B (zh) 2020-03-02 2020-03-02 一种基于相机内参在线标定的视觉/惯性组合导航方法

Publications (2)

Publication Number Publication Date
CN111156997A true CN111156997A (zh) 2020-05-15
CN111156997B CN111156997B (zh) 2021-11-30

Family

ID=70566863

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010135460.0A Active CN111156997B (zh) 2020-03-02 2020-03-02 一种基于相机内参在线标定的视觉/惯性组合导航方法

Country Status (1)

Country Link
CN (1) CN111156997B (zh)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112637496A (zh) * 2020-12-21 2021-04-09 维沃移动通信有限公司 图像矫正方法及装置
CN113029134A (zh) * 2021-03-02 2021-06-25 浙江商汤科技开发有限公司 视觉惯性系统的初始化方法、装置、电子设备及存储介质
CN113159197A (zh) * 2021-04-26 2021-07-23 北京华捷艾米科技有限公司 一种纯旋转运动状态判定方法及装置
CN113188557A (zh) * 2021-04-28 2021-07-30 江苏方天电力技术有限公司 一种融合语义特征的视觉惯性组合导航方法
CN113358134A (zh) * 2021-04-20 2021-09-07 重庆知至科技有限公司 一种视觉惯性里程计系统
CN114323002A (zh) * 2021-12-27 2022-04-12 浙江大学 一种基于双目视觉、imu和uwb融合的agv定位导航方法
WO2022100189A1 (zh) * 2020-11-16 2022-05-19 浙江商汤科技开发有限公司 视觉惯性系统的参数标定方法及装置、电子设备和介质

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110316980A1 (en) * 2008-12-22 2011-12-29 Nederlandse Organisatie voor toegepastnatuurweten schappelijk Onderzoek TNO Method of estimating a motion of a multiple camera system, a multiple camera system and a computer program product
CN102622744A (zh) * 2012-01-17 2012-08-01 浙江大学 一种基于多项式投影模型的长焦相机标定方法
CN102663763A (zh) * 2012-04-25 2012-09-12 南京航空航天大学 一种面向大视场高精度视觉测量的摄像机标定方法
CN103886595A (zh) * 2014-03-19 2014-06-25 浙江大学 一种基于广义统一模型的折反射相机自标定方法
WO2014179745A1 (en) * 2013-05-02 2014-11-06 Qualcomm Incorporated Methods for facilitating computer vision application initialization
CN104392435A (zh) * 2014-11-10 2015-03-04 中科院微电子研究所昆山分所 鱼眼相机标定方法及标定装置
US20160042515A1 (en) * 2014-08-06 2016-02-11 Thomson Licensing Method and device for camera calibration
CN109781092A (zh) * 2019-01-19 2019-05-21 北京化工大学 一种危险化工事故中移动机器人定位与建图方法
CN110207693A (zh) * 2019-05-21 2019-09-06 南京航空航天大学 一种鲁棒立体视觉惯性预积分slam方法
CN110246193A (zh) * 2019-06-20 2019-09-17 南京博蓝奇智能科技有限公司 工业机器人末端相机在线标定方法
CN110322514A (zh) * 2019-06-24 2019-10-11 西北工业大学 一种基于多中心投影模型的光场相机参数估计方法
CN110345944A (zh) * 2019-05-27 2019-10-18 浙江工业大学 融合视觉特征和imu信息的机器人定位方法

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110316980A1 (en) * 2008-12-22 2011-12-29 Nederlandse Organisatie voor toegepastnatuurweten schappelijk Onderzoek TNO Method of estimating a motion of a multiple camera system, a multiple camera system and a computer program product
CN102622744A (zh) * 2012-01-17 2012-08-01 浙江大学 一种基于多项式投影模型的长焦相机标定方法
CN102663763A (zh) * 2012-04-25 2012-09-12 南京航空航天大学 一种面向大视场高精度视觉测量的摄像机标定方法
WO2014179745A1 (en) * 2013-05-02 2014-11-06 Qualcomm Incorporated Methods for facilitating computer vision application initialization
CN103886595A (zh) * 2014-03-19 2014-06-25 浙江大学 一种基于广义统一模型的折反射相机自标定方法
US20160042515A1 (en) * 2014-08-06 2016-02-11 Thomson Licensing Method and device for camera calibration
CN104392435A (zh) * 2014-11-10 2015-03-04 中科院微电子研究所昆山分所 鱼眼相机标定方法及标定装置
CN109781092A (zh) * 2019-01-19 2019-05-21 北京化工大学 一种危险化工事故中移动机器人定位与建图方法
CN110207693A (zh) * 2019-05-21 2019-09-06 南京航空航天大学 一种鲁棒立体视觉惯性预积分slam方法
CN110345944A (zh) * 2019-05-27 2019-10-18 浙江工业大学 融合视觉特征和imu信息的机器人定位方法
CN110246193A (zh) * 2019-06-20 2019-09-17 南京博蓝奇智能科技有限公司 工业机器人末端相机在线标定方法
CN110322514A (zh) * 2019-06-24 2019-10-11 西北工业大学 一种基于多中心投影模型的光场相机参数估计方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
K. SENGUPTA 等: "A new camera projection model and its application in reprojection", 《PROCEEDINGS. FOURTEENTH INTERNATIONAL CONFERENCE ON PATTERN RECOGNITION》 *
潘林豪 等: "单目相机-IMU外参自动标定与在线估计的视觉-惯导SLAM", 《仪器仪表学报》 *

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2022100189A1 (zh) * 2020-11-16 2022-05-19 浙江商汤科技开发有限公司 视觉惯性系统的参数标定方法及装置、电子设备和介质
CN114791291A (zh) * 2020-11-16 2022-07-26 浙江商汤科技开发有限公司 视觉惯性系统的参数标定方法及装置、电子设备和介质
CN112637496A (zh) * 2020-12-21 2021-04-09 维沃移动通信有限公司 图像矫正方法及装置
CN112637496B (zh) * 2020-12-21 2022-05-31 维沃移动通信有限公司 图像矫正方法及装置
CN113029134A (zh) * 2021-03-02 2021-06-25 浙江商汤科技开发有限公司 视觉惯性系统的初始化方法、装置、电子设备及存储介质
CN113358134A (zh) * 2021-04-20 2021-09-07 重庆知至科技有限公司 一种视觉惯性里程计系统
CN113159197A (zh) * 2021-04-26 2021-07-23 北京华捷艾米科技有限公司 一种纯旋转运动状态判定方法及装置
CN113188557A (zh) * 2021-04-28 2021-07-30 江苏方天电力技术有限公司 一种融合语义特征的视觉惯性组合导航方法
CN113188557B (zh) * 2021-04-28 2023-10-20 江苏方天电力技术有限公司 一种融合语义特征的视觉惯性组合导航方法
CN114323002A (zh) * 2021-12-27 2022-04-12 浙江大学 一种基于双目视觉、imu和uwb融合的agv定位导航方法
CN114323002B (zh) * 2021-12-27 2023-12-08 浙江大学 一种基于双目视觉、imu和uwb融合的agv定位导航方法

Also Published As

Publication number Publication date
CN111156997B (zh) 2021-11-30

Similar Documents

Publication Publication Date Title
CN111156997B (zh) 一种基于相机内参在线标定的视觉/惯性组合导航方法
CN110009681B (zh) 一种基于imu辅助的单目视觉里程计位姿处理方法
CN109307508B (zh) 一种基于多关键帧的全景惯导slam方法
CN109540126B (zh) 一种基于光流法的惯性视觉组合导航方法
CN109029433B (zh) 一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法
CN106679648B (zh) 一种基于遗传算法的视觉惯性组合的slam方法
CN107941217B (zh) 一种机器人定位方法、电子设备、存储介质、装置
CN109676604B (zh) 机器人曲面运动定位方法及其运动定位系统
CN111780754B (zh) 基于稀疏直接法的视觉惯性里程计位姿估计方法
CN109648558B (zh) 机器人曲面运动定位方法及其运动定位系统
CN110702107A (zh) 一种单目视觉惯性组合的定位导航方法
CN107193279A (zh) 基于单目视觉和imu信息的机器人定位与地图构建系统
CN111024066A (zh) 一种无人机视觉-惯性融合室内定位方法
CN112649016A (zh) 一种基于点线初始化的视觉惯性里程计方法
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN112734841B (zh) 一种用轮式里程计-imu和单目相机实现定位的方法
CN110986939A (zh) 一种基于imu预积分的视觉惯性里程计方法
CN111862316B (zh) 一种基于优化的imu紧耦合稠密直接rgbd的三维重建方法
CN111238535B (zh) 一种基于因子图的imu误差在线标定方法
CN110207693B (zh) 一种鲁棒立体视觉惯性预积分slam方法
CN111609868A (zh) 一种基于改进光流法的视觉惯性里程计方法
CN114526745A (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN115371665B (zh) 一种基于深度相机和惯性融合的移动机器人定位方法
CN114529576A (zh) 一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法
CN115272596A (zh) 一种面向单调无纹理大场景的多传感器融合slam方法

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