CN111462231A - 一种基于rgbd传感器和imu传感器的定位方法 - Google Patents

一种基于rgbd传感器和imu传感器的定位方法 Download PDF

Info

Publication number
CN111462231A
CN111462231A CN202010166801.0A CN202010166801A CN111462231A CN 111462231 A CN111462231 A CN 111462231A CN 202010166801 A CN202010166801 A CN 202010166801A CN 111462231 A CN111462231 A CN 111462231A
Authority
CN
China
Prior art keywords
sensor
coordinate system
imu
imu sensor
representing
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
CN202010166801.0A
Other languages
English (en)
Other versions
CN111462231B (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.)
South China University of Technology SCUT
Guangzhou Institute of Modern Industrial Technology
Original Assignee
South China University of Technology SCUT
Guangzhou Institute of Modern Industrial Technology
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 South China University of Technology SCUT, Guangzhou Institute of Modern Industrial Technology filed Critical South China University of Technology SCUT
Priority to CN202010166801.0A priority Critical patent/CN111462231B/zh
Publication of CN111462231A publication Critical patent/CN111462231A/zh
Priority to PCT/CN2021/080007 priority patent/WO2021180128A1/zh
Priority to GB2116617.8A priority patent/GB2597632A/en
Application granted granted Critical
Publication of CN111462231B publication Critical patent/CN111462231B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
    • 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
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G05D1/0248—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means in combination with a laser
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/027—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising intertial navigation means, e.g. azimuth detector
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/50—Depth or shape recovery
    • G06T7/55—Depth or shape recovery from multiple images
    • G06T7/579—Depth or shape recovery from multiple images from motion
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10016—Video; Image sequence
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10024—Color image
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Multimedia (AREA)
  • Electromagnetism (AREA)
  • Optics & Photonics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明公开了一种基于RGBD传感器和IMU传感器的定位方法，过程如下：首先，移动机器人搭载RGBD传感器和IMU传感器采集数据；其次，采用视觉SLAM算法对RGBD传感器数据进行初始化获得其位姿信息，同时对IMU传感器数据进行预积分，然后对移动机器人初始化获取其初始状态信息；基于重投影误差优化跟踪移动机器人的位姿信息、速度和IMU传感器偏置；同时，采用基于滑动窗口的非线性优化算法，实时优化位姿信息和地图；加入回环检测，避免零偏情况的出现。该定位方法综合运用RGBD传感器和IMU传感器，克服纯视觉SLAM算法在纯旋转以及背景单一时容易跟踪失败和IMU传感器在长时间运动中产生零偏现象的缺点。

Description

