CN116380039A - 一种基于固态激光雷达和点云地图的移动机器人导航系统 - Google Patents
一种基于固态激光雷达和点云地图的移动机器人导航系统 Download PDFInfo
- Publication number
- CN116380039A CN116380039A CN202310274750.7A CN202310274750A CN116380039A CN 116380039 A CN116380039 A CN 116380039A CN 202310274750 A CN202310274750 A CN 202310274750A CN 116380039 A CN116380039 A CN 116380039A
- Authority
- CN
- China
- Prior art keywords
- map
- point cloud
- coordinate system
- pose
- point
- 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.)
- Pending
Links
- 230000033001 locomotion Effects 0.000 claims abstract description 18
- 238000007781 pre-processing Methods 0.000 claims abstract description 8
- 238000000034 method Methods 0.000 claims description 32
- 230000009466 transformation Effects 0.000 claims description 22
- 238000005259 measurement Methods 0.000 claims description 16
- 230000008569 process Effects 0.000 claims description 14
- 239000007787 solid Substances 0.000 claims description 12
- 230000001133 acceleration Effects 0.000 claims description 10
- 238000012545 processing Methods 0.000 claims description 10
- 238000012216 screening Methods 0.000 claims description 9
- 230000004888 barrier function Effects 0.000 claims description 7
- 230000001186 cumulative effect Effects 0.000 claims description 7
- 230000005484 gravity Effects 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 5
- 238000005070 sampling Methods 0.000 claims description 4
- 230000008859 change Effects 0.000 claims description 3
- 238000001914 filtration Methods 0.000 claims description 3
- 239000011159 matrix material Substances 0.000 claims description 3
- 230000011218 segmentation Effects 0.000 claims description 3
- 230000003068 static effect Effects 0.000 claims description 3
- 230000017105 transposition Effects 0.000 claims description 3
- 238000009966 trimming Methods 0.000 claims description 3
- 230000000694 effects Effects 0.000 description 7
- 238000010586 diagram Methods 0.000 description 5
- 230000008901 benefit Effects 0.000 description 2
- IIBYAHWJQTYFKB-UHFFFAOYSA-N bezafibrate Chemical compound C1=CC(OC(C)(C)C(O)=O)=CC=C1CCNC(=O)C1=CC=C(Cl)C=C1 IIBYAHWJQTYFKB-UHFFFAOYSA-N 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000003631 expected effect Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000000007 visual effect 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/1652—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 ranging devices, e.g. LIDAR or RADAR
-
- 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
-
- 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
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种基于固态激光雷达和点云地图的移动机器人导航系统，包括：点云地图预处理模块，用于生成供路径规划使用的占据栅格地图；激光雷达惯性里程计模块，用于对雷达点云去运动畸变并提供较为准确的里程计；地图匹配定位模块，用于将累积的去畸变点云与点云地图匹配，更新机器人在地图系下的位姿；路径规划模块，用于综合地图和定位信息，规划到达目标点的路径。本发明解决了二维栅格地图对复杂环境描述不够完整，三维点云地图不便于进行移动机器人的路径规划的矛盾，综合了固态激光雷达和IMU紧耦合的里程计以及与点云地图匹配更新位姿的方式，使得机器人的定位、导航在复杂场景中也具有高精度和鲁棒性。
Description
技术领域
本发明涉及移动机器人导航的技术领域，尤其是指一种基于固态激光雷达和点云地图的移动机器人导航系统，利用固态雷达在三维点云地图中定位，并将点云地图处理为栅格地图进行路径规划。
背景技术
目前各类应用场景下对移动机器人智能化程度的要求不断提高，许多任务要求它们能够辨别周围障碍，根据环境信息进行自主导航或接受远程指令完成定点导航，基于WiFi、UWB等通讯定位的方式逐渐无法满足复杂作业的要求。其中，获取周围环境的详细信息以及自身在其中的准确位置便是解决移动机器人自主导航问题的基础，使用点云描绘三维环境具有较高的精度，在自动驾驶、测绘和机器人领域有着广泛应用。
针对室内简单环境的机器人导航，在建图、地图中重定位以及路径规划的关键步骤上均有较为成熟的开源方案。例如，可以使用单线激光雷达，通过gmapping、cartographer等绘制环境的二维栅格地图，并将此栅格地图导入ROS-Navigation框架进行地图中的重定位和路径规划，从而完成完整的机器人导航。此类方案虽然在室内结构化场景可以运行，但一方面，在室外大场景和复杂或空旷的室内场景下，会由于单线激光雷达的探测距离和安装高度限制，无法将环境中所有的障碍物绘制进栅格地图，从而无法正常工作；另一方面，三维点云地图又不便于进行移动机器人的路径规划。因此，如何在使用三维点云地图进行定位的同时，转化点云地图进行路径规划成为了提出鲁棒导航系统的关键问题。本发明提出了一种基于固态激光雷达和点云地图的移动机器人导航系统，解决了众多使用单线激光雷达建图、重定位的系统对三维环境描述不完整，适用场景少的问题，在复杂室内场景及室外等空旷场景均能取得较高的定位、导航精度。
发明内容
本发明的目的在于克服现有技术的缺点与不足，提出了一种高精度、鲁棒性强、适用场景广的基于固态激光雷达和点云地图的移动机器人导航系统，解决了二维栅格地图对复杂环境描述不够完整，三维点云地图不便于进行移动机器人的路径规划的矛盾，综合了固态激光雷达和IMU紧耦合的里程计以及与点云地图匹配更新位姿的方式，使得机器人的定位、导航在复杂场景中也具有高精度和鲁棒性。
为实现上述目的，本发明所提供的技术方案为：一种基于固态激光雷达和点云地图的移动机器人导航系统，包括：
点云地图预处理模块，用于生成供路径规划使用的占据栅格地图并记录两个地图坐标系间的位姿变换，以便对齐点云地图坐标系和栅格地图坐标系；
激光雷达惯性里程计模块，用于处理固态激光雷达数据和IMU数据，通过高频的IMU数据为原始的雷达点云消除运动畸变，输出去畸变点云，将去畸变点云与累积点云进行匹配、计算残差从而更新位姿状态，实现一个准确的里程计；
地图匹配定位模块，用于加载离线点云地图，利用当前机器人在地图坐标系下的位姿和雷达的视场角剖分出局部点云，利用激光雷达惯性里程计模块输出的去畸变点云拼接成滑动窗口结构，并将局部点云与滑动窗口内点云进行匹配，最终更新机器人在地图坐标系下的位姿；
路径规划模块，利用栅格地图加载障碍物信息，利用地图匹配定位模块输出的机器人位姿，在接收到目标地点后开始进行路径规划，向机器人底层驱动输出运动控制指令实现导航功能。
进一步，所述点云地图预处理模块，用于生成供路径规划使用的占据栅格地图并记录两个地图坐标系间的位姿变换，同时对齐点云地图坐标系和栅格地图坐标系，包括以下步骤：
1-1)输入原始点云，在水平方向x，y上按固定大小划分为多块区域，设定其中一处作为初始区域，给定粗略的地面方程；
1-2)判断当前区域是否处理过，若未处理过，则按照点到当前设定的地面的距离筛选出地面点云A，对其中每个点p条件为：
式中，
分别为当前地面方程fi中的法向量和平面到原点的距离，ε为设定的距离阈值；第二个式子表示点p的表面法向量Normal(p)与重力向量g的反方向差距不超过角度/>
右上角标T为矩阵转置运算；
1-3)判断点云A中的点数是否低于设定阈值，若低于阈值，则使用基于区域生长的点云分割，对聚类出的前r个区域，依次检查最多点数区域的平面方程hi参数是否符合以下要求：
式中，
分别为平面方程hi中的法向量和平面到原点的距离，θ为平面方程hi与当前地面方程fi的法向量角度差的阈值，εd近似看为两平面间的距离阈值；若符合要求，则令参考地面方程fi+1＝hi，标记相关区域的点；若前r个区域中无符合要求的hi，则令fi+1＝fi，此区域内无标记点；
当点云A的点数高于阈值时，则以A内的所有点拟合平面方程hi，再以hi做一次相同的地面点云筛选，结果记为点云B；当点云B的点数大于点云A中点数的2倍时，令fi+1＝hi，标记B中的点为地面点；否则，fi+1＝hi，标记A中的点为地面点；
1-4)去除标记为地面的部分、在地面高度之下的部分、以及在地面之上并高于机器人高度的部分；将当前区域设为已处理，设定fi+1为当前地面方程，然后依次检查当前区域的四邻域区域，对其进行步骤1-2)～1-4)中操作；
1-5)待所有区域处理完毕后，合并所有区域的点云后向z轴投影，此时所有点云中的地面区域已被删去，剩余点云表示障碍物或不能行走的区域，删去一些远的边界点云控制地图大小，对点云统计滤波去除噪点后，再次划分平面网格，根据网格内的点数决定生成栅格地图时此栅格的占据值；
1-6)由于步骤1-5)中会对地图的做一些边界的修剪和旋转操作，记录操作过程中栅格地图与点云地图坐标系间的位姿变换和比例尺，用于后续路径规划时两个地图坐标系的统一。
进一步，所述激光雷达惯性里程计模块执行以下操作：
2-1)机器人保持静止，收集固态激光雷达数据构建初始的累积点云用于后续匹配计算残差；收集静止状态IMU数据，统计均值粗略估计角速度计和加速度计的测量偏差bω,ba和重力向量g；静置2～3s完成里程计算法部分的初始化；
2-2)处理雷达数据和IMU数据，后续将一帧固态激光雷达点云数据简记为scan，利用高频的IMU数据进行当前位姿的前向传播，直至下一scan输入后，利用位姿插值将此scan内所有采样点投影到扫描结束时间，从而去除运动畸变；其中位姿前向传播的具体数学形式如下：
式中，xi、xi+1分别为IMU在当前时刻i和下一时刻i+1的状态，Δt为两次IMU数据间的间隔时间；各物理量的右下角标为该物理量所描述的对象坐标系，左上角标为表示该物理量所使用的坐标系；IMU坐标系记为I，i时刻的IMU坐标系记为Ii，第一帧IMU坐标系为里程计坐标系，后简写为Odom坐标系，记为O；其中状态xi包括：i时刻的IMU坐标系的姿态
位置/>
速度/>
IMU角速度计和加速度计的测量偏差/>
和重力向量Ogi在Odom坐标系下的表示；/>
为广义加符号，状态量中除姿态以外的物理量在广义加时进行线性相加操作，姿态按三维旋转群及其李代数运算规则操作；ui为i时刻的运动输入，即来自IMU输入的角速度测量/>
加速度测量/>
wi为i时刻系统噪声，包括角速度测量噪声/>
加速度测量噪声/>
角速度偏差噪声/>
和加速度偏差噪声/>
03×1表示3行1列的零向量；F(xi,ui,wi)为单位时间内的状态变化量；
2-3)将去畸变后的scan中的每个点pj以下式从k时刻的激光雷达坐标系Lk变换至Odom坐标系：
式中，IRL，ItL为事先标定好的激光雷达坐标系与IMU坐标系间的位姿变换参数；Ik为k时刻的IMU坐标系；
为待优化状态量Oxk中的姿态和位置项，以前向传播后的结果作为初值；
后对Odom坐标系下scan中的每个点Opj，在累积点云中使用最近邻搜索找到最近点Oqj，并用找到最近的5个点拟合平面，记其法向量为nj，则残差resj形式为：
通过优化状态Oxk最小化scan中各点的总残差，迭代收敛后的最优
即为当前Odom坐标系中机器人状态；
2-4)以最优估计
中的姿态和位置/>
把去畸变后的当前scan加入累积点云。
进一步，所述地图匹配定位模块执行以下操作：
3-1)根据机器人在点云地图坐标系下的位姿与雷达的视场角α筛选出在视野内及附近的局部点云，后续将点云地图坐标系简记为Map坐标系，在公式角标中记为M，具体操作如下式，即局部点云由满足下式的点Mp构成：
式中，Lφ为激光雷达正面方向向量在激光雷达坐标系L下的表示，δ为允许的角度误差；
为机器人在Map坐标系下的姿态和位置，由步骤2-3)中的/>
经由Odom坐标系与Map坐标系的位姿变换MRO,MtO所得，在算法初始化时由预先设定值作为初值；
3-2)为保证足够的点数用于匹配，且防止Map坐标系下位姿误差导致与步骤3-1)中切分的局部点云共视区域过小，将步骤2-2)中去畸变后的scan存入队列式数据结构，由多帧scan组成Odom坐标系下的滑动窗口内点云；
3-3)将步骤3-1)中Map坐标系下的局部点云和步骤3-2)中Odom坐标系下的窗口内点云进行ICP匹配，求解并更新Odom坐标系与Map坐标系的位姿变换MRO,MtO；
3-4)使用步骤2-3)中求得的Odom坐标系与Map坐标系的位姿变换MRO,MtO和步骤2-3)中求得的Odom坐标系下机器人位姿估计
得到Map坐标系下机器人位姿：
之后，Map坐标系下机器人位姿
能作为后续路径规划模块的输入。
进一步，所述路径规划模块执行以下操作：
4-1)输入步骤1-5)中的栅格地图，将地图中高于设定阈值的网格视为障碍物，低于阈值的网格视为可通行区域；
4-2)持续接收步骤3-4)中的Map坐标系下机器人位姿，利用在步骤1-6)中记录的点云地图和栅格地图坐标系间变换关系，对齐两个坐标系，因此所得位姿即为机器人当前在栅格地图中的位置；
4-3)输入导航的目的地坐标，结合机器人当前位置与地图障碍信息做出路径规划，随后转为运动控制指令输出，完成导航。
本发明与现有技术相比，具有如下优点与有益效果：
1、固态激光雷达相较机械旋转式多线激光雷达成本更低，且能够获取更为稠密的三维点云信息。但受限于大部分固态雷达的视角较小，单帧固态雷达点云与地图匹配的效果较差，本发明采用了滑动窗口结构积累多帧去畸变点云，与按固态雷达位姿和视场角切分出的局部地图进行匹配，大大增强了匹配的速度与位姿精度，充分发挥了固态雷达的性能优势。
2、通过三维点云地图转化成的二维栅格地图相较于单线激光雷达所建的栅格地图，可以获取更为丰富的障碍物信息，避免了死角和高度限制。同时二维栅格地图上的路径规划算法效率更高，也有着更多的成熟方案参考。本发明综合了三维点云的环境信息丰富和二维栅格地图便于移动机器人导航的特性，提升了整个导航系统的鲁棒性和适用范围。
3、在三维点云地图转化为二维栅格地图的算法中，采用了分割多个网格并依次拟合平面方程的方式，可以应对含有斜坡的区域以及地面不完全平坦的场景，从而提升了算法的鲁棒性和适用范围。
4、采用了紧耦合方式融合固态激光雷达的点云数据和IMU测量数据，充分利用了IMU在短时间内对快速运动的捕捉特性，在获得了高频的里程计输出的同时，可以更好地去除激光雷达点云由于采样点时间不同而附带的运动畸变。
5、采用IMU位姿递推和激光雷达点云作为观测修正更新状态量的紧耦合里程计，可以输出精度较高的里程计坐标系位姿，从而为后续的匹配点云地图进行ICP的算法提供一个良好的位姿初值，增加了成功匹配的平均点数，减少了ICP算法失败的概率，提升了定位算法的精度和鲁棒性。
附图说明
图1为本发明的整体逻辑流程图。
图2为本发明中将点云地图预处理模块为供路径规划使用的二维栅格地图算法的流程图。
图3为本发明中使用固态雷达在点云地图中定位算法的流程图。
图4为实施例中未采用分区域去除地面点云和采用本发明方法所得结果的俯视图对比。
图5为实施例中点云地图转化得到的栅格地图。
图6为实施例中点云地图与栅格地图对齐的效果图。
图7为实施例中采用固态雷达视场角切分点云地图的效果图。
图8为实施例中采用本发明提出的重定位及导航系统实际运行效果图。
具体实施方式
下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。
如附图1所示，本实施例提供了一种基于固态激光雷达和点云地图的移动机器人导航系统，包括以下模块：
点云地图预处理模块，用于生成供路径规划使用的占据栅格地图并记录两个地图坐标系间的位姿变换，以便对齐点云地图坐标系和栅格地图坐标系；
激光雷达惯性里程计模块，用于处理固态激光雷达数据和IMU数据。通过高频的IMU数据为原始的雷达点云消除运动畸变，输出去畸变点云。同时将去畸变点云与累积点云进行匹配、计算残差从而更新位姿状态，实现一个较为准确的里程计；
地图匹配定位模块，用于加载离线点云地图，利用当前机器人在地图坐标系下的位姿和雷达的视场角剖分出局部点云，利用激光雷达惯性里程计模块输出的去畸变点云拼接成滑动窗口结构，并将局部点云与滑动窗口内点云进行匹配，最终更新机器人在地图坐标系下的位姿。
路径规划模块，利用栅格地图加载障碍物信息；利用地图匹配定位模块输出的机器人位姿，在接收到目标地点后开始进行路径规划，向机器人底层驱动输出运动控制指令实现导航功能。
进一步地，如附图3所示，所述点云地图预处理模块具体执行以下操作：
1-1)输入原始点云，在水平方向x，y上按固定大小划分为多块区域，设定其中一处作为初始区域，并给定粗略的地面方程f0。在本实施例中，原始点云地图的场景为一个约180m×240m的室外广场，在去除部分边界后，按照20×25m的网格大小可划分为共64个区域。选定初始区域为原始点云中原点所在区域，由于点云地图的z轴即为竖直方向，建图设备距地面高度为0.6m，因此将初始地面方程设为
式中
为初始地面方程的法向量，/>
为初始地面到原点的距离；
1-2)判断当前区域是否处理过，若未处理过，则按照点到当前设定的地面的距离筛选出地面点云A，对其中每个点p条件为：
式中，
分别为当前地面方程fi中的法向量和平面到原点的距离，ε为设定的距离阈值；第二个式子表示点p的表面法向量Normal(p)与重力向量g的反方向差距不超过角度/>
右上角标T为矩阵转置运算。在本实施例中，距离阈值设置为10cm，角度阈值设置为30°。
1-3)判断点云A中的点数是否低于设定阈值。若低于阈值，则使用基于区域生长的点云分割，对聚类出的前r个区域，依次检查最多点数区域的平面方程hi参数是否符合以下要求：
式中，
分别为平面方程hi中的法向量和平面到原点的距离，θ为平面方程hi与当前地面方程fi的法向量角度差的阈值，εd近似看为两平面间的距离阈值；若符合要求，则令参考地面方程fi+1＝hi，标记相关区域的点；若前r个区域中无符合要求的hi，则令fi+1＝fi，此区域内无标记点；
当点云A的点数高于阈值时，则以A内的所有点拟合平面方程hi，再以hi做一次相同的地面点云筛选，结果记为点云B。当点云B的点数大于点云A中点数的2倍时，令fi+1＝hi，标记B中的点为地面点；否则，fi+1＝hi，标记A中的点为地面点；
1-4)去除标记为地面的部分、在地面高度之下的部分、以及在地面之上并高于机器人高度的部分，将当前区域设为已处理。设定fi+1为当前地面方程，然后依次检查当前区域的四邻域区域，对其进行步骤1-2)～1-4)操作；
1-5)待所有区域处理完毕后，合并所有区域的点云后向z轴投影。此时所有点云中的地面区域已被删去，剩余点云表示障碍物或不可行走的区域。对点云统计滤波去除噪点后，再次划分平面网格，根据网格内的点数决定生成栅格地图时此栅格的占据值。在本实施例中，根据场景大小将分辨率设为4cm/栅格，每个栅格代表的区域内，点数低于50个点将占据值设置为0，反之设置为255。最终生成如图5所示的栅格地图。
1-6)由于步骤1-5)中会对地图的做一些边界的修剪和旋转等操作，记录操作过程中栅格地图与点云地图坐标系间的位姿变换和比例尺，用于后续导航时两个地图坐标系的统一。
如附图4所示，在本实施例中，由于场景较大且存在几处小角度斜坡，直接按相同的平面方程去除地面点云无法达到区分障碍和可行走区域的要求(见附图4上)；而采用本发明提出的点云地图处理方式，可以获得预期效果(见附图4下)。
附图6展示了实施例中三维点云地图与二维栅格地图对齐的效果。
进一步地，关于所述激光雷达惯性里程计模块中的硬件选型，本实施例中选用了Livox Mid-70作为固态激光雷达，其视场角为70.4°。所选用的移动机器人平台为搭建的四轮AGV小车，所用工控机为研华MIC-7700H。所选用的IMU为Intel-Realsense d435i上的内置IMU。如附图2所示，具体流程为：
2-1)机器人保持静止，收集固态激光雷达数据构建初始的累积点云用于后续匹配计算残差；收集静止状态IMU数据，统计均值粗略估计角速度计和加速度计的测量偏差bω,ba和重力向量g。静置2～3s完成里程计算法部分的初始化；
2-2)处理雷达数据和惯性传感器(IMU)数据，通过高频的IMU数据(包括三轴加速度和三轴角速度)进行当前位姿的前向传播，直至下一帧雷达点云数据输入后(以下将一帧激光雷达点云数据简记为scan)，利用位姿插值将此scan内所有采样点投影到扫描结束时间从而去除运动畸变。其中位姿前向传播的具体数学形式如下：
式中，xi、xi+1分别为IMU在当前时刻i和下一时刻i+1的状态，Δt为两次IMU数据间的间隔时间；各物理量的右下角标为该物理量所描述的对象坐标系，左上角标为表示该物理量所使用的坐标系；IMU坐标系记为I，i时刻的IMU坐标系记为Ii，第一帧IMU坐标系为里程计坐标系，后简写为Odom坐标系，记为O；其中状态xi包括：i时刻的IMU坐标系的姿态
位置/>
速度/>
IMU角速度计和加速度计的测量偏差/>
和重力向量Ogi在Odom坐标系下的表示；/>
为广义加符号，状态量中除姿态以外的物理量在广义加时进行线性相加操作，姿态按三维旋转群及其李代数运算规则操作；ui为i时刻的运动输入，即来自IMU输入的角速度测量/>
加速度测量/>
wi为i时刻系统噪声，包括角速度测量噪声/>
加速度测量噪声/>
角速度偏差噪声/>
和加速度偏差噪声/>
03×1表示3行1列的零向量；F(xi,ui,wi)为单位时间内的状态变化量。
值得注意，本实施例中整个里程计算法以IMU的200Hz频率输出机器人位姿，以Livox传入雷达帧的10Hz频率进行基于雷达点云观测的状态更新。
2-3)将去畸变后的scan中的每个点pj以下式从k时刻的激光雷达坐标系Lk变换至里程计坐标系O：
其中IRL，ITL为事先标定好的激光雷达坐标系与IMU坐标系间的位姿变换参数；
为待优化状态量Oxk中的姿态和位置项，以前向传播后的结果作为初值。
后对Odom坐标系下scan中的每个点Opj，在累积点云中使用最近邻搜索找到最近点Oqj，并用找到最近的5个点拟合平面，记其法向量为nj，则残差resj形式为：
通过优化状态Oxk最小化scan中各点的总残差，迭代收敛后的最优
即为当前Odom坐标系中机器人状态；
2-4)以最优估计
中的姿态和位置/>
把去畸变后的当前scan加入累积点云。
进一步地，所述地图匹配定位模块为通过对里程计(Odom)坐标系和点云地图(Map)坐标系相对应范围的点云进行ICP匹配，将机器人在Odom坐标系下的估计状态
转至Map坐标系中，从而为导航任务提供机器人在地图中的位姿。如附图2所示，其具体流程为：
3-1)根据机器人在点云地图坐标系下的位姿与雷达的视场角α筛选出在视野内及附近的局部点云，后续将点云地图坐标系简记为Map坐标系，在公式角标中记为M。具体操作如下式，即局部点云由满足下式的点Mp构成：
其中，Lφ为激光雷达正面方向向量在激光雷达坐标系L下的表示，δ为允许的角度误差。本实施例中，Livox Mid70的x轴为其正面方向，所以Lφ＝(1,0,0)T，视场角α＝70.4°，δ设为5°。
为机器人在Map坐标系下的姿态和位置，由步骤2-3)中的/>
经由Odom坐标系与Map坐标系的位姿变换MRO,MtO所得，在算法初始化时由预先设定值作为初值。
附图7中演示了根据机器人在地图中位姿，切分可能在视场角内的局部点云的效果图。图中亮度较高的三维点云(即局部点云)呈锥形，对应了固态激光雷达的视场角，局部点云上的白色断点为当前传入的scan，白色端点贴合在点云的表面形状上，表现出定位算法的良好运行。
3-2)为保证足够的点数用于匹配，且防止Map坐标系下位姿误差导致与步骤3-1)中切分的局部点云共视区域过小，将步骤2-2)中去畸变后的scan存入队列式数据结构，由多帧scan组成Odom坐标系下的滑动窗口内点云；
3-3)将步骤3-1)中Map坐标系下的局部点云和步骤3-2)中Odom坐标系下的窗口内点云进行ICP匹配，求解并更新Odom坐标系与Map坐标系的位姿变换MRO,MtO，其中ICP匹配的形式为求解如下式的最小二乘问题：
其中pi,p'i分别为局部点云和窗口内点云中的第i个点，n为两个点云中点数较少的点云的总点数；
3-4)使用步骤3-3)中求得的Odom坐标系与Map坐标系的位姿变换MRO,MtO和步骤2-3)中求得的Odom坐标系下机器人位姿估计
得到Map坐标系下机器人位姿：
之后，Map坐标系下机器人位姿
可作为后续路径规划模块的输入。
进一步地，所述路径规划模块在本实施例中采用了ROS-Navigation作为路径规划算法的框架，具体实施步骤为：
4-1)输入步骤1-5)中的二维栅格地图至ROS-map_server节点，按照所设置的分辨率及坐标原点加载地图，将地图中高于设定阈值的网格视为障碍物，低于阈值的网格视为可通行区域。
4-2)持续接收步骤3-4)中的Map坐标系下机器人位姿，由于在步骤1-6)中点云地图和栅格地图坐标系已对齐，因此所得位姿即为机器人当前在栅格地图中的位置；
4-3)输入导航的目的地坐标、机器人当前位置和地图障碍信息至ROS-move_base节点，通过A*算法做全局路径规划，DWA算法做局部路径规划，随后转为运动控制指令输出，完成导航。
注意在本实施例中，激光雷达惯性里程计模块中所述Map坐标系下机器人位姿
实际上为Map坐标系下IMU传感器的位姿，但由于IMU固定在机器人上，两个坐标系间仅相差一个事先标定好的位姿参数，在此不做区分。
附图8展示了本实施例中机器人在场景中运行导航系统的效果。图中左边部分为导航系统路径规划部分的运行截图，其中以机器人为顶点的白色点群为当前scan，机器人前方曲线为所规划的路径。图中右上区域为机器人前方相机的画面。图中右下区域为机器人此时所处的实际位置的照片。可以看到实际场景中机器人与周围物体的位置关系，与栅格地图中各项对应准确。
应当理解，本申请的各部分可以用硬件、软件或他们的组合来实现，在上述实施方式中，多个步骤或方法可以用合适的其他硬件或软件来实现。
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。
Claims (5)
1.一种基于固态激光雷达和点云地图的移动机器人导航系统，其特征在于，包括：
点云地图预处理模块，用于生成供路径规划使用的占据栅格地图并记录两个地图坐标系间的位姿变换，以便对齐点云地图坐标系和栅格地图坐标系；
激光雷达惯性里程计模块，用于处理固态激光雷达数据和IMU数据，通过高频的IMU数据为原始的雷达点云消除运动畸变，输出去畸变点云将去畸变点云与累积点云进行匹配、计算残差从而更新位姿状态，实现一个准确的里程计；
地图匹配定位模块，加载离线点云地图，利用当前机器人在地图坐标系下的位姿和雷达的视场角剖分出局部点云，利用激光雷达惯性里程计模块输出的去畸变点云拼接成滑动窗口结构，并将局部点云与滑动窗口内点云进行匹配，最终更新机器人在地图坐标系下的位姿；
路径规划模块，利用栅格地图加载障碍物信息，利用地图匹配定位模块输出的机器人位姿，在接收到目标地点后开始进行路径规划，向机器人底层驱动输出运动控制指令实现导航功能。
2.根据权利要求1所述的一种基于固态激光雷达和点云地图的移动机器人导航系统，其特征在于：所述点云地图预处理模块，用于生成供路径规划使用的占据栅格地图并记录两个地图坐标系间的位姿变换，同时对齐点云地图坐标系和栅格地图坐标系，包括以下步骤：
1-1)输入原始点云，在水平方向x，y上按固定大小划分为多块区域，设定其中一处作为初始区域，给定粗略的地面方程；
1-2)判断当前区域是否处理过，若未处理过，则按照点到当前设定的地面的距离筛选出地面点云A，对其中每个点p条件为：
式中，
分别为当前地面方程fi中的法向量和平面到原点的距离，ε为设定的距离阈值；第二个式子表示点p的表面法向量Normal(p)与重力向量g的反方向差距不超过角度/>
右上角标T为矩阵转置运算；
1-3)判断点云A中的点数是否低于设定阈值，若低于阈值，则使用基于区域生长的点云分割，对聚类出的前r个区域，依次检查最多点数区域的平面方程hi参数是否符合以下要求：
式中，
分别为平面方程hi中的法向量和平面到原点的距离，θ为平面方程hi与当前地面方程fi的法向量角度差的阈值，εd近似看为两平面间的距离阈值；若符合要求，则令参考地面方程fi+1＝hi，标记相关区域的点；若前r个区域中无符合要求的hi，则令fi+1＝fi，此区域内无标记点；
当点云A的点数高于阈值时，则以A内的所有点拟合平面方程hi，再以hi做一次相同的地面点云筛选，结果记为点云B；当点云B的点数大于点云A中点数的2倍时，令fi+1＝hi，标记B中的点为地面点；否则，fi+1＝hi，标记A中的点为地面点；
1-4)去除标记为地面的部分、在地面高度之下的部分、以及在地面之上并高于机器人高度的部分；将当前区域设为已处理，设定fi+1为当前地面方程，然后依次检查当前区域的四邻域区域，对其进行步骤1-2)～1-4)中操作；
1-5)待所有区域处理完毕后，合并所有区域的点云后向z轴投影，此时所有点云中的地面区域已被删去，剩余点云表示障碍物或不能行走的区域，删去一些边界点云控制地图大小，对点云统计滤波去除噪点后，再次划分平面网格，根据网格内的点数决定生成栅格地图时此栅格的占据值；
1-6)由于步骤1-5)中会对地图的做一些边界的修剪和旋转操作，记录操作过程中栅格地图坐标系与点云地图坐标系间的位姿变换和比例尺，用于后续路径规划时两个地图坐标系的统一。
3.根据权利要求2所述的一种基于固态激光雷达和点云地图的移动机器人导航系统，其特征在于：所述激光雷达惯性里程计模块执行以下操作：
2-1)机器人保持静止，收集固态激光雷达数据构建初始的累积点云用于后续匹配计算残差；收集静止状态IMU数据，统计均值粗略估计角速度计和加速度计的测量偏差bω,ba和重力向量g；静置2～3s完成里程计算法部分的初始化；
2-2)处理雷达数据和IMU数据，后续将一帧固态激光雷达点云数据简记为scan，利用高频的IMU数据进行当前位姿的前向传播，直至下一scan输入后，利用位姿插值将此scan内所有采样点投影到扫描结束时间，从而去除运动畸变；其中位姿前向传播的具体数学形式如下：
式中，xi、xi+1分别为IMU在当前时刻i和下一时刻i+1的状态，Δt为两次IMU数据间的间隔时间；各物理量的右下角标为该物理量所描述的对象坐标系，左上角标为表示该物理量所使用的坐标系；IMU坐标系记为I，i时刻的IMU坐标系记为Ii，第一帧IMU坐标系为里程计坐标系，后简写为Odom坐标系，记为O；其中状态xi包括：i时刻的IMU坐标系的姿态
位置/>
速度/>
IMU角速度计和加速度计的测量偏差/>
和重力向量Ogi在Odom坐标系下的表示；/>
为广义加符号，状态量中除姿态以外的物理量在广义加时进行线性相加操作，姿态按三维旋转群及其李代数运算规则操作；ui为i时刻的运动输入，即来自IMU输入的角速度测量/>
加速度测量/>
wi为i时刻系统噪声，包括角速度测量噪声/>
加速度测量噪声/>
角速度偏差噪声/>
和加速度偏差噪声/>
03×1表示3行1列的零向量；F(xi,ui,wi)为单位时间内的状态变化量；
2-3)将去畸变后的scan中的每个点pj以下式从k时刻的激光雷达坐标系Lk变换至Odom坐标系：
式中，IRL，ItL为事先标定好的激光雷达坐标系与IMU坐标系间的位姿变换参数；Ik为k时刻的IMU坐标系；
为待优化状态量Oxk中的姿态和位置项，以前向传播后的结果作为初值；
后对Odom坐标系下scan中的每个点Opj，在累积点云中使用最近邻搜索找到最近点Oqj，并用找到最近的5个点拟合平面，记其法向量为nj，则残差resj形式为：
通过优化状态Oxk最小化scan中各点的总残差，迭代收敛后的最优
即为当前Odom坐标系中机器人状态；
2-4)以最优估计
中的姿态和位置/>
把去畸变后的当前scan加入累积点云。
4.根据权利要求3所述的一种基于固态激光雷达和点云地图的移动机器人导航系统，其特征在于：所述地图匹配定位模块执行以下操作：
3-1)根据机器人在点云地图坐标系下的位姿与雷达的视场角α筛选出在视野内及附近的局部点云，后续将点云地图坐标系简记为Map坐标系，在公式角标中记为M，具体操作如下式，即局部点云由满足下式的点Mp构成：
式中，Lφ为激光雷达正面方向向量在激光雷达坐标系L下的表示，δ为允许的角度误差；
为机器人在Map坐标系下的姿态和位置，由步骤2-3)中的/>
经由Odom坐标系与Map坐标系的位姿变换MRO,MtO所得，在算法初始化时由预先设定值作为初值；
3-2)为保证足够的点数用于匹配，且防止Map坐标系下位姿误差导致与步骤3-1)中切分的局部点云共视区域过小，将步骤2-2)中去畸变后的scan存入队列式数据结构，由多帧scan组成Odom坐标系下的滑动窗口内点云；
3-3)将步骤3-1)中Map坐标系下的局部点云和步骤3-2)中Odom坐标系下的窗口内点云进行ICP匹配，求解并更新Odom坐标系与Map坐标系的位姿变换MRO,MtO；
3-4)使用步骤2-3)中求得的Odom坐标系与Map坐标系的位姿变换MRO,MtO和步骤2-3)中求得的Odom坐标系下机器人位姿估计
得到Map坐标系下机器人位姿：
之后，Map坐标系下机器人位姿
能作为后续路径规划模块的输入。
5.根据权利要求4所述的一种基于固态激光雷达和点云地图的移动机器人导航系统，其特征在于：所述路径规划模块执行以下操作：
4-1)输入步骤1-5)中的栅格地图，将地图中高于设定阈值的网格视为障碍物，低于阈值的网格视为可通行区域；
4-2)持续接收步骤3-4)中的Map坐标系下机器人位姿，利用在步骤1-6)中记录的点云地图和栅格地图坐标系间变换关系，对齐两个坐标系，因此所得位姿即为机器人当前在栅格地图中的位置；
4-3)输入导航的目的地坐标，结合机器人当前位置与地图障碍信息做出路径规划，随后转为运动控制指令输出，完成导航。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310274750.7A CN116380039A (zh) | 2023-03-20 | 2023-03-20 | 一种基于固态激光雷达和点云地图的移动机器人导航系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310274750.7A CN116380039A (zh) | 2023-03-20 | 2023-03-20 | 一种基于固态激光雷达和点云地图的移动机器人导航系统 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116380039A true CN116380039A (zh) | 2023-07-04 |
Family
ID=86964899
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310274750.7A Pending CN116380039A (zh) | 2023-03-20 | 2023-03-20 | 一种基于固态激光雷达和点云地图的移动机器人导航系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116380039A (zh) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117589154A (zh) * | 2024-01-19 | 2024-02-23 | 深圳竹芒科技有限公司 | 自移动设备的重定位方法、自移动设备和可读存储介质 |
| CN118534899A (zh) * | 2024-05-13 | 2024-08-23 | 华智清创(苏州)农业科技有限公司 | 一种农业机器人农作物行间定位与导航的方法与装置 |
| CN118550929A (zh) * | 2024-07-26 | 2024-08-27 | 中铁四局集团有限公司 | 一种基于图像识别的预制梁场建图方法及系统 |
-
2023
- 2023-03-20 CN CN202310274750.7A patent/CN116380039A/zh active Pending
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117589154A (zh) * | 2024-01-19 | 2024-02-23 | 深圳竹芒科技有限公司 | 自移动设备的重定位方法、自移动设备和可读存储介质 |
| CN117589154B (zh) * | 2024-01-19 | 2024-05-24 | 深圳竹芒科技有限公司 | 自移动设备的重定位方法、自移动设备和可读存储介质 |
| CN118534899A (zh) * | 2024-05-13 | 2024-08-23 | 华智清创(苏州)农业科技有限公司 | 一种农业机器人农作物行间定位与导航的方法与装置 |
| CN118550929A (zh) * | 2024-07-26 | 2024-08-27 | 中铁四局集团有限公司 | 一种基于图像识别的预制梁场建图方法及系统 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113781582B (zh) | 基于激光雷达和惯导联合标定的同步定位与地图创建方法 | |
| CN112347840B (zh) | 视觉传感器激光雷达融合无人机定位与建图装置和方法 | |
| CN109974712A (zh) | 一种基于图优化的变电站巡检机器人建图方法 | |
| CN112634451A (zh) | 一种融合多传感器的室外大场景三维建图方法 | |
| Fruh et al. | Fast 3D model generation in urban environments | |
| CN116380039A (zh) | 一种基于固态激光雷达和点云地图的移动机器人导航系统 | |
| CN111664843A (zh) | 一种基于slam的智能仓储盘点方法 | |
| CN102831646A (zh) | 一种基于扫描激光的大尺度三维地形建模方法 | |
| CN114526745A (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
| CN110749895B (zh) | 一种基于激光雷达点云数据的定位方法 | |
| CN114004869A (zh) | 一种基于3d点云配准的定位方法 | |
| CN111862214B (zh) | 计算机设备定位方法、装置、计算机设备和存储介质 | |
| CN113720324A (zh) | 一种八叉树地图构建方法及系统 | |
| CN112652001B (zh) | 基于扩展卡尔曼滤波的水下机器人多传感器融合定位系统 | |
| CN115479598A (zh) | 基于多传感器融合的定位与建图方法及紧耦合系统 | |
| CN115639823A (zh) | 崎岖起伏地形下机器人地形感知与移动控制方法及系统 | |
| CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
| CN110736456A (zh) | 稀疏环境下基于特征提取的二维激光实时定位方法 | |
| Karam et al. | Integrating a low-cost mems imu into a laser-based slam for indoor mobile mapping | |
| CN116429116A (zh) | 一种机器人定位方法及设备 | |
| CN115984463A (zh) | 一种适用于狭窄巷道的三维重建方法和系统 | |
| CN116929338A (zh) | 地图构建方法、设备及存储介质 | |
| CN114529585A (zh) | 基于深度视觉和惯性测量的移动设备自主定位方法 | |
| CN115046543A (zh) | 一种基于多传感器的组合导航方法及系统 | |
| CN117029870A (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 |