基于扫地机器人的自适应激光雷达点云校正与定位方法技术

本发明专利技术提供了一种基于扫地机器人的自适应激光雷达点云校正与定位方法，包括以下步骤：S1.通过搭载在机器人上的激光雷达采集一帧新的数据；S2.通过轮式里程计采集距离d和旋转角度θ，直到采集激光数据帧一个周期结束，推测估计机器人在每个里程计时间戳对应的位姿；S3.考虑到误差的存在，对机器人位姿拟合，计算拟合结果后的均方差；S4.计算每个激光点云相对于扫地机器人的位姿T

【技术实现步骤摘要】
基于扫地机器人的自适应激光雷达点云校正与定位方法
本专利技术涉及机器人定位领域，更具体地，涉及基于扫地机器人的自适应激光雷达点云校正与定位方法。
技术介绍
随着科学技术的发展与进步，常常用于军事领域的激光雷达的成本大大降低，这使得它在商业应用上成为可能，激光雷达具有单色性好，亮度高，方向性强，抗干扰性强，分辨力强，设备小而轻等优点，尤其在测速与机器人定位方面有着广泛的应用。轮式里程计能够快速地测量移动距离和转动角度，短时间内和不打滑情况下具有较高的定位精度，且编码器价格便宜，适合商业应用。扫地机器人作为第一批商用的服务机器人，已经走进了千家万户，极大地减轻了家庭清扫清洁的负担。传统的扫地机器人仅仅使用激光雷达传感器进行定位，由于激光雷达在扫射的过程中机器人会发生平移或旋转等运动，得到的激光点云以及生成的地图会有一定程度的畸变，且畸变程度与机器人运动速度成正比，特别是旋转运动对激光点云的畸变影响巨大，因此要对激光点云进行校正；有的采用里程计插值的方法对数据进行校正，但只用到了少数里程计数据而忽略了机器人整体运动情况(LOAM:LidarOdometryandMappinginReal-time)，没考虑到里程计测量噪声的存在，从而不能准确地校正。工程上往往采用直线运动模型校正，但是对快速运动的机器人校正效果有限，鲁棒性不高。而采用多项式拟合的方式估计机器人运动，但是对于做直线运动的机器人往往会导致过拟合，从而丢失运动的整体信息，校正结果不准(ImprovedTechniquesforGridMappingwithRao-BlackwellizedParticleFilters)。
技术实现思路
本专利技术的目的在于克服现有的技术和算法的缺点与不足，提供了一种基于扫地机器人的自适应激光雷达点云校正与定位方法。同时开启两个线程对运动模型进行拟合，自适应选取最佳模型，进行点云校正，并对ICP算法提供初始值，加快ICP算法的收敛。本专利技术至少通过如下技术方案之一实现。基于扫地机器人的自适应激光雷达点云校正与定位方法，包括以下步骤：S1.扫地机器人在运动时，通过搭载在机器人上的激光雷达采集一帧新的数据，假设上一帧数据发布出去后，时间戳为ts，扫地机器人在k-1时刻的位姿为Tk-1，发出当前激光帧之后的k时刻位姿为Tk，位姿变换为Tk，k-1；S2.通过里程计不断采集运动距离d和旋转角度θ，直到采集激光数据帧一个周期结束，时间戳为te，使用机器人航迹推测模型估计机器人在每个里程计时间戳对应的位姿；S3.使用直线运动模型和曲线运动模型对机器人位姿拟合，计算拟合结果后的均方差；S4.选取产生最小均方差的模型作为这一个周期内的运动模型，计算每个激光点云相对于扫地机器人的位姿Tk-1的坐标P′k-1；S5.获取扫地机器人的位姿Tk上的激光点云坐标，并把相邻两帧数据进行ICP匹配，获取ICP匹配数据集，计算位姿变换Tk，k-1，进而得到扫地机器人的位姿Tk。进一步地，扫地机器人坐标系与搭载的激光雷达坐标系之间的坐标变换Tbl通过定点标定的方式得到。进一步地，选取扫地机器人坐标X轴方向作为激光雷达扫描角度为零时扫地机器人的指向，Z轴指向机器人上方，XYZ构成右手系；选取机器人本体作为世界坐标系，测量若干组激光扫描点P在机器人下的位置，从而通过定点标定的方式得到坐标变换Tbl。进一步地，步骤S2中所述的轮式里程计的工作频率为200Hz，激光雷达的工作频率为5Hz，在一个激光周期内不断采集里程计发布的运动距离d和扫地机器人的旋转角度θ。进一步地，设相邻里程计数据前后机器人的位姿为[xt-1,yt-1,θt-1]T和[xt,yt,θt]T，r＝d/θ，r为运动轨迹半径，步骤S2中所述的机器人航迹推测模型是：进一步地，步骤S3中所述的直线运动模型的拟合方程为：其中a、b、c、d、e、f表示待确定的线性方程系数，t表示时刻；误差模型为：其中wi为i时刻位姿的置信权重；[xiyiθi]表示机器人i时刻的位姿；使用Ransac(RandomSampleConsensus)算法估计机器人位姿[xyθ]，曲线运动模型的拟合方程为：其中a、b、c、d、e、f、g、h表示待确定的线性方程系数，t表示时刻；误差函数为：其中：其中σ为方差、n表示估计的位姿个数；使用最小二乘拟合算法求解机器人位姿[xyθ]，求解方法为高斯-牛顿法，且同时开启两个线程分别计算。进一步地，步骤S3拟合的机器人位姿曲线经过扫地机器人的位姿Tk-1，对于每个里程计位姿赋予权重w，且w随时间递减。进一步地，步骤S4中，假设相邻激光点云之间的时间间隔为δt，估计出来的机器人位姿为Tk-1,i对应的激光点云坐标为[ρk-1,iθk-1,i]T，转换成笛卡尔坐标系为Pk-1,i＝[ρk-1,icosθk-1,iρk-1,isinθk-1,i]T，转换到扫地机器人的位姿Tk-1下的坐标P′k-1为：P′k-1,i＝Tk-1,iPk-1,i。进一步地，步骤S5所述的ICP(IterativeClosestPoint)匹配数据集为：Pk-1＝{Pk-1,i,i＝1...n},Pk＝{Pk,i,i＝1...n}最小化误差函数为：先去中心化，后采用SVD(SingularValueDecomposition)分解求解，其中，R和t是从第k-1帧到第k帧的机器人位姿变换的旋转矩阵和平移向量，表示二范数。进一步地，步骤S5扫地机器人的位姿Tk如下：其中，迭代的初始R和t来自于步骤S4所选取的运动模型估计值。与现有技术相比，本专利技术技术方案的有益效果是：1.本专利技术采用了里程计数据运动模型拟合和自适应运动模型选取机制，拟合机制比依靠里程计数据插值结果更可靠，综合考虑了扫地机器人沿直线运动和作曲线运动两种场景，根据拟合结果的好坏，自适应选取运动模型，更加精确地确定每一个激光点云的实际坐标。2.本专利技术拟合的运动模型对两相邻关键帧位姿的预测可作为ICP算法的初始值，从而加快收敛速度。3.本专利技术采用双线程的方式，在做运动模型预测时，兼顾了精度和计算效率的需求。附图说明图1为本实施例基于扫地机器人的自适应激光雷达点云校正与定位方法的总流程图；图2为本实施例航迹推测原理图；图3为本实施例运动模型拟合效果图。具体实施方式附图仅用于示例性说明，不能理解为对本专利技术的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。下面结合附图和实施例对本专利技术的技术方案做进一步的说明。如图1所示，基于扫地机器人的自适应激光雷达点云校正与定位方法，包括以下步骤：S1.扫地机本文档来自技高网...