一种基于RGBD传感器和IMU传感器的定位方法
技术领域
本发明涉及视觉SLAM以及传感器融合技术领域，具体涉及一种基于RGBD传感器和IMU传感器的定位方法。
背景技术
同时定位与地图构建(Simultaneous Localization and Mapping)简称SLAM，最先是由SmithSelf和Cheeseman在1988年提出来的，被认为是实现真正全自主移动机器人的关键，SLAM问题可以描述为：机器人在未知环境中从一个未知位置开始移动，在移动过程中根据位置估计和传感器数据进行自身定位，同时建造增量式地图。只通过相机获取环境信息的SLAM被称为视觉SLAM，由于相机具有价格低、功耗低、重量轻、体积小、图像信息丰富等特点，所以视觉SLAM受到了国内外广大学者和专家的研究。
IMU(Inertial Measurement unit)为惯性测量单元，一个IMU传感器内会装有三轴的陀螺仪和三个方向的加速度计，来测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。IMU传感器常用在需要进行运动控制的设备，例如汽车和机器人上。
视觉SLAM算法虽然可以通过相机传感器数据估计自己在三维空间中的位姿信息，但在快速运动或纯旋转时容易造成特征点跟踪丢失，同时视觉SLAM一般采取第一帧作为世界坐标系，估计出来的位姿信息是相对于第一帧图像的位姿信息，而不是如东北天这样的世界坐标系，而IMU传感器可以提供各个时刻的位姿信息，但在长时间运动中存在零漂问题，导致估计的位姿信息产生偏差，而视觉SLAM中的回环能很好地解决这一问题。因此视觉SLAM和IMU传感器的融合能更好的估计机器人在未知空间中的位姿信息。
发明内容
本发明的目的是为了解决现有技术中的上述问题，提供一种基于RGBD传感器和IMU传感器的定位方法。
本发明的目的可以通过采取如下技术方案达到：
一种基于RGBD传感器和IMU传感器的定位方法，应用于搭载RGBD传感器和IMU传感器的移动机器人，实现对移动机器人的定位，所述的定位方法包括以下步骤：
S1、传感器数据采集：移动机器人搭载RGBD传感器和IMU传感器在三维空间中自由移动，采集RGBD传感器数据和IMU传感器数据；
S2、IMU预积分：采用视觉SLAM算法根据RGBD传感器不同帧的图像数据质量选定关键帧，基于选定的关键帧，采用IMU预积分方程计算关键帧之间的IMU传感器数据的预积分值；
S3、移动机器人初始化：采用视觉SLAM算法对RGBD传感器初始化得到RGBD传感器的位姿信息，然后根据RGBD传感器的位姿信息和对应关键帧的IMU预积分对移动机器人初始化获取移动机器人的初始化状态信息，其中，所述的移动机器人的状态信息包括移动机器人的位姿信息、速度和IMU传感器的偏置；
S4、移动机器人状态的跟踪优化：采用基于重投影误差的优化算法对估计的移动机器人的状态信息进行优化跟踪，然后采用基于滑动窗口的优化算法对移动机器人的状态进行非线性优化；
S5、回环检测：根据RGBD传感器数据，采用基于词袋模型的回环优化算法对RGBD传感器的位姿信息和特征点进行回环优化，然后采用基于滑动窗口的优化算法对IMU传感器误差项进行优化。
进一步地，所述的IMU传感器的采集频率大于所述的RGBD传感器采集频率，IMU传感器的采样频率为100Hz以上，RGBD传感器采样频率为30Hz左右，由于IMU传感器的采样频率远大于RGBD传感器采样频率，因此需要通过IMU预积分对齐数据。
进一步地，所述的步骤S2中所述的步骤S2中采用视觉SLAM算法选定关键帧的规则如下：根据RGBD传感器所采集的当前图像帧与上一关键帧之间的帧数间隔，以及当前图像帧与之前的关键帧共视特征点的数量进行选定。
进一步地，所述的步骤S2中计算IMU预积分，IMU传感器测量得到加速度和角速度：
Figure BDA0002407729030000031
其中ω(t)和a(t)分别表示角速度和加速度的真实值，
Figure BDA0002407729030000032
和
Figure BDA0002407729030000033
分别表示角速度和加速度的IMU传感器测量值，RWB表示IMU传感器坐标系到世界坐标系的旋转矩阵，bg(t)和ba(t)表示随时间变化的IMU传感器陀螺仪偏置和加速度计偏置，ηg(t)和ηa(t)表示随时间变化的IMU传感器的陀螺仪和加速度计高斯白噪声，gw表示在世界坐标下的重力向量；
对IMU传感器测量值进行积分，并根据采样频率将其离散化，可以得到：
Figure BDA0002407729030000041
其中
Figure BDA0002407729030000042
和
Figure BDA0002407729030000043
表示k时刻的IMU传感器陀螺仪偏置和加速度计偏置，
Figure BDA0002407729030000044
和
Figure BDA0002407729030000045
表示离散后的IMU传感器的陀螺仪和加速度计高斯白噪声，ΔRik表示i时刻到k时刻的旋转矩阵，ak和ωk分别表示IMU传感器测量的加速度和角速度离散值，pi、vi、
Figure BDA0002407729030000046
分别表示在i时刻IMU传感器离散后的位移、速度和旋转矩阵，gw表示在世界坐标下的重力向量，Δt表示离散的时刻差。
进一步地，为了避免上一时刻数据改变，导致下一时刻需要重新推导的问题，计算出两个时刻间的预积分，并分离出IMU传感器加速度计偏置和陀螺仪偏置，通过IMU预积分可以根据当前时刻的状态，推算出下一时刻的状态：
Figure BDA0002407729030000047
其中，
Figure BDA0002407729030000048
表示IMU传感器的坐标系B在i+1时刻旋转到世界坐标系的旋转矩阵，vi+1、pi+1分别表示IMU传感器坐标系B在i+1时刻在世界坐标系下的速度和位姿信息，Δti,i+1表示IMU传感器i时刻到i+1时刻的间隔，
Figure BDA0002407729030000049
分别表示在i时刻IMU传感器陀螺仪偏置和加速度计偏置，gw表示在世界坐标系下的重力加速度，ΔRi,i+1、Δvi,i+1、Δpi,i+1分别表示i时刻到i+1时刻IMU传感器的旋转矩阵、速度变换和位移变换的预积分，
Figure BDA0002407729030000051
分别表示ΔR、Δv、Δp随IMU传感器陀螺仪偏置
Figure BDA0002407729030000052
变化方程的一阶近似的雅克比矩阵，
Figure BDA0002407729030000053
分别为Δv、Δp随IMU传感器加速度计偏置
Figure BDA0002407729030000054
变化方程的一阶近似的雅克比矩阵。
进一步地，需要对RGBD传感器的位姿信息和IMU传感器的偏置、速度、重力向量、速度进行初始化，所述的步骤S3包括：
S31、视觉SLAM算法对RGBD传感器进行初始化，获取RGBD传感器的位姿信息，过程如下：
S311、采用ORB特征提取算法，提取RGBD传感器的图像数据的ORB特征点，根据ORB特征点对应的RGBD传感器的深度数据找出对应的三维空间点，从而根据ORB特征点像素坐标和空间点三维坐标获得RGBD传感器的位姿信息；
S312、对于某空间点Xk＝[Uk,Vk,Wk]T，其在RGBD传感器坐标系下的像素坐标为ui＝[xk,yk]，空间点坐标与像素坐标对应关系为：
suk＝Kexp(ξ^)Xk
其中s表示深度图的尺度因子，K表示RGBD传感器的内参矩阵，ξ表示估计位姿信息，ξ^表示位姿信息所对应的反对称矩阵，k表示特征点的序号；
S313、通过构建最小二乘问题，优化RGBD传感器的位姿信息，使其误差最小：
Figure BDA0002407729030000055
其中ξ*表示优化后的最优位姿信息，ξ表示估计的位姿信息，n表示特征点的个数；
S32、根据视觉SLAM算法选定的N个连续关键帧，采用IMU预积分方程对N个连续关键帧之间的IMU传感器陀螺仪的数据进行积分，得到IMU传感器在N个关键帧之间的旋转变换，通过建立N个关键帧之间的RGBD传感器和IMU传感器旋转变换的最小误差方程，对IMU传感器陀螺仪的偏置bg进行估计：
Figure BDA0002407729030000061
其中N表示关键帧的个数，
Figure BDA0002407729030000062
是关键帧i由视觉SLAM算法得到的RGBD传感器坐标系相对于世界坐标系的旋转，
Figure BDA0002407729030000063
表示关键帧i+1世界坐标系转换到IMU坐标系的旋转矩阵，RCB为标定的IMU传感器转换到RGBD传感器坐标系下的标定旋转矩阵，ΔRi,i+1为连续两关键帧间的IMU传感器积分所得的旋转矩阵，
Figure BDA0002407729030000064
表示ΔR随IMU传感器陀螺仪偏置变化方程的一阶近似的雅克比矩阵；
S33、利用上述IMU传感器的陀螺仪偏置bg估计的结果，对IMU传感器的加速度计偏置、重力向量进行估计：
S331、引入重力常数为G，IMU传感器坐标系下重力向量
Figure BDA0002407729030000065
Figure BDA0002407729030000066
和IMU传感器中测量的重力向量
Figure BDA0002407729030000067
通过
Figure BDA0002407729030000068
进行标准化，可以计算出旋转矩阵RWI：
Figure BDA0002407729030000069
其中RWI表示IMU传感器在世界坐标系的方向；
Figure BDA00024077290300000610
其中
Figure BDA00024077290300000611
表示IMU传感器的重力和IMU测量的重力向量的旋转轴，θ表示其旋转角度；
IMU传感器重力向量可以表示为：
Figure BDA00024077290300000612
加入重力扰动向量δθ：
Figure BDA0002407729030000071
其中δθ＝[δθxy 0]T,δθxy＝[δθx δθy]T
对gW进行一阶近似：
Figure BDA0002407729030000072
其中，
Figure BDA0002407729030000073
表示重力向量
Figure BDA0002407729030000074
的一阶近似；
根据RGBD传感器坐标系C和IMU传感器坐标系B之间的变换：
Figure BDA0002407729030000075
根据IMU传感器预积分计算方程可以联立下述方程：
Figure BDA0002407729030000076
其中
Figure BDA0002407729030000077
表示Δp随IMU传感器陀螺仪偏置变化方程的一阶近似的雅克比矩阵，
Figure BDA0002407729030000078
表示IMU传感器坐标系中心在RGBD传感器坐标系下的坐标，
Figure BDA0002407729030000079
分别表示在第i关键帧时RGBD传感器坐标系中心在世界坐标系下的坐标，vi表示在第i关键帧时IMU传感器的速度，
Figure BDA00024077290300000710
表示在第i关键帧时RGBD坐标系相对于世界坐标的旋转矩阵，
Figure BDA00024077290300000711
分别表示在第i关键帧时IMU传感器坐标系相对于世界坐标系的旋转矩阵，Δti,i+1表示IMU传感器第i关键帧到第i+1关键帧时间的间隔。
根据三个连续关键帧之间的IMU预积分方程的递推关系，以及IMU传感器坐标系和RGBD传感器坐标系的转换方程，可以得到线性方程：
Figure BDA00024077290300000712
其中
Figure BDA00024077290300000713
Figure BDA0002407729030000081
其中δθxy为
Figure BDA0002407729030000082
中的二阶向量，δθ为重力偏差向量，ba表示初始化的IMU传感器加速度计偏置，RWI表示IMU传感器在世界坐标系的方向，Δt12、Δt23分别表示关键帧1、2到关键帧2、3的IMU传感器的时间间隔，G表示重力参考值，
Figure BDA0002407729030000087
表示在IMU传感器坐标系下的重力向量，[](:,1:2)表示只包含该矩阵前两列数据，
Figure BDA0002407729030000088
分别表示关键帧1、2时刻IMU传感器坐标系相对于世界坐标系的旋转矩阵，Δp12、Δp23分别表示关键帧1、2到关键帧2、3的IMU传感器的位移，Δv23表示关键帧2到关键帧3的速度变化，
Figure BDA0002407729030000089
分别为Δv23、Δp23、Δp12随IMU传感器加速度计偏置变化方程的一阶近似的雅克比矩阵，
Figure BDA00024077290300000810
分别表示关键帧1、2、3时刻的RGBD传感器坐标系C相对于世界坐标系的旋转矩阵，
Figure BDA00024077290300000811
分别表示关键帧1、2、3时刻RGBD传感器坐标系中心在世界坐标系下的坐标，
Figure BDA00024077290300000812
表示IMU传感器坐标系中心在RGBD传感器坐标系下的坐标；
S332、通过奇异值分解求解出重力偏差二阶向量δθxy和加速度计偏置ba；
S333、通过方程：
Figure BDA0002407729030000083
计算出IMU传感器的速度v作为初始化的IMU传感器速度，其中
Figure BDA0002407729030000084
表示Δp随IMU传感器陀螺仪偏置变化方程的一阶近似的雅克比矩阵，
Figure BDA0002407729030000085
表示IMU传感器坐标系中心在RGBD传感器坐标系下的坐标，
Figure BDA0002407729030000086
分别表示在第i关键帧时RGBD传感器坐标系中心在世界坐标系下的坐标，vi表示在第i关键帧时IMU传感器的速度，
Figure BDA0002407729030000091
表示在第i关键帧时RGBD坐标系相对于世界坐标的旋转矩阵，
Figure BDA0002407729030000092
分别表示在第i关键帧时IMU传感器坐标系相对于世界坐标系的旋转矩阵，Δti,i+1表示IMU传感器第i关键帧到第i+1关键帧时间的间隔。
进一步地，移动机器人的状态为移动机器人的位姿信息、速度和IMU传感器的偏置，所述的步骤S4过程如下：
S41、采用视觉SLAM算法估计RGBD传感器的位姿信息ξ；
S42、建立关键帧之间的约束关系，根据IMU传感器误差项和移动机器人的位姿信息，基于重投影误差建立最小二乘关系：
Eproj(k,j)为位姿信息和特征点的重投影误差，表述式如下：
Figure BDA0002407729030000093
EIMU(i,j)为IMU传感器的误差项，表述式如下：
Figure BDA0002407729030000094
其中k表示图像的特征点序号，i表示关键帧序号，j表示当前关键帧序号，s表示深度图的尺度因子，uk表示第k个特征点对应的RGBD传感器相机坐标下的二维坐标，K表示RGBD传感器内参矩阵，ξ^表示RGBD传感器位姿信息ξ的反对称矩阵，
Figure BDA0002407729030000095
表示第k个特征点Xk在RGBD传感器坐标系C下的坐标，Σk是特征点Xk和对应像素点的尺度信息矩阵，RCB为标定的IMU传感器转换到RGBD传感器坐标系下的标定旋转矩阵，
Figure BDA0002407729030000096
表示在第j关键帧时刻世界坐标系相对于IMU传感器坐标系的旋转矩阵，
Figure BDA0002407729030000097
表示第k个特征点Xk在世界坐标系W下的坐标，
Figure BDA0002407729030000101
表示第j关键帧时刻IMU传感器在世界坐标系下的坐标，
Figure BDA0002407729030000102
表示IMU传感器坐标系中心在RGBD传感器坐标系下的坐标，ΔRi,j、Δvi,j、Δpi,j分别表示关键帧i到关键帧j之间的IMU传感器的旋转矩阵、速度变换和位移变换的预积分，
Figure BDA0002407729030000103
Figure BDA0002407729030000104
分别表示ΔR、Δv、Δp随IMU传感器陀螺仪偏置变化方程的一阶近似的雅克比矩阵，
Figure BDA0002407729030000105
分别为Δv、Δp随IMU传感器加速度计偏置变化方程的一阶近似的雅克比矩阵，
Figure BDA0002407729030000106
表示IMU传感器在第j关键帧时刻的速度，
Figure BDA0002407729030000107
表示IMU传感器的坐标系B在关键帧j时旋转到世界坐标系的旋转矩阵，
Figure BDA0002407729030000108
分别表示在第j关键帧时刻IMU传感器陀螺仪偏置和加速度计偏置，ΣR、ΣI分别为IMU传感器偏置随即游走和预积分的信息矩阵，bi、bj分别表示第i和第j关键帧的IMU传感器偏置，Δti,j表示第i关键帧到第j关键帧的时间差；
S43、对于当前关键帧j的移动机器人状态跟踪，仅与相邻关键帧j-1建立约束关系：
Figure BDA0002407729030000109
其中φ表示需要优化对象的集合，φ*表示优化后的φ对象集合，k表示特征点的序号，j表示当前关键帧序号，
Figure BDA00024077290300001010
表示IMU传感器的坐标系B在关键帧j时刻旋转到世界坐标系的旋转矩阵，
Figure BDA00024077290300001011
表示IMU传感器关键帧j时刻在世界坐标系下的平移，
Figure BDA00024077290300001012
表示IMU传感器在第j关键帧时刻的速度，
Figure BDA00024077290300001013
分别表示在关键帧j时刻IMU传感器陀螺仪偏置和加速度计偏置；
求解出当前关键帧的移动机器人位姿信息和IMU传感器速度、偏置的最优值作为移动机器人的状态跟踪；
S44、由于RGBD传感器和IMU传感器需要优化9自由度的状态，如果对于所有共视关键帧都对所有状态量进行优化，则运算量过大，无法实现实时的定位，并且IMU传感器存在零偏的情况，距离当前时刻的数据时间越长，则越不可靠。
因此采用基于滑动窗口的优化算法对移动机器人的状态进行非线性优化，对于当前关键帧之前的M帧关键帧，通过对移动机器人的位姿信息和IMU传感器误差项建立约束关系进行优化：
Figure BDA0002407729030000111
其中φ表示需要优化对象的集合，φ*表示优化后的φ对象集合，k表示特征点的序号，i表示关键帧序号，j表示当前关键帧序号，
Figure BDA0002407729030000112
表示IMU传感器的坐标系B在关键帧j时刻旋转到世界坐标系的旋转矩阵，
Figure BDA0002407729030000113
表示IMU传感器关键帧j时刻在世界坐标系下的平移，
Figure BDA0002407729030000114
表示IMU传感器在第j关键帧时刻的速度，
Figure BDA0002407729030000115
分别表示在关键帧j时刻IMU传感器陀螺仪偏置和加速度计偏置；
对于当前关键帧M+1关键帧前的关键帧，仅对与当前关键帧具有共视点的关键帧位姿信息和特征点建立约束关系进行优化：
Figure BDA0002407729030000116
其中ψ表示需要优化对象的集合，ψ*表示优化后的ψ对象集合。
进一步地，所述的步骤S5过程如下：
S51、采用基于词袋模型的回环检测算法对当前关键帧的RGBD传感器采集图像进行判断是否有回环；
S52、如果未检测到回环，那么输出当前关键帧的移动机器人的位姿，结束回环优化步骤，然后对后续的移动机器人的状态进行跟踪；如果检测到回环，那么基于视觉SLAM算法对RGBD传感器位姿信息和特征点进行回环优化；
S53、基于滑动窗口的非线性优化算法对移动机器人位姿信息、特征点和IMU传感器误差项进行优化，输出当前关键帧的移动机器人的位姿。
本发明相对于现有技术具有如下的优点及效果：
本发明公开了一种基于RGBD传感器和IMU传感器的定位方法，首先通过RGBD传感器和IMU传感器获取移动机器人相关数据，然后对RGBD传感器进行初始化和对IMU传感器进行预积分，进行移动机器人初始化，初始化后对移动机器人位姿信息进行跟踪和后端优化，在产生回环时进行回环处理，实现对移动机器人的定位。该定位方法综合运动RGBD传感器和IMU传感器，克服了纯视觉SLAM算法在旋转以及背景单一时容易跟踪失败和IMU传感器在长时间运动中产生零偏现象的缺点。
附图说明
图1是本发明公开的一种基于RGBD传感器和IMU传感器的定位方法的流程图；
图2是本发明采用的滑动窗口算法的示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
实施例
本实施例公开了一种基于RGBD传感器和IMU传感器的定位方法的具体实施过程，步骤如下；
T1、RGBD传感器采用Kinect v2传感器，将Kinect v2传感器和IMU传感器固定到turtlebot2移动机器人上，通过USB接口将Kinect v2传感器、IMU传感器和turtlebot2移动机器人接入移动笔记本电脑中，控制turtlebot2移动机器人在三维空间中进行移动，采集Kinect v2传感器数据和IMU传感器数据，IMU传感器的采集频率大于Kinect v2传感器采集频率。
T2、通过ORBSLAM2算法选定关键帧，根据RGBD传感器所采集的当前图像帧与上一关键帧的帧数间隔，以及当前图像帧与之前的关键帧共视特征点的数量进行选定；
根据IMU传感器可以测量出加速度和角速度，通过IMU预积分由当前时刻的状态推算出下一时刻的状态，计算公式如下：
Figure BDA0002407729030000131
其中，
Figure BDA0002407729030000132
表示IMU传感器的坐标系B在i+1时刻旋转到世界坐标系的旋转矩阵，vi+1、pi+1分别表示IMU传感器坐标系B在i+1时刻在世界坐标系下的速度和位姿信息，Δti,i+1表示IMU传感器i时刻到i+1时刻的间隔，
Figure BDA0002407729030000133
分别表示在i时刻IMU传感器陀螺仪偏置和加速度计偏置，gw表示在世界坐标系下的重力加速度，ΔRi,i+1、Δvi,i+1、Δpi,i+1分别表示i时刻到i+1时刻IMU传感器的旋转矩阵、速度变换和位移变换的预积分，
Figure BDA0002407729030000134
分别表示ΔR、Δv、Δp随IMU传感器陀螺仪偏置
Figure BDA0002407729030000135
变化方程的一阶近似的雅克比矩阵，
Figure BDA0002407729030000136
分别为Δv、Δp随IMU传感器加速度计偏置
Figure BDA0002407729030000137
变化方程的一阶近似的雅克比矩阵。
T3、对RGBD传感器的位姿信息和IMU传感器的偏置、速度、重力向量、速度进行初始化：
T31、视觉ORBSLAM2算法对RGBD传感器进行初始化，获取RGBD传感器的位姿信息：
T311、采用ORB特征提取算法，提取RGBD传感器的图像数据的ORB特征点，根据ORB特征点对应的RGBD传感器的深度数据找出对应的三维空间点，从而根据ORB特征点像素坐标和空间点三维坐标获得RGBD传感器的位姿信息；
T 312、对于某空间点Xk＝[Uk,Vk,Wk]T，其在RGBD传感器坐标系下的像素坐标为uk＝[xk,yk]，空间点坐标与像素坐标对应关系为：
suk＝Kexp(ξ^)Xk
其中s表示深度图的尺度因子，K表示RGBD传感器的内参矩阵，ξ表示估计位姿信息，ξ^表示位姿信息所对应的反对称矩阵，k表示特征点的序号；
T 313、通过构建最小二乘问题，优化RGBD传感器的位姿信息，使其误差最小：
Figure BDA0002407729030000141
其中ξ表示优化后的最优位姿信息，ξ表示估计的位姿信息，n表示特征点的个数；
T32、根据视觉SLAM算法选定的N个连续关键帧，采用IMU预积分方程对N个连续关键帧之间的IMU传感器陀螺仪的数据进行积分，得到IMU传感器在N个关键帧之间的旋转变换，通过建立N个关键帧之间的RGBD传感器和IMU传感器旋转变换的最小误差方程，对IMU传感器陀螺仪的偏置bg进行估计：
Figure BDA0002407729030000151
其中N表示关键帧的个数，ΔRi,i+1为连续关键帧i和i+1间的IMU传感器积分所得的旋转矩阵，
Figure BDA0002407729030000152
表示ΔR随IMU传感器陀螺仪偏置变化方程的一阶近似的雅克比矩阵，
Figure BDA0002407729030000153
是关键帧i由视觉ORBSLAM2算法得到的RGBD传感器坐标系相对于世界坐标系的旋转，
Figure BDA0002407729030000154
表示关键帧i+1世界坐标系转换到IMU坐标系的旋转矩阵，RCB为标定的IMU传感器转换到RGBD传感器坐标系下的标定旋转矩阵；
本实施例中通过两个关键帧对IMU传感器的陀螺仪偏置进行估计，因此N取2；
T33、利用上述IMU传感器的陀螺仪偏置bg估计的结果，对IMU传感器的加速度计偏置、重力向量进行估计：
T331、根据三个连续关键帧之间的IMU预积分方程的递推关系，以及IMU传感器坐标系和RGBD传感器坐标系的转换方程，可以得到线性方程：
Figure BDA0002407729030000155
其中
Figure BDA0002407729030000156
其中δθxy为
Figure BDA0002407729030000157
中的二阶向量，δθ为重力偏差向量，ba表示初始化的IMU传感器加速度计偏置，RWI表示IMU传感器在世界坐标系的方向，Δt12、Δt23分别表示关键帧1、2到关键帧2、3的IMU传感器的时间间隔，G表示重力参考值，
Figure BDA0002407729030000161
表示在IMU传感器坐标系下的重力向量，
Figure BDA0002407729030000162
表示重力向量
Figure BDA0002407729030000163
的一阶近似，[](:,1:2)表示只包含该矩阵前两列数据，
Figure BDA0002407729030000164
分别表示关键帧1、2时刻IMU传感器坐标系相对于世界坐标系的旋转矩阵，Δp12、Δp23分别表示关键帧1、2到关键帧2、3的IMU传感器的位移，Δv23表示关键帧2到关键帧3的速度变化，
Figure BDA0002407729030000165
Figure BDA0002407729030000166
分别为Δv23、Δp23、Δp12随IMU传感器加速度计偏置变化方程的一阶近似的雅克比矩阵，
Figure BDA0002407729030000167
分别表示关键帧1、2、3时刻的RGBD传感器坐标系C相对于世界坐标系的旋转矩阵，
Figure BDA0002407729030000168
分别表示关键帧1、2、3时刻RGBD传感器坐标系中心在世界坐标系下的坐标，
Figure BDA0002407729030000169
表示IMU传感器坐标系中心在RGBD传感器坐标系下的坐标；
T332、通过奇异值分解求解出重力偏差二阶向量δθxy和加速度计偏置ba；
T333、通过方程：
Figure BDA00024077290300001610
其中
Figure BDA00024077290300001611
表示Δp随IMU传感器陀螺仪偏置
Figure BDA00024077290300001612
变化方程的一阶近似的雅克比矩阵，
Figure BDA00024077290300001613
表示IMU传感器坐标系中心在RGBD传感器坐标系下的坐标，
Figure BDA00024077290300001614
分别表示关键帧i时RGBD传感器坐标系中心在世界坐标系下的坐标，vi表示关键帧i时IMU传感器的速度，
Figure BDA00024077290300001615
表示在i关键帧RGBD坐标系相对于世界坐标的旋转矩阵，
Figure BDA00024077290300001616
分别表示关键帧i时IMU传感器坐标系相对于世界坐标系的旋转矩阵；
计算出IMU传感器的速度v作为初始化的IMU传感器速度。
T4、跟踪移动机器人的状态，并采用基于滑动窗口的非线性优化对移动机器人状态优化，移动机器人的状态为移动机器人的位姿信息、速度和IMU传感器的偏置：
T41、采用视觉ORBSLAM2算法估计RGBD传感器的位姿信息ξ；
T42、建立关键帧之间的约束关系，根据IMU传感器误差项和移动机器人的位姿信息，基于重投影误差建立最小二乘关系：
Eproj(k,j)为位姿信息和特征点的重投影误差，表述式如下：
Figure BDA0002407729030000171
EIMU(i,j)为IMU传感器的误差项，表述式如下：
Figure BDA0002407729030000172
其中k表示图像的特征点序号，i表示关键帧序号，j表示当前关键帧序号，s表示深度图的尺度因子，uk表示第k个特征点对应的RGBD传感器相机坐标下的二维坐标，K表示RGBD传感器内参矩阵，ξ^表示RGBD传感器位姿信息ξ的反对称矩阵，
Figure BDA0002407729030000173
表示第k个特征点Xk在RGBD传感器坐标系C下的坐标，Σk是特征点Xk和对应像素点的尺度信息矩阵，RCB为标定的IMU传感器转换到RGBD传感器坐标系下的标定旋转矩阵，
Figure BDA0002407729030000174
表示在第j关键帧时刻世界坐标系相对于IMU传感器坐标系的旋转矩阵，
Figure BDA0002407729030000175
表示第k个特征点Xk在世界坐标系W下的坐标，
Figure BDA0002407729030000176
表示第j关键帧时刻IMU传感器在世界坐标系下的坐标，
Figure BDA0002407729030000177
表示IMU传感器坐标系中心在RGBD传感器坐标系下的坐标，ΔRi,j、Δvi,j、Δpi,j分别表示关键帧i到关键帧j之间的IMU传感器的旋转矩阵、速度变换和位移变换的预积分，
Figure BDA0002407729030000178
Figure BDA0002407729030000181
分别表示ΔR、Δv、Δp随IMU传感器陀螺仪偏置变化方程的一阶近似的雅克比矩阵，
Figure BDA0002407729030000182
分别为Δv、Δp随IMU传感器加速度计偏置变化方程的一阶近似的雅克比矩阵，
Figure BDA0002407729030000183
表示IMU传感器在第j关键帧时刻的速度，
Figure BDA0002407729030000184
表示IMU传感器的坐标系B在关键帧j时旋转到世界坐标系的旋转矩阵，
Figure BDA0002407729030000185
分别表示在第j关键帧时刻IMU传感器陀螺仪偏置和加速度计偏置，ΣR、ΣI分别为IMU传感器偏置随即游走和预积分的信息矩阵，bi、bj分别表示第i和第j关键帧的IMU传感器偏置，Δti,j表示第i关键帧到第j关键帧的时间差；
T43、对于当前关键帧j的移动机器人状态跟踪，仅与相邻关键帧j-1建立约束关系：
Figure BDA0002407729030000186
其中φ表示需要优化对象的集合，φ*表示优化后的φ对象集合，k表示特征点的序号，j表示当前关键帧序号，
Figure BDA0002407729030000187
表示IMU传感器的坐标系B在关键帧j时刻旋转到世界坐标系的旋转矩阵，
Figure BDA0002407729030000188
表示IMU传感器关键帧j时刻在世界坐标系下的平移，
Figure BDA0002407729030000189
表示IMU传感器在第j关键帧时刻的速度，
Figure BDA00024077290300001810
分别表示在关键帧j时刻IMU传感器陀螺仪偏置和加速度计偏置；
求解出当前关键帧的移动机器人位姿信息和IMU传感器速度、偏置的最优值作为移动机器人的状态跟踪；
T44、采用基于滑动窗口的优化方式对移动机器人的状态进行非线性优化，对于当前关键帧之前的M帧关键帧，通过对移动机器人的位姿信息和IMU传感器误差项建立约束关系进行优化：
Figure BDA0002407729030000191
其中φ表示需要优化对象的集合，φ*表示优化后的φ对象集合，k表示特征点的序号，i表示关键帧序号，j表示当前关键帧序号，
Figure BDA0002407729030000192
表示IMU传感器的坐标系B在关键帧j时刻旋转到世界坐标系的旋转矩阵，
Figure BDA0002407729030000193
表示IMU传感器关键帧j时刻在世界坐标系下的平移，
Figure BDA0002407729030000194
表示IMU传感器在第j关键帧时刻的速度，
Figure BDA0002407729030000195
分别表示在关键帧j时刻IMU传感器陀螺仪偏置和加速度计偏置；
对于当前关键帧M+1关键帧前的关键帧，仅对与当前关键帧具有共视点的关键帧位姿信息和特征点建立约束关系进行优化：
Figure BDA0002407729030000196
其中ψ表示需要优化对象的集合，ψ*表示优化后的ψ对象集合。
本实施例中M大小随着当前关键帧与前关键帧的时间间隔变化，M根据距离当前关键帧5s内的关键帧数目变化。
T5、回环优化：
T51、采用基于词袋模型的回环检测算法对当前关键帧的RGBD传感器采集图像进行判断是否有回环；
T52、如果未检测到回环，输出当前关键帧的移动机器人的位姿，结束回环步骤，然后对后续的移动机器人的状态进行跟踪；如果检测到回环，那么基于视觉ORBSLAM2算法对RGBD传感器位姿信息和特征点进行回环优化；
T53、基于滑动窗口的非线性优化算法对移动机器人位姿信息、特征点和IMU传感器误差项进行优化，输出当前关键帧的移动机器人的位姿。
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (7)

