CN115371665A - 一种基于深度相机和惯性融合的移动机器人定位方法 - Google Patents
一种基于深度相机和惯性融合的移动机器人定位方法 Download PDFInfo
- Publication number
- CN115371665A CN115371665A CN202211119147.3A CN202211119147A CN115371665A CN 115371665 A CN115371665 A CN 115371665A CN 202211119147 A CN202211119147 A CN 202211119147A CN 115371665 A CN115371665 A CN 115371665A
- Authority
- CN
- China
- Prior art keywords
- depth
- frame
- imu
- camera
- coordinate system
- 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 59
- 230000004927 fusion Effects 0.000 title claims abstract description 18
- 230000000007 visual effect Effects 0.000 claims abstract description 26
- 230000033001 locomotion Effects 0.000 claims abstract description 12
- 238000005259 measurement Methods 0.000 claims description 86
- 230000010354 integration Effects 0.000 claims description 35
- 238000013519 translation Methods 0.000 claims description 32
- 239000011159 matrix material Substances 0.000 claims description 22
- 238000005457 optimization Methods 0.000 claims description 16
- 238000006243 chemical reaction Methods 0.000 claims description 10
- 238000004422 calculation algorithm Methods 0.000 claims description 7
- 238000006073 displacement reaction Methods 0.000 claims description 6
- 238000012545 processing Methods 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 6
- 230000009466 transformation Effects 0.000 claims description 5
- 230000009012 visual motion Effects 0.000 claims description 5
- 238000012216 screening Methods 0.000 claims description 4
- 238000012935 Averaging Methods 0.000 claims description 2
- 206010034719 Personality change Diseases 0.000 claims description 2
- 238000000354 decomposition reaction Methods 0.000 claims description 2
- 238000010606 normalization Methods 0.000 claims description 2
- 230000003287 optical effect Effects 0.000 claims description 2
- 238000004806 packaging method and process Methods 0.000 claims description 2
- 230000005484 gravity Effects 0.000 claims 1
- 230000000694 effects Effects 0.000 abstract description 6
- 238000007726 management method Methods 0.000 abstract description 3
- 238000004364 calculation method Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 238000000605 extraction Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- LFQSCWFLJHTTHZ-UHFFFAOYSA-N Ethanol Chemical compound CCO LFQSCWFLJHTTHZ-UHFFFAOYSA-N 0.000 description 1
- 238000012897 Levenberg–Marquardt algorithm Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000012856 packing Methods 0.000 description 1
- 230000017105 transposition Effects 0.000 description 1
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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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
- G01C21/1656—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 with passive imaging devices, e.g. cameras
-
- 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
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Image Analysis (AREA)
Abstract
一种基于深度相机和惯性融合的移动机器人定位方法,它属于视觉惯性导航技术领域。本发明解决了由于受到移动机器人运动方式和环境的影响,导致当前视觉SLAM系统容易出现定位失败或获得的定位效果差的问题。本发明利用了深度相机的深度不确定性模型和多视图三角化结合的方法,能够自适应地完成不同距离特征点的深度估计。并且在此基础上,提出了特征点动态分级管理方法,该方法结合跟踪次数和重投影误差动态地进行特征点分类并去除离群点。通过构建IMU预积分先验误差模型,平面运动约束,特征点深度约束以及3D‑2D特征点重投影误差模型,利用多种约束联合实现移动机器人位姿的估计。本发明方法可以应用于视觉惯性导航技术领域。
Description
技术领域
本发明属于视觉惯性导航技术领域,具体涉及一种基于深度相机和惯性融合的移动机器人定位方法。
背景技术
近年来,随着计算机视觉的高速发展,基于视觉的同时定位与环境地图构建(简称视觉SLAM)技术在移动机器人领域已经得到了广泛应用。视觉SLAM技术无需依赖外界信号,能够通过相机感知环境信息实现地图构建从而估计自身的位姿变化。然而,在机器人纯旋转或急速运动的场景中,由于相机的帧率限制,光照的剧烈变化,常发生特征点跟踪失败导致SLAM失效的情况,从而无法得到移动机器人的定位信息。此外,由于移动机器人本身主要为平面运动,若采用单目SLAM方法,系统进行特征点深度计算时会因视差不足而产生较大的深度估计误差,严重影响定位效果。因此,单一的视觉SLAM算法会因其自身传感器的限制不可避免地有各方面的缺陷,这一现象在应用于复杂场景时尤为明显。
综上所述,由于移动机器人上受到机器人运动方式和环境影响,会导致当前视觉SLAM系统定位失败或获得的定位效果差的问题,因而,采用多传感器融合的方法,充分利用各传感器间测量信息的约束以及机器人的运动学约束来提高系统的位姿估计精度和鲁棒性是当前最佳的应用方案。
发明内容
本发明的目的是为解决由于受到移动机器人运动方式和环境的影响,导致当前视觉SLAM系统容易出现定位失败或获得的定位效果差的问题,而提出的一种基于深度相机和惯性融合的移动机器人定位方法。
本发明为解决上述技术问题所采取的技术方案是:
一种基于深度相机和惯性融合的移动机器人定位方法,所述方法具体包括以下步骤:
步骤1、读取当前来自深度相机的单目RGB图像,深度图像和IMU测量序列,再对单目RGB图像,深度图像和IMU测量序列进行时间同步;
所述步骤1的具体过程为:
以单目RGB图像的时间为基准,获得时间戳位于前一帧单目RGB图像到当前帧单目RGB图像时间内的IMU测量序列集合,在匹配深度图像时,若深度图像与当前帧单目RGB图像的时间相差在t0以内,则将当前帧单目RGB图像、获得的IMU测量序列集合以及深度图像打包成数据集合,并以深度惯性模式进行处理,即继续执行步骤2;否则,若深度图像与当前帧单目RGB图像的时间相差不小于t0,则去除当前的深度图像,并以单目惯性模式进行处理;
步骤2、对前一帧单目RGB图像和当前帧单目RGB图像进行下采样,获得下采样后的图像;再追踪前一帧下采样后图像的特征点在当前帧下采样后图像上的位置,获得各个特征点对;
步骤3、建立深度相机深度测量误差模型;
步骤4、将当前帧单目RGB图像作为滑动窗口的最后一帧,将滑窗内与当前帧单目RGB图像ck具有最大视差的关键帧记为c0,计算关键帧c0与当前帧ck之间的平移和旋转初值其中,为关键帧c0与当前帧之间的平移初值,为关键帧c0与当前帧之间的旋转初值;
步骤6、利用数据集合中的IMU测量序列得到滑窗内各关键帧之间的相对位姿变换;
步骤8、基于步骤3中建立的深度相机深度测量误差模型,对滑窗中所有的路标点进行联合深度估计,根据联合深度估计结果对路标点进行分类,根据分类结果对路标点进行筛选;
步骤9、根据筛选后的路标点建立点特征投影误差模型;
步骤10、建立平面约束误差模型;
步骤11、根据滑窗中各帧相对于世界坐标系的平移和旋转以及各帧相对于世界坐标系的速度,建立IMU预积分约束模型;
步骤12、根据步骤9至步骤11的模型建立全局优化目标函数,对全局优化目标函数进行求解得到滑窗内各帧的位姿估计结果。
本发明的有益效果是:
本发明利用了深度相机的深度不确定性模型和多视图三角化结合的方法,能够自适应地完成不同距离特征点的深度估计。并且在此基础上,提出了特征点动态分级管理方法,该方法结合跟踪次数和重投影误差动态地进行特征点分类并去除离群点。通过构建IMU预积分先验误差模型,平面运动约束,特征点深度约束以及3D-2D特征点重投影误差模型,利用多种约束联合实现移动机器人位姿的估计,提高了系统位姿估计的精度和鲁棒性,提高了对移动机器人的定位效果。
附图说明
图1是本发明方法的系统框图;
图2是测量数据时间同步原理图;
图3是移动机器人装置示意图;
图4是系统因子图结构;
图中,b0…bn代表IMU偏置状态,V0…Vn代表速度状态,T0…Tn代表位姿状态。
具体实施方式
具体实施方式一:结合图1说明本实施方式。本实施方式的一种基于深度相机和惯性融合的移动机器人定位方法,该方法具体包括以下步骤:
步骤1、读取当前来自深度相机(RGB-D)的单目RGB图像,深度图像和IMU测量序列,再对单目RGB图像,深度图像和IMU测量序列进行时间同步;
如图2所示,所述步骤1的具体过程为:
以单目RGB图像的时间为基准,获得时间戳位于前一帧单目RGB图像到当前帧单目RGB图像时间内的IMU测量序列集合,在匹配深度图像时,若深度图像与当前帧单目RGB图像的时间相差在t0以内(本发明中取为3ms),则将当前帧单目RGB图像、获得的IMU测量序列集合以及深度图像打包成数据集合,并以深度惯性模式进行处理,即继续执行步骤2;否则,若深度图像与当前帧单目RGB图像的时间相差不小于t0,则去除当前的深度图像,并以单目惯性模式进行处理;
步骤2、采用三层图像金字塔对前一帧单目RGB图像和当前帧单目RGB图像进行下采样,获得下采样后的图像;再追踪前一帧下采样后图像的特征点在当前帧下采样后图像上的位置,获得各个特征点对;
所述追踪前一帧下采样后图像的特征点在当前帧下采样后图像上的位置采用的是KLT稀疏光流法,若成功追踪的特征点数量小于设定的阈值,则采用Shi-Tomasi角点提取算法在当前帧下采样后的图像上提取新的特征点,使提取的新特征点与前一帧下采样后图像中未成功追踪的特征点组成特征点对,否则不需要处理。
采用Shi-Tomasi角点提取算法提取新的特征点使得总特征点数量保持在阈值之上。
步骤3、建立深度相机深度测量误差模型;
步骤4、将当前帧单目RGB图像作为滑动窗口的最后一帧,将滑窗内与当前帧单目RGB图像ck具有最大视差的关键帧记为c0,采用2D-2D对极约束结合RANSAC(随机采样一致)的方法计算关键帧c0与当前帧ck之间的平移和旋转初值其中,为关键帧c0与当前帧之间的平移初值,为关键帧c0与当前帧之间的旋转初值;
步骤6、利用数据集合中的IMU测量序列得到滑窗内各关键帧之间的相对位姿变换;
步骤7、利用相邻帧间纯视觉和IMU预积分得到的位姿变化一致的特性,将步骤5和步骤6的结果对齐(即,滑窗内帧间纯视觉所得到的位姿和相应的IMU预积分的值对齐),求得参考帧c0到世界坐标系的旋转矩阵以及各帧的速度值
步骤8、基于步骤3中建立的深度相机深度测量误差模型,对滑窗中所有的路标点进行联合深度估计,根据联合深度估计结果对路标点进行分类,根据分类结果对路标点进行筛选;
步骤9、根据筛选后的路标点建立点特征投影误差模型;
步骤10、考虑到移动机器人平面运动的特性如图3所示,建立平面约束误差模型;
步骤11、根据滑窗中各帧相对于世界坐标系的平移和旋转以及各帧相对于世界坐标系的速度,建立IMU预积分约束模型;
步骤12、根据步骤9至步骤11的模型建立全局优化目标函数,对全局优化目标函数进行求解得到滑窗内各帧的位姿估计结果。
本发明是一种利用RGB-D相机和惯性设备实现移动机器人定位的方法,能够实现GPS失效条件下如:地下、室内等复杂环境中高精度定位。本发明方法利用了深度相机深度测量误差模型,提出了自适应深度估计的方法,能够根据特征点深度测量的不同结合融合深度测量值和多视图三角化实现特征点的深度估计,使得该系统突破了RGBD-SLAM系统在仅在室内应用的限制,在室外也能有较好的效果。此外,针对地面机器人平面运动的特点,提出采用平面运动约束,特征点深度约束,3D-2D重投影约束,IMU预积分约束联合实现机器人的六自由度位姿估计,提高了系统位姿估计的精度和鲁棒性,可有效地应用于移动机器人的位姿估计。
具体实施方式二、本实施方式与具体实施方式一不同的是:所述深度相机深度测量误差模型为:
σz(z)=a1+a2z+a3z2
其中,z为当前特征点在深度图像上的深度测量值,σz(z)为当前特征点在深度图像上的深度测量值为z时的标准差,a1,a2,a3为待标定的系数;
所述系数a1,a2,a3的标定方式为:
将深度相机置于滑轨上,当深度相机与白墙的距离为L1时采集多张深度图像,再分别从采集的每张深度图像中截取出感兴趣区域,所述感兴趣区域在每张深度图像中的位置均相同;
对于感兴趣区域内某个位置的像素点,计算出该位置的像素点在各个感兴趣区域中的深度测量值的方差,再对感兴趣区域内全部位置上的像素点所对应的方差求平均,将得到的平均值作为标准差σ;再计算出位于感兴趣区域内最中心的像素点在各个感兴趣区域中的深度测量值的均值μ;
深度相机在滑轨上滑动,当深度相机与白墙的距离每增加ΔL时均采集到对应距离下的多张深度图像,直至深度相机与白墙的距离达到L2时停止,同理,分别计算出每个距离下的标准差和均值;
利用获得的各个距离下的标准差和均值对σ=a1+a2μ+a3μ2进行拟合,得到系数a1,a2,a3的值。
本实施方式中,所述的感兴趣区域均为对应的深度图像的中间部分,尺寸大小为450*360。L1的取值为0.3m,L2的取值为2.4m,ΔL的取值为0.1m,每个距离下采集的深度图像张数为100张。经过拟合得到标定后的参数a1=0.001087,a2=0.00058,a3=0.004034。
其它步骤及参数与具体实施方式一相同。
遍历当前帧单目RGB图像上的特征点,从当前帧单目RGB图像对应的深度图像获得当前帧单目RGB图像上每个特征点的深度测量值;
并建立如下的误差方程:
其中,代表从各次迭代更新后的平移和旋转值中选择出使误差方程取值最小的平移和旋转值S3D表示由深度测量值在3m以内的特征点对组成的集合(每个特征点对均利用当前帧图像上的一个特征点和关键帧图像上的一个特征点形成),S2D表示由全部特征点对组成的集合,rre表示重投影误差方程,rep表示对极约束误差方程,表示S3D中第l1对特征点在关键帧c0中的深度测量值,表示S3D中第l1对特征点在当前帧中的深度测量值,表示S3D中第l1对特征点对应的归一化特征点对,表示S2D中第l2对特征点对应的归一化特征点对,||·||代表范数,Ωre代表相机测量的重投影误差的协方差矩阵,Ωep代表相机测量的对极约束误差的协方差矩阵。
其它步骤及参数与具体实施方式一或二相同。
具体实施方式四、本实施方式与具体实施方式一至三之一不同的是:所述步骤5中,以关键帧c0作为参考帧,基于c0与当前帧间的位姿估计结果进行纯视觉位姿估计,得到滑窗内所有的3D路标点l1,...lm以及滑窗内其它各个关键帧相对于参考帧的视觉位姿估计结果其具体过程为:
步骤52、根据步骤51中计算出的坐标值以及关键帧c0与当前帧间具有共视关系的3D路标点在滑窗内其它关键帧的相机观测值(包括坐标和深度测量值),得到滑窗内其它关键帧相对于参考帧的位姿估计初值;
其它步骤及参数与具体实施方式一至三之一相同。
具体实施方式五、本实施方式与具体实施方式一至四之一不同的是:所述步骤6的具体过程为:
其中,和为第i个IMU时刻IMU测量得到的IMU坐标系下加速度和角速度,ba和bg为第i个IMU时刻的加速度计和陀螺仪的偏置,bk表示第k帧IMU坐标系,和分别为第i个IMU时刻相对于bk的位移预积分,速度预积分和旋转预积分,为当前时刻相对于bk的旋转矩阵,代表旋转运算符,δt为当前时刻与上一时刻的时间间隔,是从IMU陀螺仪得到的当前第i时刻到第i+1时刻的姿态变化估计值,和分别为第i+1个IMU时刻相对于bk的位移预积分,速度预积分和旋转预积分;
在理想情况下,视觉运动估计所得的旋转等于IMU预积分得到的旋转值,则有目标函数:
其中,为由视觉运动估计得到的bk+1相对于第c0帧IMU坐标系的旋转,为由视觉运动估计得到的bk相对于第c0帧IMU坐标系的旋转,bk+1表示第k+1帧IMU坐标系,为理想情况下IMU旋转预积分得到的bk+1相对于bk的旋转,B代表滑窗中的所有帧的序号,||·||代表范数;
经过化简和Cholesky分解得:
其中,上角标T代表矩阵的转置,上角标-1代表矩阵的逆;
求解出陀螺仪的偏置的变化值δbg后,利用新的偏置bg=bg+δbg带入到步骤6第一个公式中对IMU预积分值进行重新计算。
其它步骤及参数与具体实施方式一至四之一相同。
具体实施方式六、本实施方式与具体实施方式一至五之一不同的是:所述步骤8的具体过程为:
对于滑窗内的任意一个3D路标点,遍历该3D路标点在滑窗中每个关键帧上的测量值,测量值包括该3D路标点在相机归一化平面上的2D坐标和在深度图像上的深度测量值;
若该3D路标点在关键帧上存在深度测量值,且深度测量值小于3m,则根据步骤3建立的深度相机深度测量误差模型得到深度测量噪声方差将该3D路标点的2D像素坐标测量噪声方差记为则该3D路标点的三维测量协方差为:
其中,fx,fy表示相机x,y方向的焦距,cx,cy表示相机x,y方向的光心;
利用得到新的深度测量噪声方差将该3D路标点的深度测量值小于3m且重投影后2D观测误差小于设定阈值的N个观测投影到相机坐标系下,每个观测的投影记为di′,i=1,2,·,N,则该3D路标点在c0帧中的联合深度估计值为
若该3D路标点在滑窗内的跟踪次数大于等于4(即联合深度估计的次数大于等于4),则表明该3D路标点为稳健特征点,将该3D路标点归为A类,A类的3D路标点无需参与下一轮的联合深度估计;
若该3D路标点在滑窗内的跟踪次数小于4,则表明该3D路标点仍属新特征点,将该3D路标点归为B类,B类的3D路标点在下一循环仍需进行联合深度估计;
对于没有实现联合深度估计的3D路标点,根据共视帧的2D观测值采用多视图三角化的方法实现联合深度估计,对于三角化误差小于等于设定的阈值且跟踪次数大于等于4的3D路标点,将其归为A类,对于三角化误差小于等于设定阈值且跟踪次数小于4的3D路标点,则将其归为B类,对于三角化误差大于设定阈值的3D路标点,则直接将其归为C类,C类特征点为离群点不再参与后续优化,在本轮循环结束后被剔除。
下一循环是指下一帧到来时,在下一个滑窗内继续进行联合深度估计,将不符合条件的路标点被剔除后,获得筛选出的路标点。
其它步骤及参数与具体实施方式一至五之一相同。
具体实施方式七、本实施方式与具体实施方式一至六之一不同的是:所述步骤9的具体过程为:
对于筛选出的3D路标点lj,遍历在滑窗中所有共视帧中的观测值,若在当前帧ck中路标点lj的观测值包含2D像素坐标(uk,j,vk,j)和深度测量值dk,j,且深度测量值在3m以内,则建立带有深度约束的特征点投影误差模型:
其中,rk,j为3×1的误差向量,[·]1,2代表误差向量rk,j的前两行,[·]3代表误差向量的第三行,代表的第三行,π(·)表示相机投影方程,是的前两行,为3D路标点lj转换到ck帧相机坐标系下的坐标;
其中,r′k,j为2×1的误差向量。
其它步骤及参数与具体实施方式一至六之一相同。
具体实施方式八、本实施方式与具体实施方式一至七之一不同的是:所述步骤10的具体过程为:
移动机器人在进行平面运动时将会受到平面约束,即移动机器人的运动变化主要是x,y坐标以及偏航角θ。而移动机器人的z方向坐标,俯仰角和横滚角主要受到移动机器人负载和地形扰动的影响,相比于x,y方向变化较小。
将当前帧ck的相机坐标系在世界坐标系下的位姿转换到当前帧ck的IMU坐标系(原点为IMU的中心,x方向指向机器人右侧,y方向指向机器人下方,z方向指向机器人前进方向)在世界坐标系下的姿态再将转换为平面位姿即将其z方向坐标,俯仰角和横滚角归零,利用表示,则
则平面约束误差方程rplane为:
其中“∨”代表反对称矩阵到向量的变换。
考虑到移动机器人实际运动时其z方向,以及自身的俯仰角和横滚角并不始终为零,而是受到自身负载的变化,地面的崎岖不平所带来的扰动。因此,本系统的假设这三个变量分别受到δz,δr,δp为标准差的零均值高斯分布的影响,构建得到相应的协方差矩阵参与运算。
其它步骤及参数与具体实施方式一至七之一相同。
具体实施方式九、本实施方式与具体实施方式一至八之一不同的是:所述步骤11的具体过程为:
利用步骤6中第k帧和第k+1帧之间的IMU预积分值,可得第k帧和第k+1帧的位置、速度和旋转(PVQ),用于约束滑窗中帧间的位姿变化。构建IMU预积分约束用下式表示:
该残差模型由IMU预积分建立的数学模型得到,其中,为IMU预积分约束模型,为bk到bk+1间的预积分值,为bk到bk+1间的位移预积分,为bk到bk+1间的速度预积分,为的变化量,为的变化量,δba为ba的变化量,为待优化变量,其中,和分别为bk相对于世界坐标系的平移,速度和旋转值,为bk+1相对于世界坐标系的平移值,分别为bk+1相对于世界坐标系的速度值,和为bk中加速度计和陀螺仪的偏置,和为bk+1中加速度计和陀螺仪的偏置,为对应的四元数形式,为对应的四元数形式,分别为bk+1相对于世界坐标系的旋转值,[·]xyz代表四元数的x,y,z三项的值,Δtk为第k帧和第k+1帧间的时间差,gw为世界坐标系下的重力向量。
其它步骤及参数与具体实施方式一至八之一相同。
具体实施方式十、本实施方式与具体实施方式一至九之一不同的是:所述全局优化目标函数为:
其中,第一项为边缘化的先验信息,rp为边缘化残差,Hp为边缘化后的海森矩阵;第二项为IMU测量残差项,为IMU测量对应的协方差;第三项为带有深度测量约束的视觉重投影残差,为对应的协方差;第四项为不带深度测量的3D-2D视觉重投影残差,为对应的协方差;第五项rplane为平面约束残差项,为rplane对应的协方差;为滑窗中的3D路标点l在cj帧中的观测值;
系统的因子图模型如图4所示。通过对全局优化目标函数进行优化,得到滑窗内各帧的位姿估计结果。
其它步骤及参数与具体实施方式一至九之一相同。
本发明中的基于深度相机惯性融合的移动机器人定位方法,首先通过建立深度测量误差模型,利用多共视帧量测信息融合的方法提高了路标点深度估计精度。然后在初始化阶段采用优化的方法,利用对极约束和重投影约束利用多模态的特征点联合实现帧间位姿估计。建立了特征点分级管理系统,依据特征点在各帧的重投影误差和追踪次数,将特征点分为A、B和C类,并移除离群点。在全局优化模块中采用滑动窗口的方法构建了多模态特征点重投影约束,平面运动学约束,IMU预积分约束和边缘化先验约束,联合实现系统的高精度实时位姿估计,提高了系统的鲁棒性。
本发明的上述算例仅为详细地说明本发明的计算模型和计算流程,而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说,在上述说明的基础上还可以做出其它不同形式的变化或变动,这里无法对所有的实施方式予以穷举,凡是属于本发明的技术方案所引伸出的显而易见的变化或变动仍处于本发明的保护范围之列。
Claims (10)
1.一种基于深度相机和惯性融合的移动机器人定位方法,其特征在于,所述方法具体包括以下步骤:
步骤1、读取当前来自深度相机的单目RGB图像,深度图像和IMU测量序列,再对单目RGB图像,深度图像和IMU测量序列进行时间同步;
所述步骤1的具体过程为:
以单目RGB图像的时间为基准,获得时间戳位于前一帧单目RGB图像到当前帧单目RGB图像时间内的IMU测量序列集合,在匹配深度图像时,若深度图像与当前帧单目RGB图像的时间相差在t0以内,则将当前帧单目RGB图像、获得的IMU测量序列集合以及深度图像打包成数据集合,并以深度惯性模式进行处理,即继续执行步骤2;否则,若深度图像与当前帧单目RGB图像的时间相差不小于t0,则去除当前的深度图像,并以单目惯性模式进行处理;
步骤2、对前一帧单目RGB图像和当前帧单目RGB图像进行下采样,获得下采样后的图像;再追踪前一帧下采样后图像的特征点在当前帧下采样后图像上的位置,获得各个特征点对;
步骤3、建立深度相机深度测量误差模型;
步骤4、将当前帧单目RGB图像作为滑动窗口的最后一帧,将滑窗内与当前帧单目RGB图像ck具有最大视差的关键帧记为c0,计算关键帧c0与当前帧ck之间的平移和旋转初值其中,为关键帧c0与当前帧之间的旋转初值,为关键帧c0与当前帧之间的平移初值;
步骤6、利用数据集合中的IMU测量序列得到滑窗内各关键帧之间的相对位姿变换;
步骤8、基于步骤3中建立的深度相机深度测量误差模型,对滑窗中所有的路标点进行联合深度估计,根据联合深度估计结果对路标点进行分类,根据分类结果对路标点进行筛选;
步骤9、根据筛选后的路标点建立点特征投影误差模型;
步骤10、建立平面约束误差模型;
步骤11、根据滑窗中各帧相对于世界坐标系的平移和旋转以及各帧相对于世界坐标系的速度,建立IMU预积分约束模型;
步骤12、根据步骤9至步骤11的模型建立全局优化目标函数,对全局优化目标函数进行求解得到滑窗内各帧的位姿估计结果。
2.根据权利要求1所述的一种基于深度相机和惯性融合的移动机器人定位方法,其特征在于,所述深度相机深度测量误差模型为:
σz(z)=a1+a2z+a3z2
其中,z为当前特征点在深度图像上的深度测量值,σz(z)为当前特征点在深度图像上的深度测量值为z时的标准差,a1,a2,a3为待标定的系数;
所述系数a1,a2,a3的标定方式为:
将深度相机置于滑轨上,当深度相机与白墙的距离为L1时采集多张深度图像,再分别从采集的每张深度图像中截取出感兴趣区域,所述感兴趣区域在每张深度图像中的位置均相同;
对于感兴趣区域内某个位置的像素点,计算出该位置的像素点在各个感兴趣区域中的深度测量值的方差,再对感兴趣区域内全部位置上的像素点所对应的方差求平均,将得到的平均值作为标准差σ;再计算出位于感兴趣区域内最中心的像素点在各个感兴趣区域中的深度测量值的均值μ;
深度相机在滑轨上滑动,当深度相机与白墙的距离每增加ΔL时均采集到对应距离下的多张深度图像,直至深度相机与白墙的距离达到L2时停止,同理,分别计算出每个距离下的标准差和均值;
利用获得的各个距离下的标准差和均值对σ=a1+a2μ+a3μ2进行拟合,得到系数a1,a2,a3的值。
遍历当前帧单目RGB图像上的特征点,从当前帧单目RGB图像对应的深度图像获得当前帧单目RGB图像上每个特征点的深度测量值;
并建立如下的误差方程:
4.根据权利要求3所述的一种基于深度相机和惯性融合的移动机器人定位方法,其特征在于,所述步骤5中,以关键帧c0作为参考帧,基于c0与当前帧间的位姿估计结果进行纯视觉位姿估计,得到滑窗内所有的3D路标点l1,...lm以及滑窗内其它各个关键帧相对于参考帧的视觉位姿估计结果其具体过程为:
步骤52、根据步骤51中计算出的坐标值以及关键帧c0与当前帧间具有共视关系的3D路标点在滑窗内其它关键帧的相机观测值,得到滑窗内其它关键帧相对于参考帧的位姿估计初值;
5.根据权利要求4所述的一种基于深度相机和惯性融合的移动机器人定位方法,其特征在于,所述步骤6的具体过程为:
其中,和为第i个IMU时刻IMU测量得到的IMU坐标系下加速度和角速度,ba和bg为第i个IMU时刻的加速度计和陀螺仪的偏置,bk表示第k帧IMU坐标系,和分别为第i个IMU时刻相对于bk的位移预积分,速度预积分和旋转预积分,为当前时刻相对于bk的旋转矩阵,代表旋转运算符,δt为当前时刻与上一时刻的时间间隔,是从IMU陀螺仪得到的当前第i时刻到第i+1时刻的姿态变化估计值,和分别为第i+1个IMU时刻相对于bk的位移预积分,速度预积分和旋转预积分;
在理想情况下,视觉运动估计所得的旋转等于IMU预积分得到的旋转值,则有目标函数:
其中,为由视觉运动估计得到的bk+1相对于第c0帧IMU坐标系的旋转,为由视觉运动估计得到的bk相对于第c0帧IMU坐标系的旋转,bk+1表示第k+1帧IMU坐标系,为理想情况下IMU旋转预积分得到的bk+1相对于bk的旋转,B代表滑窗中的所有帧的序号,||·||代表范数;
经过化简和Cholesky分解得:
其中,上角标T代表矩阵的转置,上角标-1代表矩阵的逆;
求解出陀螺仪的偏置的变化值δbg后,利用新的偏置bg=bg+δbg对IMU预积分值进行重新计算。
6.根据权利要求5所述的一种基于深度相机和惯性融合的移动机器人定位方法,其特征在于,所述步骤8的具体过程为:
对于滑窗内的任意一个3D路标点,遍历该3D路标点在滑窗中每个关键帧上的测量值,测量值包括该3D路标点在相机归一化平面上的2D坐标和在深度图像上的深度测量值;
若该3D路标点在关键帧上存在深度测量值,且深度测量值小于3m,则根据步骤3建立的深度相机深度测量误差模型得到深度测量噪声方差将该3D路标点的2D像素坐标测量噪声方差记为则该3D路标点的三维测量协方差为:
其中,fx,fy表示相机x,y方向的焦距,cx,cy表示相机x,y方向的光心;
利用得到新的深度测量噪声方差将该3D路标点的深度测量值小于3m且重投影后2D观测误差小于设定阈值的N个观测投影到相机坐标系下,每个观测的投影记为d′i,i=1,2,·,N,则该3D路标点在c0帧中的联合深度估计值为
若该3D路标点在滑窗内的跟踪次数大于等于4,则表明该3D路标点为稳健特征点,将该3D路标点归为A类,A类的3D路标点无需参与下一轮的联合深度估计;
若该3D路标点在滑窗内的跟踪次数小于4,则表明该3D路标点仍属新特征点,将该3D路标点归为B类,B类的3D路标点在下一循环仍需进行联合深度估计;
对于没有实现联合深度估计的3D路标点,根据共视帧的2D观测值采用多视图三角化的方法实现联合深度估计,对于三角化误差小于等于设定的阈值且跟踪次数大于等于4的3D路标点,将其归为A类,对于三角化误差小于等于设定阈值且跟踪次数小于4的3D路标点,则将其归为B类,对于三角化误差大于设定阈值的3D路标点,则直接将其归为C类,C类特征点为离群点不再参与后续优化,在本轮循环结束后被剔除。
7.根据权利要求6所述的一种基于深度相机和惯性融合的移动机器人定位方法,其特征在于,所述步骤9的具体过程为:
对于筛选出的3D路标点lj,遍历在滑窗中所有共视帧中的观测值,若在当前帧ck中路标点lj的观测值包含2D像素坐标(uk,j,vk,j)和深度测量值dk,j,且深度测量值在3m以内,则建立带有深度约束的特征点投影误差模型:
其中,rk,j为3×1的误差向量,[·]1,2代表误差向量rk,j的前两行,[·]3代表误差向量的第三行,代表的第三行,π(·)表示相机投影方程,是的前两行,为3D路标点lj转换到ck帧相机坐标系下的坐标;
其中,r′k,j为2×1的误差向量。
9.根据权利要求8所述的一种基于深度相机和惯性融合的移动机器人定位方法,其特征在于,所述步骤11的具体过程为:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211119147.3A CN115371665B (zh) | 2022-09-13 | 2022-09-13 | 一种基于深度相机和惯性融合的移动机器人定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211119147.3A CN115371665B (zh) | 2022-09-13 | 2022-09-13 | 一种基于深度相机和惯性融合的移动机器人定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN115371665A true CN115371665A (zh) | 2022-11-22 |
CN115371665B CN115371665B (zh) | 2023-06-23 |
Family
ID=84072528
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211119147.3A Active CN115371665B (zh) | 2022-09-13 | 2022-09-13 | 一种基于深度相机和惯性融合的移动机器人定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115371665B (zh) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115657062A (zh) * | 2022-12-27 | 2023-01-31 | 理工雷科智途(北京)科技有限公司 | 一种设备快速重定位方法、装置及存储介质 |
CN117421384A (zh) * | 2023-10-24 | 2024-01-19 | 哈尔滨理工大学 | 基于共视投影匹配的视觉惯性slam系统滑窗优化方法 |
CN117705107A (zh) * | 2024-02-06 | 2024-03-15 | 电子科技大学 | 基于两阶段稀疏舒尔补的面向视觉惯性定位方法 |
CN118244785A (zh) * | 2024-05-28 | 2024-06-25 | 浙江大学湖州研究院 | 空地双模的两栖无人机及其定位方法、装置、存储介质 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110345944A (zh) * | 2019-05-27 | 2019-10-18 | 浙江工业大学 | 融合视觉特征和imu信息的机器人定位方法 |
CN111288989A (zh) * | 2020-02-25 | 2020-06-16 | 浙江大学 | 一种小型无人机视觉定位方法 |
CN112240768A (zh) * | 2020-09-10 | 2021-01-19 | 西安电子科技大学 | 基于Runge-Kutta4改进预积分的视觉惯导融合SLAM方法 |
CN114529576A (zh) * | 2022-01-04 | 2022-05-24 | 重庆邮电大学 | 一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法 |
-
2022
- 2022-09-13 CN CN202211119147.3A patent/CN115371665B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110345944A (zh) * | 2019-05-27 | 2019-10-18 | 浙江工业大学 | 融合视觉特征和imu信息的机器人定位方法 |
CN111288989A (zh) * | 2020-02-25 | 2020-06-16 | 浙江大学 | 一种小型无人机视觉定位方法 |
CN112240768A (zh) * | 2020-09-10 | 2021-01-19 | 西安电子科技大学 | 基于Runge-Kutta4改进预积分的视觉惯导融合SLAM方法 |
CN114529576A (zh) * | 2022-01-04 | 2022-05-24 | 重庆邮电大学 | 一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法 |
Non-Patent Citations (1)
Title |
---|
郑新芳: "视觉惯导融合实时6DOF机器人定位方法研究" * |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115657062A (zh) * | 2022-12-27 | 2023-01-31 | 理工雷科智途(北京)科技有限公司 | 一种设备快速重定位方法、装置及存储介质 |
CN117421384A (zh) * | 2023-10-24 | 2024-01-19 | 哈尔滨理工大学 | 基于共视投影匹配的视觉惯性slam系统滑窗优化方法 |
CN117705107A (zh) * | 2024-02-06 | 2024-03-15 | 电子科技大学 | 基于两阶段稀疏舒尔补的面向视觉惯性定位方法 |
CN117705107B (zh) * | 2024-02-06 | 2024-04-16 | 电子科技大学 | 基于两阶段稀疏舒尔补的面向视觉惯性定位方法 |
CN118244785A (zh) * | 2024-05-28 | 2024-06-25 | 浙江大学湖州研究院 | 空地双模的两栖无人机及其定位方法、装置、存储介质 |
Also Published As
Publication number | Publication date |
---|---|
CN115371665B (zh) | 2023-06-23 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111024066B (zh) | 一种无人机视觉-惯性融合室内定位方法 | |
CN108827315B (zh) | 基于流形预积分的视觉惯性里程计位姿估计方法及装置 | |
CN108717712B (zh) | 一种基于地平面假设的视觉惯导slam方法 | |
CN111258313B (zh) | 多传感器融合slam系统及机器人 | |
CN115371665B (zh) | 一种基于深度相机和惯性融合的移动机器人定位方法 | |
Panahandeh et al. | Vision-aided inertial navigation based on ground plane feature detection | |
CN113052908B (zh) | 一种基于多传感器数据融合的移动机器人位姿估计算法 | |
CN112254729B (zh) | 一种基于多传感器融合的移动机器人定位方法 | |
CN107909614B (zh) | 一种gps失效环境下巡检机器人定位方法 | |
CN111795686A (zh) | 一种移动机器人定位与建图的方法 | |
CN112815939B (zh) | 移动机器人的位姿估计方法及计算机可读存储介质 | |
CN114526745A (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
CN110533719A (zh) | 基于环境视觉特征点识别技术的增强现实定位方法及装置 | |
CN114964276A (zh) | 一种融合惯导的动态视觉slam方法 | |
CN114608554A (zh) | 一种手持slam设备以及机器人即时定位与建图方法 | |
Xian et al. | Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach | |
CN112945233B (zh) | 一种全局无漂移的自主机器人同时定位与地图构建方法 | |
CN110108894A (zh) | 一种基于相位相关和光流法的多旋翼速度测量方法 | |
CN112907633A (zh) | 动态特征点识别方法及其应用 | |
Panahandeh et al. | Vision-aided inertial navigation using planar terrain features | |
CN112731503A (zh) | 一种基于前端紧耦合的位姿估计方法及系统 | |
CN112179373A (zh) | 一种视觉里程计的测量方法及视觉里程计 | |
CN110136168B (zh) | 一种基于特征点匹配和光流法的多旋翼速度测量方法 | |
CN115930948A (zh) | 一种果园机器人融合定位方法 | |
CN116380079A (zh) | 一种融合前视声呐与orb-slam3的水下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 |