CN113436260B - 基于多传感器紧耦合的移动机器人位姿估计方法和系统 - Google Patents
基于多传感器紧耦合的移动机器人位姿估计方法和系统 Download PDFInfo
- Publication number
- CN113436260B CN113436260B CN202110702106.6A CN202110702106A CN113436260B CN 113436260 B CN113436260 B CN 113436260B CN 202110702106 A CN202110702106 A CN 202110702106A CN 113436260 B CN113436260 B CN 113436260B
- Authority
- CN
- China
- Prior art keywords
- point
- point cloud
- plane
- edge
- points
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 49
- 230000008878 coupling Effects 0.000 title claims abstract description 23
- 238000010168 coupling process Methods 0.000 title claims abstract description 23
- 238000005859 coupling reaction Methods 0.000 title claims abstract description 23
- 230000000007 visual effect Effects 0.000 claims abstract description 35
- 238000005457 optimization Methods 0.000 claims abstract description 15
- 230000008569 process Effects 0.000 claims abstract description 6
- 230000009466 transformation Effects 0.000 claims description 12
- 238000012216 screening Methods 0.000 claims description 5
- 238000010276 construction Methods 0.000 claims description 4
- 230000033001 locomotion Effects 0.000 abstract description 14
- 238000004422 calculation algorithm Methods 0.000 description 9
- 230000004927 fusion Effects 0.000 description 8
- 239000011159 matrix material Substances 0.000 description 8
- 238000004364 calculation method Methods 0.000 description 5
- 238000005259 measurement Methods 0.000 description 5
- 230000006872 improvement Effects 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 239000000428 dust Substances 0.000 description 1
- 230000008030 elimination Effects 0.000 description 1
- 238000003379 elimination reaction Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 239000012634 fragment Substances 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 238000005295 random walk Methods 0.000 description 1
- 238000010845 search algorithm Methods 0.000 description 1
- 230000011218 segmentation Effects 0.000 description 1
- 238000000638 solvent extraction Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000013519 translation 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
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/15—Correlation function computation including computation of convolution operations
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
- G06T17/05—Geographic models
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T5/00—Image enhancement or restoration
- G06T5/80—Geometric correction
-
- 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/10—Image acquisition modality
- G06T2207/10032—Satellite or aerial image; Remote sensing
- G06T2207/10044—Radar image
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Data Mining & Analysis (AREA)
- Mathematical Analysis (AREA)
- Software Systems (AREA)
- Mathematical Physics (AREA)
- Pure & Applied Mathematics (AREA)
- Computational Mathematics (AREA)
- Mathematical Optimization (AREA)
- Databases & Information Systems (AREA)
- Algebra (AREA)
- Geometry (AREA)
- General Engineering & Computer Science (AREA)
- Life Sciences & Earth Sciences (AREA)
- Bioinformatics & Computational Biology (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Evolutionary Biology (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Probability & Statistics with Applications (AREA)
- Computing Systems (AREA)
- Operations Research (AREA)
- Remote Sensing (AREA)
- Computer Graphics (AREA)
- Length Measuring Devices By Optical Means (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种基于多传感器紧耦合的移动机器人位姿估计方法和系统，属于机器人定位技术领域，其中方法包括：对相机采集的当前帧RGB‑D图像与上一帧RGB‑D图像进行特征匹配，计算匹配过程的视觉重投影误差；对IMU测量的数据进行积分，构建IMU积分残差；从激光雷达采集的点云中提取边缘特征点和平面特征点，计算边缘特征点到边缘线的距离以及平面特征点到平面的距离，构建激光点云几何残差；以视觉重投影误差、IMU积分残差和激光点云几何残差最小为目标，进行位姿估计，得到局部位姿；使用局部位姿更新激光点云地图后在激光点云地图上进行全局优化，得到全局位姿。本发明提高了移动机器人在复杂运动和复杂环境下的定位精度和鲁棒性。
Description
技术领域
本发明属于机器人定位技术领域，更具体地，涉及一种基于多传感器紧耦合的移动机器人位姿估计方法和系统。
背景技术
在移动机器人领域，常使用视觉传感器、IMU、激光雷达中的一种或多种的融合作为实际SLAM应用算法。利用相机(单目、双目或深度)作为单一传感器的视觉SLAM方法受环境光照影响大，并且在纹理稀疏的环境下很难检测到有效的特征而造成定位缺失。利用激光雷达(2D或3D)作为单一传感器的激光SLAM方法测量频率低，并且激光雷达在运动速度变化大时存在难以矫正的运动失真。而IMU传感器是根据加速度和角速度积分来进行运动估计的，在短时间快速运动下可以提供较准确的运动估计。
将IMU和相机或激光雷达结合可以在一定程度上提高SLAM位姿估计的准确性和鲁棒性。但是视觉-IMU系统仍然无法在复杂、弱纹理的环境中长时间正常工作，激光-IMU系统在雾霾、灰尘环境下仍存在较大误差，而视觉-IMU-激光雷达系统现有尝试较少，且大都通过松耦合方式结合，无法同时发挥不同传感器的优势。
由此可见，现有机器人定位技术在快速运动和不同复杂环境下存在定位精度低、鲁棒性差的技术问题。
发明内容
针对现有技术的以上缺陷或改进需求，本发明提供了一种基于多传感器紧耦合的移动机器人位姿估计方法和系统，由此解决现有机器人定位技术在快速运动和不同复杂环境下存在定位精度低、鲁棒性差的技术问题。
为实现上述目的，按照本发明的一个方面，提供了一种基于多传感器紧耦合的移动机器人位姿估计方法，所述多传感器包括相机、IMU和激光雷达，所述方法包括：
对相机采集的当前帧RGB-D图像与上一帧RGB-D图像进行特征匹配，计算匹配过程的视觉重投影误差；
对IMU测量的数据进行积分，构建IMU积分残差；
从激光雷达采集的点云中提取边缘特征点和平面特征点，计算边缘特征点到边缘线的距离以及平面特征点到平面的距离，构建激光点云几何残差；
以视觉重投影误差、IMU积分残差和激光点云几何残差最小为目标，进行位姿估计，得到局部位姿；
使用局部位姿更新激光点云地图后在激光点云地图上进行全局优化，得到全局位姿。
进一步地，所述激光点云几何残差的具体构建方式为：
通过计算点云的局部曲率值，从激光雷达采集的点云中筛选边缘特征点和平面特征点，得到边缘特征点集合和平面特征点集合；
从当前次扫描得到的边缘特征点集合中选取一个边缘特征点，从上一次扫描得到的边缘特征点集合中选取两个边缘特征点组成边缘线；
从当前次扫描得到的平面特征点集合中选取一个平面特征点，从上一次扫描得到的平面特征点集合中选取三个平面特征点组成平面；
通过连续两次扫描的点云位姿变换关系计算边缘特征点和平面特征点的坐标，进而计算边缘特征点到边缘线的距离以及平面特征点到平面的距离，得到3D激光点云几何残差。
进一步地，所述边缘特征点集合与平面特征点集合的构建包括：
将激光雷达在某一时刻垂直切面扫描得到的点云作为线扫集合，计算线扫集合中点云的局部曲率值，将线扫集合中局部曲率值最大的两个点作为边缘特征点，将线扫集合中局部曲率值最小的四个点作为平面特征点；
将激光雷达在某一次扫描中所有线扫集合中的边缘特征点组合，构建边缘特征点集合，将激光雷达在某一次扫描中所有线扫集合中的平面特征点组合，构建平面特征点集合。
进一步地，所述边缘特征点到边缘线的距离通过如下方式计算：
从当前次扫描得到的边缘特征点集合中选取一个边缘特征点i0，在选上一次扫描得到的边缘特征点集合中选取与i0最近的边缘特征点j0，在上一次扫描得到的边缘特征点集合中选取与j0相邻扫描线中最近的边缘特征点l0，将j0和l0的连线作为边缘线，计算边缘特征点i0到边缘线的距离。
进一步地，所述平面特征点到平面的距离通过如下方式计算：
从当前次扫描得到的平面特征点集合中选取一个平面特征点i，在选上一次扫描得到的平面特征点集合中选取与i最近的平面特征点j，在上一次扫描得到的平面特征点集合中选取与j相同线扫中最近的平面特征点l，找寻相邻帧中与点j最近的平面特征点m，计算平面特征点i到j、l和m形成的平面的距离。
进一步地，所述激光雷达每次扫描一周得到的点云为得到一帧图像的点云，激光雷达每次扫描时，将得到的所有点重投影到每一帧的初始时刻。
进一步地，所述方法还包括：
采用如下方式对视觉重投影误差进行优化：
其中，r为视觉重投影误差，ρ(r)为优化后的视觉重投影误差。
进一步地，所述全局位姿估计包括：
利用激光雷达采集的点云构建激光点云地图，将使用局部位姿计算的地图点添加至激光点云地图中以更新激光点云地图；
以新增地图点对应的激光点云几何残差最小为目的，对激光点云地图中的新增地图点进行调整，调整后的地图点对应的位姿为最终的全局位姿。
进一步地，所述方法还包括：
利用相机采集的RGB-D图像构建RGB-D稠密点云地图，将使用局部位姿计算的地图点添加至RGB-D稠密点云地图中以更新RGB-D稠密点云地图；
以新增地图点对应的重投影误差最小为目的，对RGB-D稠密点云地图中的新增地图点进行调整，得到全局优化后的RGB-D稠密点云地图，用于进行路径规划。
按照本发明的另一方面，提供了一种基于多传感器紧耦合的移动机器人位姿估计系统，包括：
相机，用于采集RGB-D图像；所述相机数量为多个，分布于移动机器人的前后左右；
IMU，用于测量数据；
激光雷达，用于采集的点云；
局部位姿估计，用于对相机采集的当前帧RGB-D图像与上一帧RGB-D图像进行特征匹配，计算匹配过程的视觉重投影误差；对IMU测量的数据进行积分，构建IMU积分残差；从激光雷达采集的点云中提取边缘特征点和平面特征点，计算边缘特征点到边缘线的距离以及平面特征点到平面的距离，构建激光点云几何残差；以视觉重投影误差、IMU积分残差和激光点云几何残差最小为目标，进行位姿估计，得到局部位姿；
全局位姿估计，用于使用局部位姿更新激光点云地图后在激光点云地图上进行全局优化，得到全局位姿。
总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：
(1)本发明提出一种视觉惯性紧耦合的位姿估计方法。首先通过视觉数据构建重投影误差，然后通过对IMU测量的数据进行积分，构建IMU积分残差，弥补视觉特征缺失和机器人快速移动问题，最后结合重投影误差和IMU测量积分残差构建视觉惯性联合残差模型，提高了机器人位姿估计的鲁棒性。将激光雷达几何残差融合到视觉惯性紧耦合系统中，提出一种多传感器紧耦合的SLAM位姿估计方法。通过激光雷达为机器人提供更加精确的环境位置信息，计算边缘特征点到边缘线的距离以及平面特征点到平面的距离，构建激光点云几何残差，并将该几何约束添加到视觉-惯性里程计优化联合残差中，进一步提高了移动机器人在复杂环境下位姿估计的精确度和鲁棒性。
(2)采用点云的局部曲率值筛选边缘特征点和平面特征点，激光雷达每次扫描一周得到的点云可以组成一帧图像，分别从前后两次扫描的点云中选取特征点，进而计算点线距离和点面距离，因此，激光点云几何残差充分考虑了当前帧与前一帧的激光点云的特征匹配，将该几何约束添加到视觉-惯性里程计优化联合残差中，使用图像和激光点云联合的场景识别方法，进一步提高了闭环检测的鲁棒性。
(3)由于点云扫描一周时，机器人仍在运动，导致点云数据存在畸变，故使用轮式里程计进行运动畸变校正，将所有的点重投影到每一帧的初始时刻。对重投影误差进行优化，主要是为了避免重投影误差在计算时出现较大误差，而该较大误差会主导了联合残差的优化方向，导致联合残差优化时间过长，或者优化失败。
(4)本发明更新激光点云地图的目的，是为了通过激光雷达地图的全局优化得到比RGB-D稠密点云地图全局BA优化更精确的全局位姿。通过局部位姿添加地图点、全局位姿用来微调地图点，最终得到的RGB-D稠密点云地图精确度高，用于进行路径规划可以准确避障。
(5)本发明中相机数量为多个，分布于移动机器人的前后左右，这样可以组成360度环视相机系统，这样与360度的3D激光雷达，在感知范围上都是360度，可以更好的进行多个相机与3D激光雷达的多传感器融合，提高移动机器人位姿估计精度。现有方法往往仅有一个前视相机，与360度的3D激光雷达进行多传感器融合计算，并不是360度的融合，这样在相机覆盖不到的区域，实际上仅有3D激光雷达和IMU传感器，不利于提高位姿估计精度。同时，对于可以正反双向运动的移动机器人，传统的单一前视相机在前进时，可以利于前视相机的视觉信息进行融合，但在移动机器人倒退时，缺少行进方向的视觉信息，不利于提高位姿估计精度。
附图说明
图1是本发明实施例提供的一种基于多传感器紧耦合的移动机器人位姿估计方法的流程图；
图2中(a)是本发明实施例提供的点线距离计算原理图；
图2中(b)是本发明实施例提供的点面距离计算原理图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
一种基于多传感器紧耦合的移动机器人位姿估计方法，所述多传感器包括相机、IMU和激光雷达，所述方法包括：
对相机采集的当前帧RGB-D图像与上一帧RGB-D图像进行特征匹配，计算匹配过程的视觉重投影误差；
对IMU测量的数据进行积分，构建IMU积分残差；
从激光雷达采集的点云中提取边缘特征点和平面特征点，计算边缘特征点到边缘线的距离以及平面特征点到平面的距离，构建激光点云几何残差；
以视觉重投影误差、IMU积分残差和激光点云几何残差最小为目标，进行位姿估计，得到局部位姿；
使用局部位姿更新激光点云地图后在激光点云地图上进行全局优化，得到全局位姿。
基于单一传感器的SLAM算法均存在各种各样的不足，本发明提出了一种紧耦合的多传感器融合的SLAM算法，使用图像和激光点云联合的场景识别方法，提高了位姿估计闭环检测的鲁棒性。
本发明相机的数量为多个(优选的为4个)，分布在移动机器人的前后左右，组成360度环视相机系统。这样与360度的3D激光雷达，在感知范围上都是360度，可以更好的进行多个相机与3D激光雷达的多传感器融合，提高移动机器人位姿估计精度。
视觉重投影误差的计算包括：
采用基于测量数据构建移动机器人位姿和地图点位姿的最大后验概率估计再转为最小二乘优化问题的方法，对图像特征匹配进行重投影误差的计算，采用Huber内核对误差进行优化。
具体地，Eproj视觉重投影误差表示为：
其中，xk是图像上的像素点位置，
是像素点在相机坐标下的转换坐标，j表示当前帧，k表示当前帧与上一帧的匹配对，∑k是和像素点尺度关联的信息矩阵。
具体地，式(3)中
表示为：
其中[fu，fv]T是x，y轴上的焦距，[Cu，Cv]T是x，y轴上的主点。
采用Huber函数对视觉重投影误差进行优化：
其中，r为视觉重投影误差，ρ(r)为优化后的视觉重投影误差。
采用片段式积分方法，在视觉采集的每两帧之间利用IMU对机器人旋转、速度、位置和误差进行积分并构建最小二乘残差，作为IMU积分残差。
IMU积分误差EIMU表示为：
其中，i表示上一帧，旋转、速度、位置和误差测量为ΔRi，j、Δvi，j、ΔPi，j和Δbi，j。∑I为IMU预积分的信息矩阵，∑R是IMU的零点随机游走，pi和pj为第i帧和第j帧对应的3D观测点。EIMU(i，j)表示在第i帧和第j帧之间的IMU积分误差，ρ为Huber函数，
和
是第i帧和第j帧的机器人旋转矩阵，r为误差项，vi和vj为第i帧和第j帧的机器人速度，Δt为第i帧和第j帧的时间差，g为重力加速度，bi和bj为第i帧和第j帧的IMU误差测量。
进一步地，所述激光点云几何残差的具体构建方式为：
通过计算点云的局部曲率值，从激光雷达采集的点云中筛选边缘特征点和平面特征点，得到边缘特征点集合和平面特征点集合；
从当前次扫描得到的边缘特征点集合中选取一个边缘特征点，从上一次扫描得到的边缘特征点集合中选取两个边缘特征点组成边缘线；
从当前次扫描得到的平面特征点集合中选取一个平面特征点，从上一次扫描得到的平面特征点集合中选取三个平面特征点组成平面；
通过连续两次扫描的点云位姿变换关系计算边缘特征点和平面特征点的坐标，进而计算边缘特征点到边缘线的距离以及平面特征点到平面的距离，得到3D激光点云几何残差。
进一步地，所述边缘特征点集合与平面特征点集合的构建包括：
将激光雷达在某一时刻垂直切面扫描得到的点云作为线扫集合，计算线扫集合中点云的局部曲率值，将线扫集合中局部曲率值最大的两个点作为边缘特征点，将线扫集合中局部曲率值最小的四个点作为平面特征点；
将激光雷达在某一次扫描中所有线扫集合中的边缘特征点组合，构建边缘特征点集合，将激光雷达在某一次扫描中所有线扫集合中的平面特征点组合，构建平面特征点集合。
具体地，在实际计算中取线扫集合内当前点和前后五个点来计算局部曲率值。
如图2中(a)所示，所述边缘特征点到边缘线的距离通过如下方式计算：
从当前次扫描得到的边缘特征点集合中选取一个边缘特征点i0，在选上一次扫描得到的边缘特征点集合中选取与i0最近的边缘特征点j0，在上一次扫描得到的边缘特征点集合中选取与j0相邻扫描线中最近的边缘特征点l0，将j0和l0的连线作为边缘线，计算边缘特征点i0到边缘线的距离dE。
如图2中(b)所示，所述平面特征点到平面的距离通过如下方式计算：
从当前次扫描得到的平面特征点集合中选取一个平面特征点i，在选上一次扫描得到的平面特征点集合中选取与i最近的平面特征点j，在上一次扫描得到的平面特征点集合中选取与j相同线扫中最近的平面特征点l，找寻相邻帧中与点j最近的平面特征点m，计算平面特征点i到j、l和m形成的平面的距离dH。
进一步地，所述激光雷达每次扫描一周得到的点云为得到一帧图像的点云，激光雷达每次扫描时，将得到的所有点重投影到每一帧的初始时刻。
连续两次扫描的点云位姿变换关系通过如下方式获取：
假设激光雷达做的匀速运动，根据线性插值就可以获得任意一帧中点云相对于上一帧的位姿变换
进一步地，用旋转矩阵和平移向量描述当前帧与上一帧的位姿变换，为了简化旋转矩阵的求导，用罗德里杰斯公式对旋转矩阵进行展开，得到简化后的位姿变换公式。
将简化后的位姿变换公式作为目标函数，计算目标函数的雅克比矩阵，为了优化求解这个目标函数，在非线性最小二乘LM(Levenberg-Marquardt)算法的基础上加入半径为μ的信赖区域，对目标函数进行优化，然后构建拉格朗日函数，求得最终的位姿变换关系。
在局部位姿估计时，联合视觉重投影误差、IMU积分残差和激光点云几何残差构建最小二乘优化问题，采用g2o中的高斯-牛顿算法对该问题进行优化，得到移动机器人的局部运动位姿估计。
联合残差优化函数为：
其中，∑k是和像素点尺度关联的信息矩阵，Eproj是视觉重投影误差，i表示上一帧，j表示当前帧，k表示匹配对；EIMU表示在第i帧和第j帧之间的IMU积分误差，ELidar(i，j)表示激光点云几何残差。
优选地，本发明还可以通过去除3D点云中动态目标，来提高激光雷达的精确度。
激光雷达采集点云的预处理：先采用点云直通滤波和体素滤波算法，消除远距离点云或零星分布的小点云簇，提高算法执行效率。然后采用欧式聚类分割算法将余下的点云聚类成一个个的点云簇。在点云聚类的基础上，计算出每簇点云的最小包围长方体，将满足长、宽、高阈值范围内并且和3D激光雷达距离在一定范围内的点云簇判定为动态目标(在此主要考虑人为动态目标，故选取正常人的长、宽、高标准值为阈值进行剔除)，将这部分点云剔除后保留静态的场景点云用作后续的处理，提高了后续的点云特征提取与匹配的效率，增加了3D激光里程计的精确度。
欧式聚类分割算法：对于某一帧采集到的3D点云，假设点的坐标为(x，y，z)，设定聚类距离阈值d为：
其中α和β是人工调整参数，根据实际点云分布情况予以调整。根据kd-tree算法(k-dimensional树)，在待聚类点云簇S中随机选择一点p，将p的k领域点云和阈值d比较，将满足条件的点加入S中。在S中继续处理除p点以外的点，如此重复，当S无法加入新的点或S中点云数目低于设定阈值便完成了聚类操作，得到一个个点云簇，这些点云簇已可以大致描述物体的形状。
进一步地，所述全局位姿估计包括：
利用激光雷达采集的点云构建激光点云地图，将使用局部位姿计算的地图点添加至激光点云地图中以更新激光点云地图；
以新增地图点对应的激光点云几何残差最小为目的，对激光点云地图中的新增地图点进行调整，调整后的地图点对应的位姿为全局位姿。
进一步地，所述方法还包括：
利用相机采集的RGB-D图像构建RGB-D稠密点云地图，将使用局部位姿计算的地图点添加至RGB-D稠密点云地图中以更新RGB-D稠密点云地图；
以新增地图点对应的重投影误差最小为目的，对RGB-D稠密点云地图中的新增地图点进行调整，得到全局优化后的RGB-D稠密点云地图，用于进行路径规划。
在3D激光雷达的处理中，采用轮式里程计校正3D激光雷达的运动畸变，采用计算点云的局部曲率值并筛选的方法来提取点云的边缘特征和面特征，采用scan-to-scan的方法来实现3D激光点云帧与帧之间的特征匹配，采用kd-tree最近邻搜索算法找出激光点云每帧边缘特征点或平面特征点，采用罗德里杰斯公式对连续两次扫描的点云位姿变换关系进行简化，利用简化后的位姿变换式和提取的特征点构建点线或点面距离函数，融合得到3D激光点云几何残差，利用拉格朗日函数构建法对距离函数进行化简求导，优化相邻点云位姿变换。本发明将视觉重投影残差、IMU积分残差和激光点云几何残差进行融合，实现了移动机器人的高精度定位，提高了移动机器人在复杂运动和复杂环境下的定位精度和鲁棒性。
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。
Claims (10)
1.一种基于多传感器紧耦合的移动机器人位姿估计方法，所述多传感器包括相机、IMU和激光雷达，其特征在于，所述方法包括：
对相机采集的当前帧RGB-D图像与上一帧RGB-D图像进行特征匹配，计算匹配过程的视觉重投影误差；
对IMU测量的数据进行积分，构建IMU积分残差；
从激光雷达采集的点云中提取边缘特征点和平面特征点，计算边缘特征点到边缘线的距离以及平面特征点到平面的距离，构建激光点云几何残差；
以视觉重投影误差、IMU积分残差和激光点云几何残差最小为目标，进行位姿估计，得到局部位姿；
使用局部位姿更新激光点云地图后在激光点云地图上进行全局优化，得到全局位姿。
2.如权利要求1所述的一种基于多传感器紧耦合的移动机器人位姿估计方法，其特征在于，所述激光点云几何残差的具体构建方式为：
通过计算点云的局部曲率值，从激光雷达采集的点云中筛选边缘特征点和平面特征点，得到边缘特征点集合和平面特征点集合；
从当前次扫描得到的边缘特征点集合中选取一个边缘特征点，从上一次扫描得到的边缘特征点集合中选取两个边缘特征点组成边缘线；
从当前次扫描得到的平面特征点集合中选取一个平面特征点，从上一次扫描得到的平面特征点集合中选取三个平面特征点组成平面；
通过连续两次扫描的点云位姿变换关系计算边缘特征点和平面特征点的坐标，进而计算边缘特征点到边缘线的距离以及平面特征点到平面的距离，得到激光点云几何残差。
3.如权利要求2所述的一种基于多传感器紧耦合的移动机器人位姿估计方法，其特征在于，所述边缘特征点集合与平面特征点集合的构建包括：
将激光雷达在某一时刻垂直切面扫描得到的点云作为线扫集合，计算线扫集合中点云的局部曲率值，将线扫集合中局部曲率值最大的两个点作为边缘特征点，将线扫集合中局部曲率值最小的四个点作为平面特征点；
将激光雷达在某一次扫描中所有线扫集合中的边缘特征点组合，构建边缘特征点集合，将激光雷达在某一次扫描中所有线扫集合中的平面特征点组合，构建平面特征点集合。
4.如权利要求2所述的一种基于多传感器紧耦合的移动机器人位姿估计方法，其特征在于，所述边缘特征点到边缘线的距离通过如下方式计算：
从当前次扫描得到的边缘特征点集合中选取一个边缘特征点i0，在选上一次扫描得到的边缘特征点集合中选取与i0最近的边缘特征点j0，在上一次扫描得到的边缘特征点集合中选取与j0相邻扫描线中最近的边缘特征点l0，将j0和l0的连线作为边缘线，计算边缘特征点i0到边缘线的距离。
5.如权利要求2所述的一种基于多传感器紧耦合的移动机器人位姿估计方法，其特征在于，所述平面特征点到平面的距离通过如下方式计算：
从当前次扫描得到的平面特征点集合中选取一个平面特征点i，在选上一次扫描得到的平面特征点集合中选取与i最近的平面特征点j，在上一次扫描得到的平面特征点集合中选取与j相同线扫中最近的平面特征点l，找寻相邻帧中与点j最近的平面特征点m，计算平面特征点i到j、l和m形成的平面的距离。
6.如权利要求1-5任一所述的一种基于多传感器紧耦合的移动机器人位姿估计方法，其特征在于，所述激光雷达每次扫描一周得到的点云为得到一帧图像的点云，激光雷达每次扫描时，将得到的所有点重投影到每一帧的初始时刻。
7.如权利要求1-5任一所述的一种基于多传感器紧耦合的移动机器人位姿估计方法，其特征在于，所述方法还包括：
采用如下方式对视觉重投影误差进行优化：
其中，r为视觉重投影误差，ρ(r)为优化后的视觉重投影误差。
8.如权利要求1-5任一所述的一种基于多传感器紧耦合的移动机器人位姿估计方法，其特征在于，所述全局位姿估计包括：
利用激光雷达采集的点云构建激光点云地图，将使用局部位姿计算的地图点添加至激光点云地图中以更新激光点云地图；
以新增地图点对应的激光点云几何残差最小为目的，对激光点云地图中的新增地图点进行调整，调整后的地图点对应的位姿为全局位姿。
9.如权利要求1-5任一所述的一种基于多传感器紧耦合的移动机器人位姿估计方法，其特征在于，所述方法还包括：
利用相机采集的RGB-D图像构建RGB-D稠密点云地图，将使用局部位姿计算的地图点添加至RGB-D稠密点云地图中以更新RGB-D稠密点云地图；
以新增地图点对应的重投影误差最小为目的，对RGB-D稠密点云地图中的新增地图点进行调整，得到全局优化后的RGB-D稠密点云地图，用于进行路径规划。
10.一种基于多传感器紧耦合的移动机器人位姿估计系统，其特征在于，包括：
相机，用于采集RGB-D图像；所述相机数量为多个，分布于移动机器人的前后左右；
IMU，用于测量数据；
激光雷达，用于采集的点云；
局部位姿估计，用于对相机采集的当前帧RGB-D图像与上一帧RGB-D图像进行特征匹配，计算匹配过程的视觉重投影误差；对IMU测量的数据进行积分，构建IMU积分残差；从激光雷达采集的点云中提取边缘特征点和平面特征点，计算边缘特征点到边缘线的距离以及平面特征点到平面的距离，构建激光点云几何残差；以视觉重投影误差、IMU积分残差和激光点云几何残差最小为目标，进行位姿估计，得到局部位姿；
全局位姿估计，用于使用局部位姿更新激光点云地图后在激光点云地图上进行全局优化，得到全局位姿。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110702106.6A CN113436260B (zh) | 2021-06-24 | 2021-06-24 | 基于多传感器紧耦合的移动机器人位姿估计方法和系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110702106.6A CN113436260B (zh) | 2021-06-24 | 2021-06-24 | 基于多传感器紧耦合的移动机器人位姿估计方法和系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113436260A CN113436260A (zh) | 2021-09-24 |
| CN113436260B true CN113436260B (zh) | 2022-04-19 |
Family
ID=77753736
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110702106.6A Active CN113436260B (zh) | 2021-06-24 | 2021-06-24 | 基于多传感器紧耦合的移动机器人位姿估计方法和系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113436260B (zh) |
Families Citing this family (19)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114018236B (zh) * | 2021-09-30 | 2023-11-03 | 哈尔滨工程大学 | 一种基于自适应因子图的激光视觉强耦合slam方法 |
| CN113607166B (zh) * | 2021-10-08 | 2022-01-07 | 广东省科学院智能制造研究所 | 基于多传感融合的自主移动机器人室内外定位方法及装置 |
| CN113947636B (zh) * | 2021-10-19 | 2024-04-26 | 中南大学 | 一种基于深度学习的激光slam定位系统及方法 |
| CN114370871A (zh) * | 2022-01-13 | 2022-04-19 | 华南理工大学 | 一种可见光定位与激光雷达惯性里程计的紧耦合优化方法 |
| CN114397642B (zh) * | 2022-01-13 | 2024-10-29 | 北京工业大学 | 一种基于图优化的三维激光雷达与imu外参标定方法 |
| CN114593737A (zh) * | 2022-03-11 | 2022-06-07 | 美智纵横科技有限责任公司 | 控制方法、装置、机器人及存储介质 |
| CN114739409A (zh) * | 2022-03-23 | 2022-07-12 | 深圳市优必选科技股份有限公司 | 机器人定位方法、装置、计算机可读存储介质及机器人 |
| CN114754781A (zh) * | 2022-03-31 | 2022-07-15 | 深圳市优必选科技股份有限公司 | 地图更新方法、装置、机器人及介质 |
| CN114966734B (zh) * | 2022-04-28 | 2024-11-08 | 华中科技大学 | 一种结合多线激光雷达的双向深度视觉惯性位姿估计方法 |
| CN114648584B (zh) * | 2022-05-23 | 2022-08-30 | 北京理工大学前沿技术研究院 | 一种用于多源融合定位的鲁棒性控制方法和系统 |
| CN115511962B (zh) * | 2022-09-20 | 2024-05-28 | 上海人工智能创新中心 | 基于光电触觉传感器的目标主动探测方法及系统 |
| CN115655262B (zh) * | 2022-12-26 | 2023-03-21 | 广东省科学院智能制造研究所 | 基于深度学习感知的多层级语义地图构建方法和装置 |
| CN116184430B (zh) * | 2023-02-21 | 2023-09-29 | 合肥泰瑞数创科技有限公司 | 一种激光雷达、可见光相机、惯性测量单元融合的位姿估计算法 |
| CN116222543B (zh) * | 2023-04-26 | 2023-07-28 | 齐鲁工业大学(山东省科学院) | 用于机器人环境感知的多传感器融合地图构建方法及系统 |
| CN116958452A (zh) * | 2023-09-18 | 2023-10-27 | 北京格镭信息科技有限公司 | 三维重建方法和系统 |
| CN117058430B (zh) * | 2023-10-12 | 2023-12-22 | 北京万龙精益科技有限公司 | 用于视场匹配的方法、装置、电子设备和存储介质 |
| CN117367412B (zh) * | 2023-12-07 | 2024-03-29 | 南开大学 | 一种融合捆集调整的紧耦合激光惯导里程计与建图方法 |
| CN118155039B (zh) * | 2024-05-13 | 2024-07-26 | 齐鲁空天信息研究院 | 多源信息融合的定位地图构建方法、装置、介质及设备 |
| CN118209101B (zh) * | 2024-05-21 | 2024-08-16 | 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) | 一种应用于动态环境的多传感器融合slam方法及系统 |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107991683A (zh) * | 2017-11-08 | 2018-05-04 | 华中科技大学 | 一种基于激光雷达的机器人自主定位方法 |
| CN109993113A (zh) * | 2019-03-29 | 2019-07-09 | 东北大学 | 一种基于rgb-d和imu信息融合的位姿估计方法 |
| CN110261870A (zh) * | 2019-04-15 | 2019-09-20 | 浙江工业大学 | 一种用于视觉-惯性-激光融合的同步定位与建图方法 |
| CN110375738A (zh) * | 2019-06-21 | 2019-10-25 | 西安电子科技大学 | 一种融合惯性测量单元的单目同步定位与建图位姿解算方法 |
| CN111045017A (zh) * | 2019-12-20 | 2020-04-21 | 成都理工大学 | 一种激光和视觉融合的巡检机器人变电站地图构建方法 |
| CN111207774A (zh) * | 2020-01-17 | 2020-05-29 | 山东大学 | 一种用于激光-imu外参标定的方法及系统 |
| CN112785702A (zh) * | 2020-12-31 | 2021-05-11 | 华南理工大学 | 一种基于2d激光雷达和双目相机紧耦合的slam方法 |
| CN112923934A (zh) * | 2019-12-06 | 2021-06-08 | 北理慧动（常熟）车辆科技有限公司 | 一种适用于非结构化场景中结合惯导的激光slam技术 |
-
2021
- 2021-06-24 CN CN202110702106.6A patent/CN113436260B/zh active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107991683A (zh) * | 2017-11-08 | 2018-05-04 | 华中科技大学 | 一种基于激光雷达的机器人自主定位方法 |
| CN109993113A (zh) * | 2019-03-29 | 2019-07-09 | 东北大学 | 一种基于rgb-d和imu信息融合的位姿估计方法 |
| CN110261870A (zh) * | 2019-04-15 | 2019-09-20 | 浙江工业大学 | 一种用于视觉-惯性-激光融合的同步定位与建图方法 |
| CN110375738A (zh) * | 2019-06-21 | 2019-10-25 | 西安电子科技大学 | 一种融合惯性测量单元的单目同步定位与建图位姿解算方法 |
| CN112923934A (zh) * | 2019-12-06 | 2021-06-08 | 北理慧动（常熟）车辆科技有限公司 | 一种适用于非结构化场景中结合惯导的激光slam技术 |
| CN111045017A (zh) * | 2019-12-20 | 2020-04-21 | 成都理工大学 | 一种激光和视觉融合的巡检机器人变电站地图构建方法 |
| CN111207774A (zh) * | 2020-01-17 | 2020-05-29 | 山东大学 | 一种用于激光-imu外参标定的方法及系统 |
| CN112785702A (zh) * | 2020-12-31 | 2021-05-11 | 华南理工大学 | 一种基于2d激光雷达和双目相机紧耦合的slam方法 |
Non-Patent Citations (3)
| Title |
|---|
| Pose Estimation Based on Wheel Speed Anomaly Detection in Monocular Visual-Inertial SLAM;Gang Peng 等;《IEEE SENSORS JOURNAL》;20200515;第21卷(第10期);第11692-11703页 * |
| RTM框架下基于点线特征的视觉SLAM算法;贾松敏 等;《机器人》;20190531;第41卷(第03期);全文 * |
| 基于Vision-IMU的机器人同时定位与地图创建算法;姚二亮等;《仪器仪表学报》;20180415(第04期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113436260A (zh) | 2021-09-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113436260B (zh) | 基于多传感器紧耦合的移动机器人位姿估计方法和系统 | |
| CN110264567B (zh) | 一种基于标记点的实时三维建模方法 | |
| CN111258313B (zh) | 多传感器融合slam系统及机器人 | |
| CN110070615B (zh) | 一种基于多相机协同的全景视觉slam方法 | |
| CN111076733B (zh) | 一种基于视觉与激光slam的机器人室内建图方法及系统 | |
| CN112785702A (zh) | 一种基于2d激光雷达和双目相机紧耦合的slam方法 | |
| CN113706626B (zh) | 一种基于多传感器融合及二维码校正的定位与建图方法 | |
| CN107301654A (zh) | 一种多传感器的高精度即时定位与建图方法 | |
| CN110807809A (zh) | 基于点线特征和深度滤波器的轻量级单目视觉定位方法 | |
| CN112734765A (zh) | 基于实例分割与多传感器融合的移动机器人定位方法、系统及介质 | |
| CN111127613B (zh) | 基于扫描电子显微镜的图像序列三维重构方法及系统 | |
| CN111998862B (zh) | 一种基于bnn的稠密双目slam方法 | |
| CN116449384A (zh) | 基于固态激光雷达的雷达惯性紧耦合定位建图方法 | |
| CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
| CN116222543B (zh) | 用于机器人环境感知的多传感器融合地图构建方法及系统 | |
| CN117197333A (zh) | 基于多目视觉的空间目标重构与位姿估计方法及系统 | |
| CN112752028A (zh) | 移动平台的位姿确定方法、装置、设备和存储介质 | |
| CN112197773A (zh) | 基于平面信息的视觉和激光定位建图方法 | |
| CN112270698A (zh) | 基于最邻近曲面的非刚性几何配准方法 | |
| CN115639547A (zh) | 多线激光雷达与gnss_ins联合标定方法、系统及介质 | |
| CN118168545A (zh) | 基于多源传感器融合的除草机器人定位导航系统及方法 | |
| CN116468786B (zh) | 一种面向动态环境的基于点线联合的语义slam方法 | |
| CN113487631A (zh) | 基于lego-loam的可调式大角度探测感知及控制方法 | |
| CN116879870A (zh) | 一种适用于低线束3d激光雷达的动态障碍物去除方法 | |
| CN112767459A (zh) | 基于2d-3d转换的无人机激光点云与序列影像配准方法 |
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 |