1.一种基于RGBD传感器和IMU传感器的定位方法，应用于搭载RGBD传感器和IMU传感器的移动机器人，实现对移动机器人的定位，其特征在于，所述的定位方法包括以下步骤：
S1、传感器数据采集：移动机器人搭载RGBD传感器和IMU传感器在三维空间中自由移动，采集RGBD传感器数据和IMU传感器数据；
S2、IMU预积分：采用视觉SLAM算法根据RGBD传感器不同帧的图像数据质量选定关键帧，基于选定的关键帧，采用IMU预积分方程计算关键帧之间的IMU传感器数据的预积分值；
S3、移动机器人初始化：采用视觉SLAM算法对RGBD传感器初始化得到RGBD传感器的位姿信息，然后根据RGBD传感器的位姿信息和对应关键帧的IMU预积分对移动机器人初始化获取移动机器人的初始化状态信息，其中，所述的移动机器人的状态信息包括移动机器人的位姿信息、速度和IMU传感器的偏置；
S4、移动机器人状态的跟踪优化：采用基于重投影误差的优化算法对估计的移动机器人的状态信息进行优化跟踪，然后采用基于滑动窗口的优化算法对移动机器人的状态进行非线性优化；
S5、回环检测：根据RGBD传感器数据，采用基于词袋模型的回环优化算法对RGBD传感器的位姿信息和特征点进行回环优化，然后采用基于滑动窗口的优化算法对IMU传感器误差项进行优化。
2.根据权利要求1所述的一种基于RGBD传感器和IMU传感器的定位方法，其特征在于，所述的IMU传感器的采集频率大于所述的RGBD传感器采集频率。
3.根据权利要求1所述的一种基于RGBD传感器和IMU传感器的定位方法，其特征在于，所述的步骤S2中采用视觉SLAM算法选定关键帧的规则如下：根据RGBD传感器所采集的当前图像帧与上一关键帧之间的帧数间隔，以及当前图像帧与之前的关键帧共视特征点的数量进行选定。
4.根据权利要求1所述的一种基于RGBD传感器和IMU传感器的定位方法，其特征在于，所述的步骤S2中计算IMU预积分，通过IMU预积分由当前时刻的状态推算出下一时刻的状态，计算公式如下：
Figure FDA0002407729020000021
其中，
Figure FDA0002407729020000022
表示IMU传感器的坐标系B在i+1时刻旋转到世界坐标系的旋转矩阵，vi+1、pi+1分别表示IMU传感器坐标系B在i+1时刻在世界坐标系下的速度和位姿信息，Δti,i+1表示IMU传感器i时刻到i+1时刻的间隔，
Figure FDA0002407729020000023
分别表示在i时刻IMU传感器陀螺仪偏置和加速度计偏置，gw表示在世界坐标系下的重力加速度，ΔRi,i+1、Δvi,i+1、Δpi,i+1分别表示i时刻到i+1时刻IMU传感器的旋转矩阵、速度变换和位移变换的预积分，
Figure FDA0002407729020000024
分别表示ΔR、Δv、Δp随IMU传感器陀螺仪偏置
Figure FDA0002407729020000025
变化方程的一阶近似的雅克比矩阵，
Figure FDA0002407729020000026
分别为Δv、Δp随IMU传感器加速度计偏置
Figure FDA0002407729020000027
变化方程的一阶近似的雅克比矩阵。
5.根据权利要求1所述的一种基于RGBD传感器和IMU传感器的定位方法，其特征在于，所述的步骤S3包括：
S31、采用视觉SLAM算法对RGBD传感器进行初始化，获取RGBD传感器的位姿信息，过程如下：
S311、采用ORB特征提取算法，提取RGBD传感器的图像数据的ORB特征点，根据ORB特征点对应的RGBD传感器的深度数据找出对应的三维空间点，从而根据ORB特征点像素坐标和空间点三维坐标获得RGBD传感器的位姿信息；
S312、对于某空间点Xk＝[Uk,Vk,Wk]T，其在RGBD传感器坐标系下的像素坐标为uk＝[xk,yk]，空间点坐标与像素坐标对应关系为：
suk＝Kexp(ξ^)Xk
其中s表示深度图的尺度因子，K表示RGBD传感器的内参矩阵，ξ表示估计位姿信息，ξ^表示位姿信息所对应的反对称矩阵，k表示特征点的序号；
S313、通过构建最小二乘问题，优化RGBD传感器的位姿信息，使其误差最小：
Figure FDA0002407729020000031
其中ξ*表示优化后的最优位姿信息，ξ表示估计的位姿信息，n表示特征点的个数；
S32、根据视觉SLAM算法选定的N个连续关键帧，采用IMU预积分方程对N个连续关键帧之间的IMU传感器陀螺仪的数据进行积分，得到IMU传感器在N个关键帧之间的旋转变换，通过建立N个关键帧之间的RGBD传感器和IMU传感器旋转变换的最小误差方程，对IMU传感器陀螺仪的偏置bg进行估计：
Figure FDA0002407729020000032
其中N表示关键帧的个数，ΔRi,i+1为关键帧i和i+1间的IMU传感器积分所得的旋转矩阵，
Figure FDA0002407729020000033
表示ΔR随IMU传感器陀螺仪偏置变化方程的一阶近似的雅克比矩阵，
Figure FDA0002407729020000034
Figure FDA0002407729020000035
是关键帧i由视觉SLAM算法得到的RGBD传感器坐标系相对于世界坐标系的旋转，
Figure FDA0002407729020000036
表示关键帧i+1世界坐标系转换到IMU坐标系的旋转矩阵，RCB为标定的IMU传感器转换到RGBD传感器坐标系下的标定旋转矩阵；
S33、利用上述IMU传感器的陀螺仪偏置bg估计的结果，对IMU传感器的加速度计偏置、重力向量进行估计：
S331、根据三个连续关键帧之间的IMU预积分方程的递推关系，以及IMU传感器坐标系和RGBD传感器坐标系的转换方程，得到线性方程：
Figure FDA0002407729020000041
其中
Figure FDA0002407729020000042
Figure FDA0002407729020000043
Figure FDA0002407729020000044
其中δθxy为
Figure FDA0002407729020000045
中的二阶向量，δθ为重力偏差向量，ba表示初始化的IMU传感器加速度计偏置，RWI表示IMU传感器在世界坐标系的方向，Δt12、Δt23分别表示关键帧1、2到关键帧2、3的IMU传感器的时间间隔，G表示重力参考值，
Figure FDA0002407729020000046
表示在IMU传感器坐标系下的重力向量，
Figure FDA0002407729020000047
表示重力向量
Figure FDA0002407729020000048
的一阶近似，[](:,1:2)表示只包含该矩阵前两列数据，
Figure FDA0002407729020000049
分别表示关键帧1、2时刻IMU传感器坐标系相对于世界坐标系的旋转矩阵，Δp12、Δp23分别表示关键帧1、2到关键帧2、3的IMU传感器的位移，Δv23表示关键帧2到关键帧3的速度变化，
Figure FDA00024077290200000410
分别为Δv23、Δp23、Δp12随IMU传感器加速度计偏置变化方程的一阶近似的雅克比矩阵，
Figure FDA00024077290200000411
分别表示关键帧1、2、3时刻的RGBD传感器坐标系C相对于世界坐标系的旋转矩阵，
Figure FDA00024077290200000412
分别表示关键帧1、2、3时刻RGBD传感器坐标系中心在世界坐标系下的坐标，
Figure FDA0002407729020000051
表示IMU传感器坐标系中心在RGBD传感器坐标系下的坐标；
S332、通过奇异值分解求解出重力偏差二阶向量δθxy和加速度计偏置ba；
S333、通过方程：
Figure FDA0002407729020000052
计算出IMU传感器的速度v作为初始化的IMU传感器速度，其中
Figure FDA0002407729020000053
表示Δp随IMU传感器陀螺仪偏置变化方程的一阶近似的雅克比矩阵，
Figure FDA0002407729020000054
表示IMU传感器坐标系中心在RGBD传感器坐标系下的坐标，
Figure FDA0002407729020000055
分别表示在第i关键帧时RGBD传感器坐标系中心在世界坐标系下的坐标，vi表示在第i关键帧时IMU传感器的速度，
Figure FDA0002407729020000056
表示在第i关键帧时RGBD坐标系相对于世界坐标的旋转矩阵，
Figure FDA0002407729020000057
分别表示在第i关键帧时IMU传感器坐标系相对于世界坐标系的旋转矩阵，Δti,i+1表示IMU传感器第i关键帧到第i+1关键帧时间的间隔。
6.根据权利要求1所述的一种基于RGBD传感器和IMU传感器的定位方法，其特征在于，所述的步骤S4过程如下：
S41、采用视觉SLAM算法估计RGBD传感器的位姿信息ξ；
S42、建立关键帧之间的约束关系，根据IMU传感器误差项和移动机器人的位姿信息，基于重投影误差建立最小二乘关系：
Eproj(k,j)为位姿信息和特征点的重投影误差，表述式如下：
Figure FDA0002407729020000058
Figure FDA0002407729020000059
EIMU(i,j)为IMU传感器的误差项，表述式如下：
Figure FDA00024077290200000510
Figure FDA0002407729020000061
Figure FDA0002407729020000062
Figure FDA0002407729020000063
eb＝bj-bi
其中k表示图像的特征点序号，i表示关键帧序号，j表示当前关键帧序号，s表示深度图的尺度因子，uk表示第k个特征点对应的RGBD传感器相机坐标下的二维坐标，K表示RGBD传感器内参矩阵，ξ^表示RGBD传感器位姿信息ξ的反对称矩阵，
Figure FDA0002407729020000064
表示第k个特征点Xk在RGBD传感器坐标系C下的坐标，Σk是特征点Xk和对应像素点的尺度信息矩阵，RCB为标定的IMU传感器转换到RGBD传感器坐标系下的标定旋转矩阵，
Figure FDA0002407729020000065
表示在第j关键帧时刻世界坐标系相对于IMU传感器坐标系的旋转矩阵，
Figure FDA0002407729020000066
表示第k个特征点Xk在世界坐标系W下的坐标，
Figure FDA0002407729020000067
表示第j关键帧时刻IMU传感器在世界坐标系下的坐标，
Figure FDA0002407729020000068
表示IMU传感器坐标系中心在RGBD传感器坐标系下的坐标，ΔRi,j、Δvi,j、Δpi,j分别表示关键帧i到关键帧j之间的IMU传感器的旋转矩阵、速度变换和位移变换的预积分，
Figure FDA0002407729020000069
Figure FDA00024077290200000610
分别表示ΔR、Δv、Δp随IMU传感器陀螺仪偏置变化方程的一阶近似的雅克比矩阵，
Figure FDA00024077290200000611
分别为Δv、Δp随IMU传感器加速度计偏置变化方程的一阶近似的雅克比矩阵，
Figure FDA00024077290200000612
表示IMU传感器在第j关键帧时刻的速度，
Figure FDA00024077290200000613
表示IMU传感器的坐标系B在关键帧j时旋转到世界坐标系的旋转矩阵，
Figure FDA00024077290200000614
分别表示在第j关键帧时刻IMU传感器陀螺仪偏置和加速度计偏置，ΣR、ΣI分别为IMU传感器偏置随即游走和预积分的信息矩阵，bi、bj分别表示第i和第j关键帧的IMU传感器偏置，Δti,j表示第i关键帧到第j关键帧的时间差；
S43、对于当前关键帧j的移动机器人的状态跟踪，当前关键帧j仅与相邻关键帧j-1建立约束关系：
Figure FDA0002407729020000071
Figure FDA0002407729020000072
其中φ表示需要优化对象的集合，φ*表示优化后的φ对象集合，k表示特征点的序号，j表示当前关键帧序号，
Figure FDA0002407729020000073
表示IMU传感器的坐标系B在关键帧j时刻旋转到世界坐标系的旋转矩阵，
Figure FDA0002407729020000074
表示IMU传感器关键帧j时刻在世界坐标系下的平移，
Figure FDA0002407729020000075
表示IMU传感器在第j关键帧时刻的速度，
Figure FDA0002407729020000076
分别表示在关键帧j时刻IMU传感器陀螺仪偏置和加速度计偏置；
求解出当前关键帧的移动机器人位姿信息和IMU传感器速度、偏置的最优值作为移动机器人的状态跟踪；
S44、采用基于滑动窗口的优化方式对移动机器人的状态进行非线性优化，对于当前关键帧之前的M帧关键帧，通过对移动机器人的位姿信息、特征点和IMU传感器误差项建立约束关系进行优化：
Figure FDA0002407729020000077
Figure FDA0002407729020000078
其中φ表示需要优化对象的集合，φ*表示优化后的φ对象集合，k表示特征点的序号，i表示关键帧序号，j表示当前关键帧序号，
Figure FDA0002407729020000079
表示IMU传感器的坐标系B在关键帧j时刻旋转到世界坐标系的旋转矩阵，
Figure FDA00024077290200000710
表示IMU传感器关键帧j时刻在世界坐标系下的平移，
Figure FDA00024077290200000711
表示IMU传感器在第j关键帧时刻的速度，
Figure FDA00024077290200000712
分别表示在关键帧j时刻IMU传感器陀螺仪偏置和加速度计偏置，对于当前关键帧M+1关键帧前的关键帧，仅对与当前关键帧具有共视点的关键帧位姿信息和特征点建立约束关系进行优化：
Figure FDA0002407729020000081
Figure FDA0002407729020000082
其中ψ表示需要优化对象的集合，ψ*表示优化后的ψ对象集合。
7.根据权利要求1所述的一种基于RGBD传感器和IMU传感器的定位方法，其特征在于，所述的步骤S5过程如下：
S51、采用基于词袋模型的回环检测算法对当前关键帧的RGBD传感器采集图像进行判断是否有回环；
S52、如果未检测到回环，输出当前关键帧的移动机器人的位姿，结束回环优化步骤，然后对后续的移动机器人的状态进行跟踪；如果检测到回环，采用视觉SLAM算法对RGBD传感器位姿信息和特征点进行回环优化；
S53、基于滑动窗口的非线性优化算法对移动机器人位姿信息、特征点和IMU传感器误差项进行优化，输出当前关键帧的移动机器人的位姿。
CN202010166801.0A 2020-03-11 2020-03-11 一种基于rgbd传感器和imu传感器的定位方法 Active CN111462231B (zh)