【技术保护点】
1.基于扫地机器人的自适应激光雷达点云校正与定位方法，其特征在于，包括以下步骤：/nS1.扫地机器人在运动时，通过搭载在机器人上的激光雷达采集一帧新的数据，假设上一帧数据发布出去后，时间戳为t

【技术特征摘要】
1.基于扫地机器人的自适应激光雷达点云校正与定位方法，其特征在于，包括以下步骤：
S1.扫地机器人在运动时，通过搭载在机器人上的激光雷达采集一帧新的数据，假设上一帧数据发布出去后，时间戳为ts，扫地机器人在k-1时刻的位姿为Tk-1，发出当前激光帧之后的k时刻位姿为Tk，位姿变换为Tk，k-1；
S2.通过里程计不断采集运动距离d和旋转角度θ，直到采集激光数据帧一个周期结束，时间戳为te，使用机器人航迹推测模型估计机器人在每个里程计时间戳对应的位姿；
S3.使用直线运动模型和曲线运动模型对机器人位姿拟合，计算拟合结果后的均方差；
S4.选取产生最小均方差的模型作为这一个周期内的运动模型，计算每个激光点云相对于扫地机器人的位姿Tk-1的坐标P′k-1；
S5.获取扫地机器人的位姿Tk上的激光点云坐标，并把相邻两帧数据进行ICP匹配，获取ICP匹配数据集，计算位姿变换Tk，k-1，进而得到扫地机器人的位姿Tk。


2.根据权利要求1所述的基于扫地机器人的自适应激光雷达点云校正与定位方法，其特征在于，扫地机器人坐标系与搭载的激光雷达坐标系之间的坐标变换Tbl通过定点标定的方式得到。


3.根据权利要求2所述的基于扫地机器人的自适应激光雷达点云校正与定位方法，其特征在于，扫地机器人坐标X轴方向作为激光雷达扫描角度为零时扫地机器人的指向，Z轴指向机器人上方，XYZ构成右手系机器人本体作为世界坐标系，测量若干组激光扫描点P在机器人下的位置，从而通过定点标定的方式得到坐标变换Tbl。


4.根据权利要求1所述的基于扫地机器人的自适应激光雷达点云校正与定位方法，其特征在于，步骤S2中所述的轮式里程计的工作频率为200Hz，激光雷达的工作频率为5Hz，在一个激光周期内不断采集里程计发布的运动距离d和扫地机器人的旋转角度θ。


5.根据权利要求1所述的基于扫地机器人的自适应激光雷达点云校正与定位方法，其特征在于，设相邻里程计数据前后机器人的位姿为[xt-1,yt-1,θt-1]T和[xt,yt,θt]T，r＝d/θ，r为运动轨迹半径，步骤S2中所述的机器人航迹推测模型是：





6.根据权利要求1所述的基于扫地机器人的自适应激光雷达点云校正与定位方法，其特征在于，步骤S3中所述的直线运动模型的拟合方程为：...

【专利技术属性】
技术研发人员：罗永恒，魏武，黄林青，李奎霖，周方华，
申请(专利权)人：华南理工大学，
类型：发明
国别省市：广东;44