CN111462231A - 一种基于rgbd传感器和imu传感器的定位方法 - Google Patents
一种基于rgbd传感器和imu传感器的定位方法 Download PDFInfo
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 20
- 238000005457 optimization Methods 0.000 claims abstract description 32
- 230000000007 visual effect Effects 0.000 claims abstract description 10
- 238000001514 detection method Methods 0.000 claims abstract description 6
- 239000011159 matrix material Substances 0.000 claims description 93
- 230000005484 gravity Effects 0.000 claims description 32
- 230000010354 integration Effects 0.000 claims description 29
- 230000009466 transformation Effects 0.000 claims description 17
- 230000001133 acceleration Effects 0.000 claims description 10
- 238000006073 displacement reaction Methods 0.000 claims description 10
- 230000008859 change Effects 0.000 claims description 6
- CLOMYZFHNHFSIQ-UHFFFAOYSA-N clonixin Chemical compound CC1=C(Cl)C=CC=C1NC1=NC=CC=C1C(O)=O CLOMYZFHNHFSIQ-UHFFFAOYSA-N 0.000 claims description 6
- 238000013519 translation Methods 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 4
- 238000006243 chemical reaction Methods 0.000 claims description 4
- 230000008569 process Effects 0.000 claims description 4
- 238000000354 decomposition reaction Methods 0.000 claims description 3
- 238000000605 extraction Methods 0.000 claims description 3
- 238000005259 measurement Methods 0.000 description 4
- 238000005070 sampling Methods 0.000 description 4
- 241000270666 Testudines Species 0.000 description 3
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 1
- 239000006185 dispersion Substances 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000010606 normalization Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
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
技术领域
本发明涉及视觉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传感器测量得到加速度和角速度:
其中ω(t)和a(t)分别表示角速度和加速度的真实值,和分别表示角速度和加速度的IMU传感器测量值,RWB表示IMU传感器坐标系到世界坐标系的旋转矩阵,bg(t)和ba(t)表示随时间变化的IMU传感器陀螺仪偏置和加速度计偏置,ηg(t)和ηa(t)表示随时间变化的IMU传感器的陀螺仪和加速度计高斯白噪声,gw表示在世界坐标下的重力向量;
对IMU传感器测量值进行积分,并根据采样频率将其离散化,可以得到:
其中和表示k时刻的IMU传感器陀螺仪偏置和加速度计偏置,和表示离散后的IMU传感器的陀螺仪和加速度计高斯白噪声,ΔRik表示i时刻到k时刻的旋转矩阵,ak和ωk分别表示IMU传感器测量的加速度和角速度离散值,pi、vi、分别表示在i时刻IMU传感器离散后的位移、速度和旋转矩阵,gw表示在世界坐标下的重力向量,Δt表示离散的时刻差。
进一步地,为了避免上一时刻数据改变,导致下一时刻需要重新推导的问题,计算出两个时刻间的预积分,并分离出IMU传感器加速度计偏置和陀螺仪偏置,通过IMU预积分可以根据当前时刻的状态,推算出下一时刻的状态:
其中,表示IMU传感器的坐标系B在i+1时刻旋转到世界坐标系的旋转矩阵,vi+1、pi+1分别表示IMU传感器坐标系B在i+1时刻在世界坐标系下的速度和位姿信息,Δti,i+1表示IMU传感器i时刻到i+1时刻的间隔,分别表示在i时刻IMU传感器陀螺仪偏置和加速度计偏置,gw表示在世界坐标系下的重力加速度,ΔRi,i+1、Δvi,i+1、Δpi,i+1分别表示i时刻到i+1时刻IMU传感器的旋转矩阵、速度变换和位移变换的预积分,分别表示ΔR、Δv、Δp随IMU传感器陀螺仪偏置变化方程的一阶近似的雅克比矩阵,分别为Δv、Δp随IMU传感器加速度计偏置变化方程的一阶近似的雅克比矩阵。
进一步地,需要对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传感器的位姿信息,使其误差最小:
其中ξ*表示优化后的最优位姿信息,ξ表示估计的位姿信息,n表示特征点的个数;
S32、根据视觉SLAM算法选定的N个连续关键帧,采用IMU预积分方程对N个连续关键帧之间的IMU传感器陀螺仪的数据进行积分,得到IMU传感器在N个关键帧之间的旋转变换,通过建立N个关键帧之间的RGBD传感器和IMU传感器旋转变换的最小误差方程,对IMU传感器陀螺仪的偏置bg进行估计:
其中N表示关键帧的个数,是关键帧i由视觉SLAM算法得到的RGBD传感器坐标系相对于世界坐标系的旋转,表示关键帧i+1世界坐标系转换到IMU坐标系的旋转矩阵,RCB为标定的IMU传感器转换到RGBD传感器坐标系下的标定旋转矩阵,ΔRi,i+1为连续两关键帧间的IMU传感器积分所得的旋转矩阵,表示ΔR随IMU传感器陀螺仪偏置变化方程的一阶近似的雅克比矩阵;
S33、利用上述IMU传感器的陀螺仪偏置bg估计的结果,对IMU传感器的加速度计偏置、重力向量进行估计:
其中RWI表示IMU传感器在世界坐标系的方向;
IMU传感器重力向量可以表示为:
加入重力扰动向量δθ:
其中δθ=[δθxy 0]T,δθxy=[δθx δθy]T
对gW进行一阶近似:
根据RGBD传感器坐标系C和IMU传感器坐标系B之间的变换:
根据IMU传感器预积分计算方程可以联立下述方程:
其中表示Δp随IMU传感器陀螺仪偏置变化方程的一阶近似的雅克比矩阵,表示IMU传感器坐标系中心在RGBD传感器坐标系下的坐标,分别表示在第i关键帧时RGBD传感器坐标系中心在世界坐标系下的坐标,vi表示在第i关键帧时IMU传感器的速度,表示在第i关键帧时RGBD坐标系相对于世界坐标的旋转矩阵,分别表示在第i关键帧时IMU传感器坐标系相对于世界坐标系的旋转矩阵,Δti,i+1表示IMU传感器第i关键帧到第i+1关键帧时间的间隔。
根据三个连续关键帧之间的IMU预积分方程的递推关系,以及IMU传感器坐标系和RGBD传感器坐标系的转换方程,可以得到线性方程:
其中
其中δθxy为中的二阶向量,δθ为重力偏差向量,ba表示初始化的IMU传感器加速度计偏置,RWI表示IMU传感器在世界坐标系的方向,Δt12、Δt23分别表示关键帧1、2到关键帧2、3的IMU传感器的时间间隔,G表示重力参考值,表示在IMU传感器坐标系下的重力向量,[](:,1:2)表示只包含该矩阵前两列数据,分别表示关键帧1、2时刻IMU传感器坐标系相对于世界坐标系的旋转矩阵,Δp12、Δp23分别表示关键帧1、2到关键帧2、3的IMU传感器的位移,Δv23表示关键帧2到关键帧3的速度变化,分别为Δv23、Δp23、Δp12随IMU传感器加速度计偏置变化方程的一阶近似的雅克比矩阵,分别表示关键帧1、2、3时刻的RGBD传感器坐标系C相对于世界坐标系的旋转矩阵,分别表示关键帧1、2、3时刻RGBD传感器坐标系中心在世界坐标系下的坐标,表示IMU传感器坐标系中心在RGBD传感器坐标系下的坐标;
S332、通过奇异值分解求解出重力偏差二阶向量δθxy和加速度计偏置ba;
S333、通过方程:
计算出IMU传感器的速度v作为初始化的IMU传感器速度,其中表示Δp随IMU传感器陀螺仪偏置变化方程的一阶近似的雅克比矩阵,表示IMU传感器坐标系中心在RGBD传感器坐标系下的坐标,分别表示在第i关键帧时RGBD传感器坐标系中心在世界坐标系下的坐标,vi表示在第i关键帧时IMU传感器的速度,表示在第i关键帧时RGBD坐标系相对于世界坐标的旋转矩阵,分别表示在第i关键帧时IMU传感器坐标系相对于世界坐标系的旋转矩阵,Δti,i+1表示IMU传感器第i关键帧到第i+1关键帧时间的间隔。
进一步地,移动机器人的状态为移动机器人的位姿信息、速度和IMU传感器的偏置,所述的步骤S4过程如下:
S41、采用视觉SLAM算法估计RGBD传感器的位姿信息ξ;
S42、建立关键帧之间的约束关系,根据IMU传感器误差项和移动机器人的位姿信息,基于重投影误差建立最小二乘关系:
Eproj(k,j)为位姿信息和特征点的重投影误差,表述式如下:
EIMU(i,j)为IMU传感器的误差项,表述式如下:
其中k表示图像的特征点序号,i表示关键帧序号,j表示当前关键帧序号,s表示深度图的尺度因子,uk表示第k个特征点对应的RGBD传感器相机坐标下的二维坐标,K表示RGBD传感器内参矩阵,ξ^表示RGBD传感器位姿信息ξ的反对称矩阵,表示第k个特征点Xk在RGBD传感器坐标系C下的坐标,Σk是特征点Xk和对应像素点的尺度信息矩阵,RCB为标定的IMU传感器转换到RGBD传感器坐标系下的标定旋转矩阵,表示在第j关键帧时刻世界坐标系相对于IMU传感器坐标系的旋转矩阵,表示第k个特征点Xk在世界坐标系W下的坐标,表示第j关键帧时刻IMU传感器在世界坐标系下的坐标,表示IMU传感器坐标系中心在RGBD传感器坐标系下的坐标,ΔRi,j、Δvi,j、Δpi,j分别表示关键帧i到关键帧j之间的IMU传感器的旋转矩阵、速度变换和位移变换的预积分, 分别表示ΔR、Δv、Δp随IMU传感器陀螺仪偏置变化方程的一阶近似的雅克比矩阵,分别为Δv、Δp随IMU传感器加速度计偏置变化方程的一阶近似的雅克比矩阵,表示IMU传感器在第j关键帧时刻的速度,表示IMU传感器的坐标系B在关键帧j时旋转到世界坐标系的旋转矩阵,分别表示在第j关键帧时刻IMU传感器陀螺仪偏置和加速度计偏置,ΣR、ΣI分别为IMU传感器偏置随即游走和预积分的信息矩阵,bi、bj分别表示第i和第j关键帧的IMU传感器偏置,Δti,j表示第i关键帧到第j关键帧的时间差;
S43、对于当前关键帧j的移动机器人状态跟踪,仅与相邻关键帧j-1建立约束关系:
其中φ表示需要优化对象的集合,φ*表示优化后的φ对象集合,k表示特征点的序号,j表示当前关键帧序号,表示IMU传感器的坐标系B在关键帧j时刻旋转到世界坐标系的旋转矩阵,表示IMU传感器关键帧j时刻在世界坐标系下的平移,表示IMU传感器在第j关键帧时刻的速度,分别表示在关键帧j时刻IMU传感器陀螺仪偏置和加速度计偏置;
求解出当前关键帧的移动机器人位姿信息和IMU传感器速度、偏置的最优值作为移动机器人的状态跟踪;
S44、由于RGBD传感器和IMU传感器需要优化9自由度的状态,如果对于所有共视关键帧都对所有状态量进行优化,则运算量过大,无法实现实时的定位,并且IMU传感器存在零偏的情况,距离当前时刻的数据时间越长,则越不可靠。
因此采用基于滑动窗口的优化算法对移动机器人的状态进行非线性优化,对于当前关键帧之前的M帧关键帧,通过对移动机器人的位姿信息和IMU传感器误差项建立约束关系进行优化:
其中φ表示需要优化对象的集合,φ*表示优化后的φ对象集合,k表示特征点的序号,i表示关键帧序号,j表示当前关键帧序号,表示IMU传感器的坐标系B在关键帧j时刻旋转到世界坐标系的旋转矩阵,表示IMU传感器关键帧j时刻在世界坐标系下的平移,表示IMU传感器在第j关键帧时刻的速度,分别表示在关键帧j时刻IMU传感器陀螺仪偏置和加速度计偏置;
对于当前关键帧M+1关键帧前的关键帧,仅对与当前关键帧具有共视点的关键帧位姿信息和特征点建立约束关系进行优化:
其中ψ表示需要优化对象的集合,ψ*表示优化后的ψ对象集合。
进一步地,所述的步骤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预积分由当前时刻的状态推算出下一时刻的状态,计算公式如下:
其中,表示IMU传感器的坐标系B在i+1时刻旋转到世界坐标系的旋转矩阵,vi+1、pi+1分别表示IMU传感器坐标系B在i+1时刻在世界坐标系下的速度和位姿信息,Δti,i+1表示IMU传感器i时刻到i+1时刻的间隔,分别表示在i时刻IMU传感器陀螺仪偏置和加速度计偏置,gw表示在世界坐标系下的重力加速度,ΔRi,i+1、Δvi,i+1、Δpi,i+1分别表示i时刻到i+1时刻IMU传感器的旋转矩阵、速度变换和位移变换的预积分,分别表示ΔR、Δv、Δp随IMU传感器陀螺仪偏置变化方程的一阶近似的雅克比矩阵,分别为Δv、Δp随IMU传感器加速度计偏置变化方程的一阶近似的雅克比矩阵。
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传感器的位姿信息,使其误差最小:
其中ξ表示优化后的最优位姿信息,ξ表示估计的位姿信息,n表示特征点的个数;
T32、根据视觉SLAM算法选定的N个连续关键帧,采用IMU预积分方程对N个连续关键帧之间的IMU传感器陀螺仪的数据进行积分,得到IMU传感器在N个关键帧之间的旋转变换,通过建立N个关键帧之间的RGBD传感器和IMU传感器旋转变换的最小误差方程,对IMU传感器陀螺仪的偏置bg进行估计:
其中N表示关键帧的个数,ΔRi,i+1为连续关键帧i和i+1间的IMU传感器积分所得的旋转矩阵,表示ΔR随IMU传感器陀螺仪偏置变化方程的一阶近似的雅克比矩阵,是关键帧i由视觉ORBSLAM2算法得到的RGBD传感器坐标系相对于世界坐标系的旋转,表示关键帧i+1世界坐标系转换到IMU坐标系的旋转矩阵,RCB为标定的IMU传感器转换到RGBD传感器坐标系下的标定旋转矩阵;
本实施例中通过两个关键帧对IMU传感器的陀螺仪偏置进行估计,因此N取2;
T33、利用上述IMU传感器的陀螺仪偏置bg估计的结果,对IMU传感器的加速度计偏置、重力向量进行估计:
T331、根据三个连续关键帧之间的IMU预积分方程的递推关系,以及IMU传感器坐标系和RGBD传感器坐标系的转换方程,可以得到线性方程:
其中
其中δθxy为中的二阶向量,δθ为重力偏差向量,ba表示初始化的IMU传感器加速度计偏置,RWI表示IMU传感器在世界坐标系的方向,Δt12、Δt23分别表示关键帧1、2到关键帧2、3的IMU传感器的时间间隔,G表示重力参考值,表示在IMU传感器坐标系下的重力向量,表示重力向量的一阶近似,[](:,1:2)表示只包含该矩阵前两列数据,分别表示关键帧1、2时刻IMU传感器坐标系相对于世界坐标系的旋转矩阵,Δp12、Δp23分别表示关键帧1、2到关键帧2、3的IMU传感器的位移,Δv23表示关键帧2到关键帧3的速度变化, 分别为Δv23、Δp23、Δp12随IMU传感器加速度计偏置变化方程的一阶近似的雅克比矩阵,分别表示关键帧1、2、3时刻的RGBD传感器坐标系C相对于世界坐标系的旋转矩阵,分别表示关键帧1、2、3时刻RGBD传感器坐标系中心在世界坐标系下的坐标,表示IMU传感器坐标系中心在RGBD传感器坐标系下的坐标;
T332、通过奇异值分解求解出重力偏差二阶向量δθxy和加速度计偏置ba;
T333、通过方程:
其中表示Δp随IMU传感器陀螺仪偏置变化方程的一阶近似的雅克比矩阵,表示IMU传感器坐标系中心在RGBD传感器坐标系下的坐标,分别表示关键帧i时RGBD传感器坐标系中心在世界坐标系下的坐标,vi表示关键帧i时IMU传感器的速度,表示在i关键帧RGBD坐标系相对于世界坐标的旋转矩阵,分别表示关键帧i时IMU传感器坐标系相对于世界坐标系的旋转矩阵;
计算出IMU传感器的速度v作为初始化的IMU传感器速度。
T4、跟踪移动机器人的状态,并采用基于滑动窗口的非线性优化对移动机器人状态优化,移动机器人的状态为移动机器人的位姿信息、速度和IMU传感器的偏置:
T41、采用视觉ORBSLAM2算法估计RGBD传感器的位姿信息ξ;
T42、建立关键帧之间的约束关系,根据IMU传感器误差项和移动机器人的位姿信息,基于重投影误差建立最小二乘关系:
Eproj(k,j)为位姿信息和特征点的重投影误差,表述式如下:
EIMU(i,j)为IMU传感器的误差项,表述式如下:
其中k表示图像的特征点序号,i表示关键帧序号,j表示当前关键帧序号,s表示深度图的尺度因子,uk表示第k个特征点对应的RGBD传感器相机坐标下的二维坐标,K表示RGBD传感器内参矩阵,ξ^表示RGBD传感器位姿信息ξ的反对称矩阵,表示第k个特征点Xk在RGBD传感器坐标系C下的坐标,Σk是特征点Xk和对应像素点的尺度信息矩阵,RCB为标定的IMU传感器转换到RGBD传感器坐标系下的标定旋转矩阵,表示在第j关键帧时刻世界坐标系相对于IMU传感器坐标系的旋转矩阵,表示第k个特征点Xk在世界坐标系W下的坐标,表示第j关键帧时刻IMU传感器在世界坐标系下的坐标,表示IMU传感器坐标系中心在RGBD传感器坐标系下的坐标,ΔRi,j、Δvi,j、Δpi,j分别表示关键帧i到关键帧j之间的IMU传感器的旋转矩阵、速度变换和位移变换的预积分, 分别表示ΔR、Δv、Δp随IMU传感器陀螺仪偏置变化方程的一阶近似的雅克比矩阵,分别为Δv、Δp随IMU传感器加速度计偏置变化方程的一阶近似的雅克比矩阵,表示IMU传感器在第j关键帧时刻的速度,表示IMU传感器的坐标系B在关键帧j时旋转到世界坐标系的旋转矩阵,分别表示在第j关键帧时刻IMU传感器陀螺仪偏置和加速度计偏置,ΣR、ΣI分别为IMU传感器偏置随即游走和预积分的信息矩阵,bi、bj分别表示第i和第j关键帧的IMU传感器偏置,Δti,j表示第i关键帧到第j关键帧的时间差;
T43、对于当前关键帧j的移动机器人状态跟踪,仅与相邻关键帧j-1建立约束关系:
其中φ表示需要优化对象的集合,φ*表示优化后的φ对象集合,k表示特征点的序号,j表示当前关键帧序号,表示IMU传感器的坐标系B在关键帧j时刻旋转到世界坐标系的旋转矩阵,表示IMU传感器关键帧j时刻在世界坐标系下的平移,表示IMU传感器在第j关键帧时刻的速度,分别表示在关键帧j时刻IMU传感器陀螺仪偏置和加速度计偏置;
求解出当前关键帧的移动机器人位姿信息和IMU传感器速度、偏置的最优值作为移动机器人的状态跟踪;
T44、采用基于滑动窗口的优化方式对移动机器人的状态进行非线性优化,对于当前关键帧之前的M帧关键帧,通过对移动机器人的位姿信息和IMU传感器误差项建立约束关系进行优化:
其中φ表示需要优化对象的集合,φ*表示优化后的φ对象集合,k表示特征点的序号,i表示关键帧序号,j表示当前关键帧序号,表示IMU传感器的坐标系B在关键帧j时刻旋转到世界坐标系的旋转矩阵,表示IMU传感器关键帧j时刻在世界坐标系下的平移,表示IMU传感器在第j关键帧时刻的速度,分别表示在关键帧j时刻IMU传感器陀螺仪偏置和加速度计偏置;
对于当前关键帧M+1关键帧前的关键帧,仅对与当前关键帧具有共视点的关键帧位姿信息和特征点建立约束关系进行优化:
其中ψ表示需要优化对象的集合,ψ*表示优化后的ψ对象集合。
本实施例中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预积分由当前时刻的状态推算出下一时刻的状态,计算公式如下:
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传感器的位姿信息,使其误差最小:
其中ξ*表示优化后的最优位姿信息,ξ表示估计的位姿信息,n表示特征点的个数;
S32、根据视觉SLAM算法选定的N个连续关键帧,采用IMU预积分方程对N个连续关键帧之间的IMU传感器陀螺仪的数据进行积分,得到IMU传感器在N个关键帧之间的旋转变换,通过建立N个关键帧之间的RGBD传感器和IMU传感器旋转变换的最小误差方程,对IMU传感器陀螺仪的偏置bg进行估计:
其中N表示关键帧的个数,ΔRi,i+1为关键帧i和i+1间的IMU传感器积分所得的旋转矩阵,表示ΔR随IMU传感器陀螺仪偏置变化方程的一阶近似的雅克比矩阵, 是关键帧i由视觉SLAM算法得到的RGBD传感器坐标系相对于世界坐标系的旋转,表示关键帧i+1世界坐标系转换到IMU坐标系的旋转矩阵,RCB为标定的IMU传感器转换到RGBD传感器坐标系下的标定旋转矩阵;
S33、利用上述IMU传感器的陀螺仪偏置bg估计的结果,对IMU传感器的加速度计偏置、重力向量进行估计:
S331、根据三个连续关键帧之间的IMU预积分方程的递推关系,以及IMU传感器坐标系和RGBD传感器坐标系的转换方程,得到线性方程:
其中
其中δθxy为中的二阶向量,δθ为重力偏差向量,ba表示初始化的IMU传感器加速度计偏置,RWI表示IMU传感器在世界坐标系的方向,Δt12、Δt23分别表示关键帧1、2到关键帧2、3的IMU传感器的时间间隔,G表示重力参考值,表示在IMU传感器坐标系下的重力向量,表示重力向量的一阶近似,[](:,1:2)表示只包含该矩阵前两列数据,分别表示关键帧1、2时刻IMU传感器坐标系相对于世界坐标系的旋转矩阵,Δp12、Δp23分别表示关键帧1、2到关键帧2、3的IMU传感器的位移,Δv23表示关键帧2到关键帧3的速度变化,分别为Δv23、Δp23、Δp12随IMU传感器加速度计偏置变化方程的一阶近似的雅克比矩阵,分别表示关键帧1、2、3时刻的RGBD传感器坐标系C相对于世界坐标系的旋转矩阵,分别表示关键帧1、2、3时刻RGBD传感器坐标系中心在世界坐标系下的坐标,表示IMU传感器坐标系中心在RGBD传感器坐标系下的坐标;
S332、通过奇异值分解求解出重力偏差二阶向量δθxy和加速度计偏置ba;
S333、通过方程:
6.根据权利要求1所述的一种基于RGBD传感器和IMU传感器的定位方法,其特征在于,所述的步骤S4过程如下:
S41、采用视觉SLAM算法估计RGBD传感器的位姿信息ξ;
S42、建立关键帧之间的约束关系,根据IMU传感器误差项和移动机器人的位姿信息,基于重投影误差建立最小二乘关系:
Eproj(k,j)为位姿信息和特征点的重投影误差,表述式如下:
EIMU(i,j)为IMU传感器的误差项,表述式如下:
eb=bj-bi
其中k表示图像的特征点序号,i表示关键帧序号,j表示当前关键帧序号,s表示深度图的尺度因子,uk表示第k个特征点对应的RGBD传感器相机坐标下的二维坐标,K表示RGBD传感器内参矩阵,ξ^表示RGBD传感器位姿信息ξ的反对称矩阵,表示第k个特征点Xk在RGBD传感器坐标系C下的坐标,Σk是特征点Xk和对应像素点的尺度信息矩阵,RCB为标定的IMU传感器转换到RGBD传感器坐标系下的标定旋转矩阵,表示在第j关键帧时刻世界坐标系相对于IMU传感器坐标系的旋转矩阵,表示第k个特征点Xk在世界坐标系W下的坐标,表示第j关键帧时刻IMU传感器在世界坐标系下的坐标,表示IMU传感器坐标系中心在RGBD传感器坐标系下的坐标,ΔRi,j、Δvi,j、Δpi,j分别表示关键帧i到关键帧j之间的IMU传感器的旋转矩阵、速度变换和位移变换的预积分, 分别表示ΔR、Δv、Δp随IMU传感器陀螺仪偏置变化方程的一阶近似的雅克比矩阵,分别为Δv、Δp随IMU传感器加速度计偏置变化方程的一阶近似的雅克比矩阵,表示IMU传感器在第j关键帧时刻的速度,表示IMU传感器的坐标系B在关键帧j时旋转到世界坐标系的旋转矩阵,分别表示在第j关键帧时刻IMU传感器陀螺仪偏置和加速度计偏置,ΣR、ΣI分别为IMU传感器偏置随即游走和预积分的信息矩阵,bi、bj分别表示第i和第j关键帧的IMU传感器偏置,Δti,j表示第i关键帧到第j关键帧的时间差;
S43、对于当前关键帧j的移动机器人的状态跟踪,当前关键帧j仅与相邻关键帧j-1建立约束关系:
其中φ表示需要优化对象的集合,φ*表示优化后的φ对象集合,k表示特征点的序号,j表示当前关键帧序号,表示IMU传感器的坐标系B在关键帧j时刻旋转到世界坐标系的旋转矩阵,表示IMU传感器关键帧j时刻在世界坐标系下的平移,表示IMU传感器在第j关键帧时刻的速度,分别表示在关键帧j时刻IMU传感器陀螺仪偏置和加速度计偏置;
求解出当前关键帧的移动机器人位姿信息和IMU传感器速度、偏置的最优值作为移动机器人的状态跟踪;
S44、采用基于滑动窗口的优化方式对移动机器人的状态进行非线性优化,对于当前关键帧之前的M帧关键帧,通过对移动机器人的位姿信息、特征点和IMU传感器误差项建立约束关系进行优化:
其中φ表示需要优化对象的集合,φ*表示优化后的φ对象集合,k表示特征点的序号,i表示关键帧序号,j表示当前关键帧序号,表示IMU传感器的坐标系B在关键帧j时刻旋转到世界坐标系的旋转矩阵,表示IMU传感器关键帧j时刻在世界坐标系下的平移,表示IMU传感器在第j关键帧时刻的速度,分别表示在关键帧j时刻IMU传感器陀螺仪偏置和加速度计偏置,对于当前关键帧M+1关键帧前的关键帧,仅对与当前关键帧具有共视点的关键帧位姿信息和特征点建立约束关系进行优化:
其中ψ表示需要优化对象的集合,ψ*表示优化后的ψ对象集合。
7.根据权利要求1所述的一种基于RGBD传感器和IMU传感器的定位方法,其特征在于,所述的步骤S5过程如下:
S51、采用基于词袋模型的回环检测算法对当前关键帧的RGBD传感器采集图像进行判断是否有回环;
S52、如果未检测到回环,输出当前关键帧的移动机器人的位姿,结束回环优化步骤,然后对后续的移动机器人的状态进行跟踪;如果检测到回环,采用视觉SLAM算法对RGBD传感器位姿信息和特征点进行回环优化;
S53、基于滑动窗口的非线性优化算法对移动机器人位姿信息、特征点和IMU传感器误差项进行优化,输出当前关键帧的移动机器人的位姿。
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)
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)
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)
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)
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传感器的定位方法 |
-
2020
- 2020-03-11 CN CN202010166801.0A patent/CN111462231B/zh active Active
-
2021
- 2021-03-10 GB GB2116617.8A patent/GB2597632A/en active Pending
- 2021-03-10 WO PCT/CN2021/080007 patent/WO2021180128A1/zh active Application Filing
Patent Citations (2)
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)
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 |