Priority Applications (3)

Application Number Priority Date Filing Date Title
CN202010166801.0A CN111462231B (zh) 2020-03-11 2020-03-11 一种基于rgbd传感器和imu传感器的定位方法
PCT/CN2021/080007 WO2021180128A1 (zh) 2020-03-11 2021-03-10 一种基于rgbd传感器和imu传感器的定位方法
GB2116617.8A GB2597632A (en) 2020-03-11 2021-03-10 RGBD sensor and IMU sensor-based positioning method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010166801.0A CN111462231B (zh) 2020-03-11 2020-03-11 一种基于rgbd传感器和imu传感器的定位方法

Publications (2)

Publication Number Publication Date
CN111462231A true CN111462231A (zh) 2020-07-28
CN111462231B CN111462231B (zh) 2023-03-31

Family

ID=71680707

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010166801.0A Active CN111462231B (zh) 2020-03-11 2020-03-11 一种基于rgbd传感器和imu传感器的定位方法

Country Status (3)

Country Link
CN (1) CN111462231B (zh)
GB (1) GB2597632A (zh)
WO (1) WO2021180128A1 (zh)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112085776A (zh) * 2020-07-31 2020-12-15 山东科技大学 一种直接法无监督单目图像场景深度估计方法
CN112179373A (zh) * 2020-08-21 2021-01-05 同济大学 一种视觉里程计的测量方法及视觉里程计
CN112461228A (zh) * 2020-11-03 2021-03-09 南昌航空大学 一种相似环境下基于imu和视觉的二次回环检测定位方法
CN113159197A (zh) * 2021-04-26 2021-07-23 北京华捷艾米科技有限公司 一种纯旋转运动状态判定方法及装置
WO2021180128A1 (zh) * 2020-03-11 2021-09-16 华南理工大学 一种基于rgbd传感器和imu传感器的定位方法
CN113610001A (zh) * 2021-08-09 2021-11-05 西安电子科技大学 基于深度相机和imu组合的室内移动终端定位方法

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114234967B (zh) * 2021-12-16 2023-10-20 浙江大学 一种基于多传感器融合的六足机器人定位方法
CN114549656A (zh) * 2022-02-14 2022-05-27 希姆通信息技术（上海）有限公司 Ar眼镜相机与imu的标定方法
CN114913245B (zh) * 2022-06-08 2023-07-28 上海鱼微阿科技有限公司 一种基于无向有权图的多标定块多摄像头标定方法及***
CN115855042A (zh) * 2022-12-12 2023-03-28 北京自动化控制设备研究所 一种基于激光雷达协同辅助的行人视觉导航方法
CN116442248A (zh) * 2023-06-19 2023-07-18 山东工程职业技术大学 一种基于目标检测的机器人视觉定位模块及方法
CN117272593B (zh) * 2023-08-24 2024-04-05 无锡北微传感科技有限公司 一种风洞试验数据分析处理方法
CN116883502B (zh) * 2023-09-05 2024-01-09 深圳市智绘科技有限公司 相机位姿和路标点位置的确定方法、装置、介质及设备

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法
CN110345944A (zh) * 2019-05-27 2019-10-18 浙江工业大学 融合视觉特征和imu信息的机器人定位方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11080540B2 (en) * 2018-03-20 2021-08-03 Vangogh Imaging, Inc. 3D vision processing using an IP block
CN108846867A (zh) * 2018-08-29 2018-11-20 安徽云能天智能科技有限责任公司 一种基于多目全景惯导的slam***
CN109307508B (zh) * 2018-08-29 2022-04-08 中国科学院合肥物质科学研究院 一种基于多关键帧的全景惯导slam方法
CN111462231B (zh) * 2020-03-11 2023-03-31 华南理工大学 一种基于rgbd传感器和imu传感器的定位方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法
CN110345944A (zh) * 2019-05-27 2019-10-18 浙江工业大学 融合视觉特征和imu信息的机器人定位方法

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2021180128A1 (zh) * 2020-03-11 2021-09-16 华南理工大学 一种基于rgbd传感器和imu传感器的定位方法
GB2597632A (en) * 2020-03-11 2022-02-02 Univ South China Tech RGBD sensor and IMU sensor-based positioning method
CN112085776A (zh) * 2020-07-31 2020-12-15 山东科技大学 一种直接法无监督单目图像场景深度估计方法
CN112085776B (zh) * 2020-07-31 2022-07-19 山东科技大学 一种直接法无监督单目图像场景深度估计方法
CN112179373A (zh) * 2020-08-21 2021-01-05 同济大学 一种视觉里程计的测量方法及视觉里程计
CN112461228A (zh) * 2020-11-03 2021-03-09 南昌航空大学 一种相似环境下基于imu和视觉的二次回环检测定位方法
CN112461228B (zh) * 2020-11-03 2023-05-09 南昌航空大学 一种相似环境下基于imu和视觉的二次回环检测定位方法
CN113159197A (zh) * 2021-04-26 2021-07-23 北京华捷艾米科技有限公司 一种纯旋转运动状态判定方法及装置
CN113610001A (zh) * 2021-08-09 2021-11-05 西安电子科技大学 基于深度相机和imu组合的室内移动终端定位方法
CN113610001B (zh) * 2021-08-09 2024-02-09 西安电子科技大学 基于深度相机和imu组合的室内移动终端定位方法

