基于2D激光雷达的移动机器人同步定位与地图构建算法研究.pdf

Modeling and Simulation 建模与仿真建模与仿真,2023,12(4),3299-3318 Published Online July 2023 in Hans.https:/www.hanspub.org/journal/mos https:/doi.org/10.12677/mos.2023.124303 文章引用文章引用:汤玉春.基于 2D 激光雷达的移动机器人同步定位与地图构建算法研究J.建模与仿真,2023,12(4):3299-3318.DOI:10.12677/mos.2023.124303 基于基于2D激光雷达的移动机器人激光雷达的移动机器人 同步定位与地图构建算法同步定位与地图构建算法 研究研究 汤玉春汤玉春 上海理工大学光电信息与计算机工程学院，上海 收稿日期：2023年4月12日；录用日期：2023年6月21日；发布日期：2023年6月30日 摘摘 要要 文中提出一种将激光雷达和轮式里程计相融合的方法，用于提升文中提出一种将激光雷达和轮式里程计相融合的方法，用于提升SLAM(Simultaneous Localization and Mapping)算法前端点云配准效率和点云畸变矫正效果。在点云配准方面，对算法前端点云配准效率和点云畸变矫正效果。在点云配准方面，对PL-ICP算法做出改进，首先算法做出改进，首先对激光点云进行预处理去除无效点，再利用自适应体素滤波方法对激光点云进行下采样，在保留点云特对激光点云进行预处理去除无效点，再利用自适应体素滤波方法对激光点云进行下采样，在保留点云特征的同时将点云稀疏化，从而减少点云配准的计算量，利用轮式里程计的测量值为点云配准提供初值，征的同时将点云稀疏化，从而减少点云配准的计算量，利用轮式里程计的测量值为点云配准提供初值，提高点云配准的效率和定位效果。在点云畸变矫正方面，按照轮式里程计测量的机器人位姿的时间戳，提高点云配准的效率和定位效果。在点云畸变矫正方面，按照轮式里程计测量的机器人位姿的时间戳，利用拉格朗日线性插值法对点云配准配得到的机器人位姿进行线性插值，利用利用拉格朗日线性插值法对点云配准配得到的机器人位姿进行线性插值，利用EKF算法融合轮式里程计算法融合轮式里程计测量的位姿和点云配准插值得到的位姿对轮式里程计的测量误差做出矫测量的位姿和点云配准插值得到的位姿对轮式里程计的测量误差做出矫正，然后为激光点云提供运动补正，然后为激光点云提供运动补偿，从而去除点云畸变提升偿，从而去除点云畸变提升SLAM算法定位和建图效果。利用算法定位和建图效果。利用ROS搭建仿真环境验证了本文提出的算法的搭建仿真环境验证了本文提出的算法的有效性。有效性。关键词关键词 SLAM，轮式里程计轮式里程计，PL-ICP，EKF，多传感器融合多传感器融合，点云畸变矫正点云畸变矫正 Simultaneous Localization and Map Construction Algorithm for Mobile Robots Based on 2D LiDAR Yuchun Tang School of Optical-Electrical and Computer Engineering,University of Shanghai for Science and Technology,Shanghai Received:Apr.12th,2023;accepted:Jun.21st,2023;published:Jun.30th,2023 汤玉春 DOI:10.12677/mos.2023.124303 3300 建模与仿真 Abstract This paper proposes a method for fusing data from a lidar and wheel odometry to improve the efficiency of front-end point cloud registration and distortion correction in SLAM(Simultaneous Localization and Mapping)algorithms.Specifically,in terms of point cloud registration,the paper improves the PL-ICP algorithm by first preprocessing the lidar point cloud to remove invalid points,and then using an adaptive voxel filtering method to down sample the lidar point cloud,which reduces the computational load of point cloud registration while preserving the point cloud features.In addition,the paper uses the measurement values from the wheel odometry to provide an initial value for point cloud registration,which improves the efficiency and accuracy of point cloud registration.In terms of point cloud distortion correction,the paper uses the timestamp of the robot pose measured by the wheel odometry,and applies Lagrangian linear interpolation to linearly interpolate the robot pose obtained from point cloud registration.The paper then uses the EKF algorithm to fuse the robot pose measured by the wheel odometry and the pose obtained from point cloud registration to correct for the measurement errors of the wheel odometry.Final-ly,the paper provides motion compensation for the lidar point cloud to remove point cloud dis-tortion and improve the localization and mapping effects of the SLAM algorithm.The paper veri-fies the effectiveness of the proposed algorithm by using ROS to build a simulation environment.Keywords SLAM,Wheel Odometry,PL-ICP,EKF,Multi-Sensor Fusion,Point Cloud Distortion Correction Copyright 2023 by author(s)and Hans Publishers Inc.This work is licensed under the Creative Commons Attribution International License(CC BY 4.0).http:/creativecommons.org/licenses/by/4.0/1.引言引言 SLAM 的关键技术根据传感器在陌生环境中获取自身的位姿信息和环境信息，估计自身运动的位置进行实时定位并利用这些信息增量式的构建环境地图1，SLAM 算法按照传感器不同可分为基于相机的视觉 SLAM 2 3基于激光雷达的激光 SLAM 4 5。相机使用便捷、能够获取丰富的环境信息、但容易受到外部环境的干扰。激光雷达抗干扰能力强、受光照影响弱、能直接获取深度信息。其中 2D 激光雷达因其售价便宜，对算力要求低，易于构建栅格地图等特点而被广泛应用于机器人领域。仅使用激光雷达进行定位与建图时，在变速场景和特征较少的退化场景中点云配准效率低、定位效果差，在移动机器人快速运动时激光点云易发生畸变，最终导致建图效果差，因此多传感器的融合6 7成了近几年的研究热点。Chen S 等人8将激光雷达和相机结合，通过构建视觉相似性的词袋模型提升回环检测的效果，以提高 SLAM 算法的后端优化性能，但在纹理缺失的场景鲁棒性较差。章弘凯等人9提出将 IMU 和激光雷达相结合，用 IMU 的预积分结果为点云配准提供初值提升点云配准效率，同时将其积分结果作为约束条件加入到 SLAM 算法的后端优化中减小轨迹飘移的影响提高载体的定位精度，但IMU 对线加速度进行二次积分的误差较大。Hess W 等人10提出了基于图优化的 Cartographer 算法，经过不断的发展已经能和 GPS、IMU、轮式里程计等多种传感器融合，但仅将辅助传感器的数据用于未来帧的位姿推，并未用于点云畸变矫正。激光点云畸变会使环境地图发生偏移，进而影响到后端优化和回环的效果最终影响移动机器人定位Open AccessOpen Access汤玉春 DOI:10.12677/mos.2023.124303 3301 建模与仿真 精度和建图效果。点云畸变的矫正最直接的方法是利用高频率的激光雷达，在激光雷达频率较高情况下点云畸变可以忽略，但这会带来高昂的硬件成本。M.Roh 等人11利用 GPS/INS 传感器对点云信息进行校正，在每次扫描的同时获取激光雷达载体的确切位置和方向，但是其测量方式依赖卫星，因此难以应用于室内或隧道等封闭环境。基于帧间配准的纯运动估计法因其不需要额外辅助的传感器而被广泛的应用于点云畸矫正12 13 14。如果激光雷达载体在一个扫描周期内的运动状态非匀速或则激光频率太低，使用该方法对点云矫正的效果并不理想。Yang W 等人15提出一种激光雷达和相机融合方法，通过全速度估计来进行点云畸变矫正。激光雷达在径向方向上能精确测量距离信息，但只能使用稀疏的角度信息，而相机作为补充传感器可以提供密集的角度分辨率，此外，还利用概率卡尔曼滤波方法融合估计的速度提升点云矫正效果，但相机提供的信息易受环境干扰。Zhang B 等人16提出利用 UKF 融合激光雷达和 IMU 数据的方式进行点云畸变矫正，UKF 用于融合 IMU 的角速度、线加速度和激光雷达的位姿，得到 IMU 的角速度偏差、线加速度偏差和激光雷达的线速度，然后得到激光雷达在一个扫描周期内各时刻的位姿，从而消除点云的失真得到激光雷达在一个扫描周期内各时刻的位姿，从而消除点云的失真。虽然 IMU 能测量激光雷达载体的角速度和线加速，但线速度累计误差较大。轮式里程计能够直接测量移动机器人的位移和角度，具有较高的局部位置测量精度和局部角度测量精度，并且有较高的位姿更新频率，通常能达到 100200 Hz 左右，本文将轮式里程计和激光雷达相融合用于点云畸变矫正，和提升在不同场景下的点云配准效率和定位效果。2.移动机器人模型移动机器人模型与点云配准与点云配准 2.1.移动机器人运动模型移动机器人运动模型 轮式里程计通过测量移动机器人两主动轮的转速得到移动移动机器人的线速度 v 和角速度 w，从而推算出移动机器人在世界坐标系下的位姿变化信息。在世界坐标系下移动机器人两轮差分底盘运动学模型如图 1 所示。Figure 1.Two-wheel differential chassis kinematic model 图图 1.两轮差分底盘运动学模型 设左轮转速为lw，半径为lr，右轮的转速为rw半径与左轮相同，两轮间的距离为 2d，移动机器人的角速度为和线速度分别为 w 和 v，计算如式(1)。2222lrlllrrrrrwwvJrwrwwdd=(1)汤玉春 DOI:10.12677/mos.2023.124303 3302 建模与仿真 在间隔时间ot极短时，假设其线速度角速度恒定，移动机器人运动变化的圆心角的计算如式(2)。2122lrwJ wJ ww t=+=(2)()()11121111sincoslrcttcttvJ wJ wvxxwvyyw=+=+(3)()()1111111sinsincoscosttttttttttvvw twwxxvvyyw twww t+=+(4)设()T1111,ttttpxy=表示移动机器人上一时刻在世界坐标系中的位姿，由几何关系可得角、和相等，(),ccxy表示其运动半径的圆心所在的位置，其计算如式(3)，则当前时刻移动机器人在世界坐标系的位姿()T,ttttpx y=计算如式(4)。2.2.激光点云配准原理激光点云配准原理 激光 SLAM 建图主要分为前端点云配准，后端优化，和回环检测等三个部分。点云配准即寻求一组旋转和平移参数使得对齐后的上一帧点云和参考点云在同一参考坐标系下达到最大的重叠，通过求解的旋转和平移参数恢复出两帧点云数据扫描期间的移动机器人的位姿变化信息。常用的二维激光点云配准算法有 ICP 17算法和 PL-ICP 18算法，ICP 算法是基于点对点的距离扫描匹配算法，构造点到点的距离误差函数。123,jjjjjiCcccc=表示当前帧的扫描点云，11111123,jjjjjiCcccc=表示与之对应的上一帧点云，N 表示一帧点云中激光点的个数。构建误差方程如式(5)寻求一组解()(),Rt使得误差方程的值最小。()()()2111,pNjjiiiE RtcRctN=(5)PL-ICP 则是在 ICP 的基础上利用点到线的距离作为误差取代了点到点的距离作为误差。激光点实际上是对真实环境中曲面的离散采样，最好的误差尺度是激光点到实际曲面的距离，而 PL-ICP 采用分段线性的方法对实际曲面进行近似，用当前帧点云中的激光点到上一帧点云中对应最近两激光点连线的距离来近似实际激光点到曲面的距离。PL-ICP的误差方程更贴近实际情况，因为比起直接计算点到点的距离，其考虑了点云的局部结构，因此精度更高不容易陷入局部最优，对场景的适应性和收敛精度都高于 ICP。2.3.点云配准初值获取点云配准初值获取 无论使用哪种点云配准方法，若没有为点云配准提供准确的初值，点云配准的定位性能会快速劣化甚至完全失效。文中将轮式里程计和激光雷达相融合对 PL-ICP 算法做出改进，提高点云匹配的效率和对不同场景的适应性，具体流程如图 2 所示。在长廊和特征物极少的退化场景中，由于没有参照物激光雷达的观测数据在某一维度高度相似，如图 3 所示。随着机器人的移动雷达坐标系由1jCO移动到jCO处，但在两个坐标系下的激光点jic和1jic的 汤玉春 DOI:10.12677/mos.2023.124303 3303 建模与仿真 Figure 2.Flow chart of the improved PL-ICP algorithm 图图 2.改进的 PL-ICP 算法流程图 Figure 3.Lidar coordinate transformation 图图 3.激光雷达坐标变化 距离信息完全一样，在这种情况下通过点云配准得到的机器人位姿变化信息接近为零。在有初值的情况下点云jC变换到点云1jC，参考坐标系由jCO变换到1jCO，再由点云1jC继续和上一帧点云1jC进行配准，配准误差可以明显减小，且初值越准确点云配准效果越好。获取点云配准初值常用的方法是利用匀速预测模型，由上一次激光点云配准的结果基于匀速假设计算激光雷达载体的运动速度，并将计算的结果用于推测未来帧的激光雷达载体在世界坐标系下的位姿，计算出激光雷达的相对位姿变化信息，然后将其用于点云配准的初值。但在变速场景和雷达频率较低的场景中匀速预测模型提供的初值误差较大，不能准确反应出激光雷达载体的位姿变换情况，最终导致点云配准的结果无法快速收敛和定位失败。轮式里程计的频率远高于激光雷达，在一帧激光扫描时期内可以多次测量机器人位姿，因此将轮式里程计的测量值用于点云配准的初值能准确的反应出在一帧扫描时间内机器人的位姿变化。点云配准初值()(),sesekkkt tt tbtt=，由轮式里程计测量的移动机器人在世界坐标系下的位姿计算相对位姿变换矩阵得到。已知一帧激光点云扫描的起止时间为st和et。轮式里程计测量机器人位姿的频率远高于激光雷达，在st和et之间存在多帧机器人位姿信息，两个传感器的测量值存在不同的时间戳，如图 4 所示，因此需要进行时间同步。Figure 4.Different sensor timelines 图图 4.不同传感器时间轴 用11,lseses ne ntt t tttt+=表示激光点云的时间轴，1234(1),ooooo jojottttttt=表示里程计的时间轴。利用两个双端容器ldeq，odeq分别存储点云数据和里程计信息进行时间同步，每拿到一帧点云数据则在odeq容器中搜索对应时间的机器人位姿，若st、et时刻没有对应的机器人位姿则分别搜索最接近st、et时刻的位姿，然后利用线性插值得到对应的位姿，为了增加搜索效率每次对odeq中小于et时刻汤玉春 DOI:10.12677/mos.2023.124303 3304 建模与仿真 的位姿信息进行删除。假设st时刻对应的前后两帧里程计的时间戳为1ot、2ot，对应机器人位姿为1otp、2otp，则st时刻的机器人位姿stp计算如式(6)同理可计算得到etp。轮式里程计测量得到的移动机器人位姿为1 7的矩阵组成，由世界坐标系下的距离信息和四元数组成如式(7)。将stp和etp转换成4 4矩阵形式用stT和etT表示，其相对位姿变换矩阵用esttT表示，计算如式(8)，计算,set tt和,set t的值如式(9)。21122121sooootttoooottttppptttt=+(6),pX Y Z x y z w=(7)1=essettttT TT (8)()()()()()0,3,1,3,1,00,0=tan2,esseeseesessttt tttttt tttTtTaTT (9)2.4.自适应体素滤波自适应体素滤波 一帧点云激光点较多，如果将所有激光点都用于点云匹配计算量较大，并非所有激光点数据都有效，有的导致配准结果无法收敛，如激光打在桌子边缘导致返回的激光强度低、小于或大于有效距离而产生的 inf 点和 nan 点，这些噪声点可能会导致两帧点云无法通过旋转平移重叠。因此需对点云数据进行预处理去除噪声点以提高点云配准效率。设定最大测距阀值maxd和最近离阀值mind，初始扫描点jic与maxd和mind进行距离比较去除无效点。距离激光雷达近的物体可以扫描得到更多的激光点，这就导致点云比较密集，同时也会出现大量的重叠点，而远处的点则相对稀疏。为了降低计算量基于体素滤波对点云进行下采样，在保存点云特征的同时降低点云的稀疏性，同时能够在一定程度上去除离群点。已知点云数据在 X、Y、Z 轴上分布的最大最小值记为maxx、minx、maxy、miny、maxz、minz，计算体素栅格的边长xl、yl、Zl如式(10)。maxminmaxminmaxminxyZlxxlyylzz=(10)设置体素小栅格的边长为vl，将 X、Y、Z 三轴均分为 l、w、h 份，则可以划分为lw h个体素小栅格，其中sum表示体素小栅格的个数sumlw h=，.表示向下取正，其中 l、w、h 的计算如式(11)。对每个激光点所属的体素小栅格进行编号确定每个激光点所属的体素小栅格，设编号为(),f j k，其中 f、j、k 的计算如式(12)。yxzvvvllllwhlll=(11)minminmin iiivvvxxyyzzfjklll=(12)n 表示每个小栅格内的激光点数，计算每个体素小栅格的质心ig替代栅格内的其他激光点，其计算如式(13)。1njjiiigCn=(13)汤玉春 DOI:10.12677/mos.2023.124303 3305 建模与仿真 得到滤波后的点云123,jjjjjiCgggg=，若质心点不存在则用距离质心最近的点替代其他点。滤波阈值vl的选取对点云的配准效果至关重要，vl选取过大会使点云过于稀疏导致有效特征点太少而使得点云配准失败。vl选取太小，虽然能保留足够的激光点，但并不能达到减少点云配准计算量的目的。机器人在移动过程中通常会遇到空间环境变化明显的情况，如从走廊进入室内，如果使用固定的vl值进行体素滤波，在不同的场景下滤波后的激光点会相差数倍，采用固定滤波阀值会影响到点云配准的效率。因此本文使用自适应体素滤对滤波阀值的选取做出改进，利用固定的激光点数取代固定的滤波阀值。设置最大滤波阀值maxl和最小滤波阀值minl进行体素滤波，如果滤波后的激光点数远小于或大于目标点数则利用二分法继续滤波，如果体素滤波后激光点数接近目标点数，则输出滤波后的点云。滤波后的平均激光点数与原始激光点数的比值用 表示。在点云配准搜索对应点的时候，找到其中一个点后，另一个点通常在其附近，但滤波后的激光点为无序状态，为了提升搜索对应点的效率，将滤波后的激光数据按照角度递增的关系进行排序。2.5.点云配准迭代点云配准迭代 计算当前帧的激光点在上一帧激光点云所在激光雷达坐标系下的位置wig如式(14)，其中(),kkkbt=。对激光点wig在上一帧点云1jC中寻找距离它最近的两个激光点，其下标记为 i 和1i+，设in为1jig和11jig+连线的法向量，构建误差方程如(15)求解1kb+使得误差方程的值最小。()wiikkikgbgRgt=+(14)()()()2T11111wkikikiijERgtgbn+=+(15)将1kb+作为下一次迭代的初值计算得到新的点云1jC继续和上一帧点云1jC进行配准，求解结果用2kb+表示，循环迭代 n 次以后直到 n 等于配准次数的阀值或则k nb+小于配准误差阀值则停止迭代。将每次求解的结果用矩阵k ibT+表示，则最终两帧点云的配准结果11jjk nk nkcbbbcTTTT+=，迭代过程如图 5 所示。机器人初始时刻在世界坐标系下的位姿矩阵为0T，经过 j 帧点云扫描配准后定位得到的移动机器人在世界坐标系中的位姿矩阵321210jjcccjcccTTT TT=。改进 PL-ICP 算法的伪代码如表 1。3.激光点云畸变矫正激光点云畸变矫正 3.1.点云畸变原理分析点云畸变原理分析 激光雷达通过旋转能够覆盖 360的视野，在一次测量中，每束激光产生一个包含距离和角度信息的单点，一圈扫描结束所有激光点的集合构成一帧点云信息。常用的二维激光雷达的频率在 520 Hz，这就意味着激光雷达旋转一周获取周围环境的信息需要 0.2 s0.05 s，如果激光雷达在一个扫描周期内是静止的则一帧点云中每个激光点对应的移动机器人位姿是相同的，都为激光雷达扫描开始时的第一个激光 Figure 5.Point cloud alignment iterative process 图图 5.点云配准迭代过程 汤玉春 DOI:10.12677/mos.2023.124303 3306 建模与仿真 Table 1.Improved PL-ICP algorithm pseudocode 表表 1.改进 PL-ICP 算法伪代码 1 input:点云数据jC，里程计数据tp 2 output:一帧点云扫描期间的机器人位姿变换信息矩阵1jjccT 3 begin:4 do 5 jotldeqp deqC/将轮式里程计数据和点云数据存入对应的双端队列 6 while(ldeq.size)delete/数据预处理去除无效点 12 do 13 num1=VoxelFilter(Cj,lmax,lmin)/利用体素滤波对点云进行下采样num1、num2分别表示滤波后的激光点数和目标激光点数 14 mid=(lmax+lmin)/2 15 if(num1num2)lmax=midelse lmin=mid 16 Whilenum11.1*num2 17 Sort(Cj)/利用选择排序将滤波后的激光点按照角度递增的关系进行排序 18 do 19 fori=1:n 20 FindClosePoint(Cj,Cj1)21()()()2T11111wkikikiijERgtbnc+=+22 1kjjbCTC+=;111jjjjkccbccTTT+=;wjiigc=23 while 1kb+/直到配准误差小于设定的值停止迭代 24 return 1jjccT 25 end 点所对应的移动机器人位姿。激光雷达测量到的物体的位置信息由激光雷达坐标系转换到机器人坐标系再转换到世界坐标系从而得到被测物体在整个地图中的位置。激光雷达在进行 360 度扫描时会伴随着移动机器人的移动，这导致单个激光点在空间中有不同的参考位置，但激光雷达的点云数据在进行封装时会默认所有激光点的信息是在同一机器人位姿下瞬时获取的，因此点云数据会失真。如图 6 所示假设激光雷达的频率是 10 Hz，逆时针方向旋转，搭载激光雷达的移动机器人沿 x 方向运动，线速度为 1 m/s，机器人由位置 A 在激光雷达的四分之一个扫描周期内经过位置 B 运动到了位置 C，AC 的距离为 2.5 cm。激光雷达在位置 A 处扫描得到激光点 s，在位置 B 处扫描得到激光点 p，在位置 C 处扫描得到激光点 t，但所有激光点都会以 A 的位置为参考坐标系将其转换到世界坐标系下，因为忽略了点 C 所在的位置，最终呈现在地图中激光点t的位置会在点f处，以此类推当前帧的整个点云都会发生偏移如图中的点云2C，而点云1C才是激光雷达扫描得到的真实点云，点云2C为由于机器人移动引起的畸变的点云。点云矫正的目的就是将点云2C还原成点云1C。汤玉春 DOI:10.12677/mos.2023.124303 3307 建模与仿真 Figure 6.Point cloud distortion principle 图图 6.点云畸变原理 3.2.点云矫正流程点云矫正流程 消除点云畸变的关键是还原出激光雷达载体在一帧点云扫描周期内准确的位姿信息。基于匀速预测模型的纯估计法对激光点云进行畸变矫正的主要步骤：1)对于一帧激光点云中的每一激光点都赋予对应的时间戳，假设激光雷达载体匀速运动，通过帧间匹配的位移除以时间求出激光雷达载体的移动速度；2)根据每一个激光点对应的时间戳进行运动补偿。由于运动控制器噪声的存在移动机器人运动并非匀速的，且在实际应用中移动机器人的使用不局限于匀速场景。轮式里程计能够直接测量移动机器人的位移和角度，具有较高的局部角度测量精度和局部位置测量精度，并且更新频率远高于激光雷达，相比于匀速预测模型轮式里程计的测量值能够更加准确的反应机器人的实际运动情况。设当前帧激光雷达扫描的起始时间为st终止时间为et，lsettt=，激光雷达的频率为lf，一帧里程计的时间为ot，频率为of，olkff=。算法的整体流程如下：1)通过改进的 PL-ICP 算法配准得到lt时间内移动机器人的相对位姿变化信息，通过线性插值得到lt时间内 k 个机器人位姿tlT，计算如式(16)。2)使用 EKF 算法融合改进 PL-ICP 算法配准后插值得到的机器人位姿和轮式里程计测量的位姿，得到矫正后的机器人位姿信息，对矫正后的位姿进行线性插值，使每个激光点对应一个移动机器人位姿。3)已知机器人位姿通过坐标变换把所有激光点转换到以第一个激光点对应雷达位姿为基准的坐标系下，得到矫正后的点云数据。1jjtscsltcesttTTTtt=(16)3.3.EKF 数据融合数据融合 在 SLAM 算法中卡尔曼滤波算法19常用来做多传感器数据的融合，以达到传感器载体位姿矫正的目的，其原理是利用上一时刻矫正后的载体位姿通过状态转移方程得到当前时刻的预测位姿，同时利用当前时刻的观测值修正当前时刻的预测值，得到当前时刻矫正后的位姿。但卡尔曼滤波不适用于非线性系统，为了解决非线性问题扩展卡尔曼滤波算法20在卡尔曼滤波算法的基础上对状态转移方程和观测方程进行一阶泰勒展开达到线性化的目的。移动机器人的运动往往是非线性的，因此本文利用扩展卡尔曼滤波算法进行数据融合。输入为轮式里程计测量位姿tp和激光雷达点云配准插值的位姿tl，输出为矫正后的机器人位姿t，其中tl由tlT转换为 1 3 的矩阵得到。将移动机器人的运动模型分解为一个无噪声分量和一个随机噪声分量如式(17)，式中(),tttuv w=表示控制量，对式(17)进行一阶泰勒展开线性化得到式(18)。,p tG和,u tG表示方程().g关于 p 和 u 求导的雅克比矩阵。,p tG和,u tG的计算如式(19)和(20)。汤玉春 DOI:10.12677/mos.2023.124303 3308 建模与仿真 ()()1,0,ttttpg upNR=+(17)()()()11,11,ttttp tttg upg uGp+(18)()()()1,1,1,1,11,1,1,1,1,1,1,1,1,1,10coscos01sinsintxtytttp tttxtyttxtytttttttttttttttxxxg uyyyGpvvwtwwvvwtww=+=+001 (19)()()()()()()()()()1,22,sinsincossinsincoscossincoscos0ttttu ttttttttttttttttttttxxvwg uyyGuvwvwvwtwwttwtwwvwtwwttwtwwt=+=(20)扩展卡尔曼滤波的预测方程如式(21)(22)，当前时刻移动机器人预测位姿期望值tp由上一时刻校正后的位姿期望值和无误差项的移动机器人运动模型得到，t表示位姿的协方差矩阵。T,u ttu tG M G表示控制空间运动噪声向状态空间运动噪声的近似映射。()()11111sinsincoscosttttttvvw twwvvpw twww t+=+(21)TT,1,tp ttp tu ttu tGGG M G=+(22)观测模型(),ttzh p l a=描述的是tp和tl的相对位置信息，如式(23)，a 是传感器的误差()(),0,ra aNA。其中()T,zr=，r 是距离，是方位角。汤玉春 DOI:10.12677/mos.2023.124303 3309 建模与仿真 ()()()()22,1,tanrt xt xt yt yt yt yt xt xaplplZaplpl+=+(23)22rA=(24)观测模型的泰勒展开线性近似如式(26)，式(27)为h关于机器人位姿的雅克比矩阵。()()22,t xt xt yt yqll=+(25)()()(),ttttttth p lhlHpp+(26)(),0,1t yt yt xt xttttt yt yt xt xlplph p lqqHplplpqq=(27)()10,01ttwh p lHz=(28)计算卡尔曼增益矩阵K如式(29)TTT1tttwwtttSHHH AHKH S=+=(29)()()()()22,1,tant xt xt yt yt yt yt xt xplplzplpl+=(30)状态更新得到矫正后的机器人位姿的期望值和协方差如式(31)。()()tttttttttpKzzIK H=+=(31)3.4.点云数据矫正点云数据矫正 从st到et时间段一共有 k 个机器人位姿122,ssss kettttt+，位姿由 EKF 融合轮式里程测量位姿和帧间配准插值的位姿得到。在 k 个机器人位姿之间进行线性插值，设st和1st+之间有2n个位姿12(2)1,ssss nsttttt+，插值计算如式(32)。1111sssstttsssstttttttt+=+(32)在获取每个激光点对应的位姿信息后需要将所有激光点转换到以第一个激光点为基准的坐标系下。如图 7 所示，wO表示世界坐标系，rO表示机器人坐标系，lO表示雷达坐标系。设ip(),wwiixy为某一帧激光进行信息采集时第一束激光采集信息时机器人所在的位置，该位置所对应的激光雷达坐标系也作为基坐标系。在一帧点云扫描期间激光束扫描到障碍物时产生激光点sip的距离和角度信息，此时机器人所在位置为1ip+()11,wwiixy+，此时的机器人坐标系用2rO表示，雷达坐标系用2lO表示，2lO坐标系下的激光点sip的数据信息存储一帧点云中，存储数据时参考的雷达坐标系为lO，点sip的位置经过坐标的旋转和平移变换后相对于世界坐标系wO的位置在sip处。在已知1ip+的情况下将sip的位置转换到lO坐标 汤玉春 DOI:10.12677/mos.2023.124303 3310 建模与仿真 Figure 7.Point cloud coordinate transformation 图图 7.点云坐标变换 系下即可得到校正后的激光点，其中1itp+=。设risit和risiR分别表示从激光雷达坐标系到机器人坐标系的平移向量和旋转矩阵，wirit和wiriR分别表示从机器人坐标系到世界坐标系的平移向量和旋转向量。rip和wip分别表示激光点在机器人坐标系和世界坐标系下的位置，其计算如式(33)(34)。ririsirisisipR pt=+(33)()wiwiriwiwirisiriwiriririsisiripR ptRR ptt=+=+(34)将其转换为齐次形式如式(35)(36)wiwirisirisipT Tp=(35)0101 1wiwiwiwisiwiwisiririririririTTRtRtpTTp=(36)再将激光点由世界坐标系转换到换到以lO为原点的坐标系下用sisp表示，计算如式(37)，最终得到矫正后的点云新信息。()10siwiwisripTpp=(37)4.实验实验与分析与分析 4.1.实验说明实验说明 基于 ROS(Robot Operating System)操作系统进行仿真实验，验证本文提出的算法。利用 Gazebo 平台构建 3D 仿真室内环境和仿真移动机器人如图 8 所示，整体为矩形长 40 m，宽 20 m，在室内和过道均布置有不同的障碍物构成结构化场景如图 8，世界坐标系原点在图中蓝色直线处，垂直向左为 x 轴正方向，垂直向下为 y 轴正方向。所用平台为 Ubuntu20.04，ROSkinetic，搭载处理器为 i7-6500U 运行内存为 4G的计算机。仿真机器人底盘为四轮模型，底盘为左右两个主动轮和前后方各一个万向轮，搭载有单线激光雷达和里程计。轮式里程计频率为 100 Hz，测量误差为0.1%，激光雷达频率为 10 Hz，测量最远距离8 m，最小 0.05 m，水平视角 360，角度分辨率为 0.36，测量误差0.02 m。汤玉春 DOI:10.12677/mos.2023.124303 3311 建模与仿真 Figure 8.Experimental scenes 图图 8.实验场景 实验分为三个部分：1)在室内结构化场景中和退化场景中，机器人处于变速运动的情况下，对比本文提出的改进的点云配准算法和其他点云配准算法在点云配准效率和定位效果上的差异。2)讨论点云稀疏性对点云配准效率和定位效果的影响。3)将本文提出的融合轮式里程计进行点云畸变矫正的方法移植到 Cartographer 算法中分别对比无点云畸变矫正、基于匀速预测模型进行点云畸变矫正的 Cartographer算法在回环误差和场景还原度方便的差异。1)点云配准效率：以点云配准的平均耗时和平均迭代次数为准