Also Published As

Publication number Publication date
CN111462231B (zh) 2023-03-31
WO2021180128A1 (zh) 2021-09-16
GB2597632A (en) 2022-02-02
GB202116617D0 (en) 2022-01-05

Similar Documents

Publication Publication Date Title
CN111462231B (zh) 一种基于rgbd传感器和imu传感器的定位方法
EP3940421A1 (en) Positioning method and device based on multi-sensor fusion
CN109029433B (zh) 一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法
CN109376785B (zh) 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN110398245B (zh) 基于脚戴式惯性测量单元的室内行人导航姿态估计方法
CN106052584B (zh) 一种基于视觉及惯性信息融合的轨道空间线形测量方法
CN107193279A (zh) 基于单目视觉和imu信息的机器人定位与地图构建***
CN110986939A (zh) 一种基于imu预积分的视觉惯性里程计方法
CN112815939B (zh) 移动机器人的位姿估计方法及计算机可读存储介质
CN107767425A (zh) 一种基于单目vio的移动端AR方法
CN112985450B (zh) 一种具有同步时间误差估计的双目视觉惯性里程计方法
CN109764870B (zh) 基于变换估计量建模方案的载体初始航向估算方法
CN114234967B (zh) 一种基于多传感器融合的六足机器人定位方法
CN111156997A (zh) 一种基于相机内参在线标定的视觉/惯性组合导航方法
CN115574816B (zh) 仿生视觉多源信息智能感知无人平台
CN111189474A (zh) 基于mems的marg传感器的自主校准方法
CN115371665A (zh) 一种基于深度相机和惯性融合的移动机器人定位方法
CN109282810A (zh) 一种惯导和角度传感器融合的蛇形机器人姿态估计方法
CN111307146A (zh) 一种基于双目相机和imu的虚拟现实头戴显示设备定位***
CN113503872B (zh) 一种基于相机与消费级imu融合的低速无人车定位方法
CN113865584B (zh) 一种基于视觉惯性里程计的uwb三维寻物方法和装置
CN114397642A (zh) 一种基于图优化的三维激光雷达与imu外参标定方法
CN112284381B (zh) 视觉惯性实时初始化对准方法及***
CN117710476A (zh) 基于单目视觉的无人机位姿估计与稠密建图方法
CN110160530B (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
CB02 Change of applicant information

Address after: Nansha District Avenue South Ring of 511458 cities in Guangdong province Guangzhou City, No. 25 Hua Da Guangzhou production and Research Institute

Applicant after: SOUTH CHINA University OF TECHNOLOGY

Applicant after: GUANGZHOU INSTITUTE OF MODERN INDUSTRIAL TECHNOLOGY

Address before: 510640 Tianhe District, Guangdong, No. five road, No. 381,

Applicant before: SOUTH CHINA University OF TECHNOLOGY

Applicant before: GUANGZHOU INSTITUTE OF MODERN INDUSTRIAL TECHNOLOGY

CB02 Change of applicant information
GR01 Patent grant
GR01 Patent grant