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

Abstract

本发明提供了一种基于扫地机器人的自适应激光雷达点云校正与定位方法，包括以下步骤：S1.通过搭载在机器人上的激光雷达采集一帧新的数据；S2.通过轮式里程计采集距离d和旋转角度θ，直到采集激光数据帧一个周期结束，推测估计机器人在每个里程计时间戳对应的位姿；S3.考虑到误差的存在，对机器人位姿拟合，计算拟合结果后的均方差；S4.计算每个激光点云相对于扫地机器人的位姿T_k‑1的坐标P′_k‑1；S5.获取扫地机器人的位姿T_k上的激光点云坐标，并把相邻两帧数据进行ICP匹配，得到扫地机器人的位姿T_k。本发明根据均方差结果自适应选用拟合模型，解决由于扫地机器人运动导致激光点云畸变进而导致地图畸变及定位不准的问题。

Description

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

技术领域

本发明涉及机器人定位领域，更具体地，涉及基于扫地机器人的自适应激光雷达点云校正与定位方法。

背景技术

随着科学技术的发展与进步，常常用于军事领域的激光雷达的成本大大降低，这使得它在商业应用上成为可能，激光雷达具有单色性好，亮度高，方向性强，抗干扰性强，分辨力强，设备小而轻等优点，尤其在测速与机器人定位方面有着广泛的应用。轮式里程计能够快速地测量移动距离和转动角度，短时间内和不打滑情况下具有较高的定位精度，且编码器价格便宜，适合商业应用。扫地机器人作为第一批商用的服务机器人，已经走进了千家万户，极大地减轻了家庭清扫清洁的负担。

传统的扫地机器人仅仅使用激光雷达传感器进行定位，由于激光雷达在扫射的过程中机器人会发生平移或旋转等运动，得到的激光点云以及生成的地图会有一定程度的畸变，且畸变程度与机器人运动速度成正比，特别是旋转运动对激光点云的畸变影响巨大，因此要对激光点云进行校正；有的采用里程计插值的方法对数据进行校正，但只用到了少数里程计数据而忽略了机器人整体运动情况(LOAM:LidarOdometry and Mapping in Real-time)，没考虑到里程计测量噪声的存在，从而不能准确地校正。工程上往往采用直线运动模型校正，但是对快速运动的机器人校正效果有限，鲁棒性不高。而采用多项式拟合的方式估计机器人运动，但是对于做直线运动的机器人往往会导致过拟合，从而丢失运动的整体信息，校正结果不准(Improved Techniques for Grid Mapping with Rao-BlackwellizedParticle Filters)。

发明内容

本发明的目的在于克服现有的技术和算法的缺点与不足，提供了一种基于扫地机器人的自适应激光雷达点云校正与定位方法。同时开启两个线程对运动模型进行拟合，自适应选取最佳模型，进行点云校正，并对ICP算法提供初始值，加快ICP算法的收敛。

本发明至少通过如下技术方案之一实现。

基于扫地机器人的自适应激光雷达点云校正与定位方法，包括以下步骤：

S1.扫地机器人在运动时，通过搭载在机器人上的激光雷达采集一帧新的数据，假设上一帧数据发布出去后，时间戳为t_s，扫地机器人在k-1时刻的位姿为T_k-1，发出当前激光帧之后的k时刻位姿为T_k，位姿变换为T_k，k-1；

S2.通过里程计不断采集运动距离d和旋转角度θ，直到采集激光数据帧一个周期结束，时间戳为t_e，使用机器人航迹推测模型估计机器人在每个里程计时间戳对应的位姿；

S3.使用直线运动模型和曲线运动模型对机器人位姿拟合，计算拟合结果后的均方差；

S4.选取产生最小均方差的模型作为这一个周期内的运动模型，计算每个激光点云相对于扫地机器人的位姿T_k-1的坐标P′_k-1；

S5.获取扫地机器人的位姿T_k上的激光点云坐标，并把相邻两帧数据进行ICP匹配，获取ICP匹配数据集，计算位姿变换T_k，k-1，进而得到扫地机器人的位姿T_k。

进一步地，扫地机器人坐标系与搭载的激光雷达坐标系之间的坐标变换T_bl通过定点标定的方式得到。

进一步地，选取扫地机器人坐标X轴方向作为激光雷达扫描角度为零时扫地机器人的指向，Z轴指向机器人上方，XYZ构成右手系；选取机器人本体作为世界坐标系，测量若干组激光扫描点P在机器人下的位置，从而通过定点标定的方式得到坐标变换T_bl。

进一步地，步骤S2中所述的轮式里程计的工作频率为200Hz，激光雷达的工作频率为5Hz，在一个激光周期内不断采集里程计发布的运动距离d和扫地机器人的旋转角度θ。

进一步地，设相邻里程计数据前后机器人的位姿为[x_t-1,y_t-1,θ_t-1]^T和[x_t,y_t,θ_t]^T，r＝d/θ，r为运动轨迹半径，步骤S2中所述的机器人航迹推测模型是：

进一步地，步骤S3中所述的直线运动模型的拟合方程为：

其中a、b、c、d、e、f表示待确定的线性方程系数，t表示时刻；

误差模型为：

其中w_i为i时刻位姿的置信权重；[x_iy_iθ_i]表示机器人i时刻的位姿；

使用Ransac(Random Sample Consensus)算法估计机器人位姿[x y θ]，曲线运动模型的拟合方程为：

其中a、b、c、d、e、f、g、h表示待确定的线性方程系数，t表示时刻；

误差函数为：

其中：

其中σ为方差、n表示估计的位姿个数；

使用最小二乘拟合算法求解机器人位姿[x y θ]，求解方法为高斯-牛顿法，且同时开启两个线程分别计算。

进一步地，步骤S3拟合的机器人位姿曲线经过扫地机器人的位姿T_k-1，对于每个里程计位姿赋予权重w，且w随时间递减。

进一步地，步骤S4中，假设相邻激光点云之间的时间间隔为δt，估计出来的机器人位姿为T_k-1,i对应的激光点云坐标为[ρ_k-1,i θ_k-1,i]^T，转换成笛卡尔坐标系为P_k-1,i＝[ρ_{k-1, i}cosθ_k-1,i ρ_k-1,isinθ_k-1,i]^T，转换到扫地机器人的位姿T_k-1下的坐标P′_k-1为：

P′_k-1,i＝T_k-1,iP_k-1,i。

进一步地，步骤S5所述的ICP(Iterative Closest Point)匹配数据集为：

P_k-1＝{P_k-1,i,i＝1...n},P_k＝{P_k,i,i＝1...n}

最小化误差函数为：

先去中心化，后采用SVD(Singular Value Decomposition)分解求解，其中，R和t是从第k-1帧到第k帧的机器人位姿变换的旋转矩阵和平移向量，

表示二范数。

进一步地，步骤S5扫地机器人的位姿T_k如下：

其中，迭代的初始R和t来自于步骤S4所选取的运动模型估计值。

与现有技术相比，本发明技术方案的有益效果是：

1.本发明采用了里程计数据运动模型拟合和自适应运动模型选取机制，拟合机制比依靠里程计数据插值结果更可靠，综合考虑了扫地机器人沿直线运动和作曲线运动两种场景，根据拟合结果的好坏，自适应选取运动模型，更加精确地确定每一个激光点云的实际坐标。

2.本发明拟合的运动模型对两相邻关键帧位姿的预测可作为ICP算法的初始值，从而加快收敛速度。

3.本发明采用双线程的方式，在做运动模型预测时，兼顾了精度和计算效率的需求。

附图说明

图1为本实施例基于扫地机器人的自适应激光雷达点云校正与定位方法的总流程图；

图2为本实施例航迹推测原理图；

图3为本实施例运动模型拟合效果图。

具体实施方式

附图仅用于示例性说明，不能理解为对本发明的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。下面结合附图和实施例对本发明的技术方案做进一步的说明。

如图1所示，基于扫地机器人的自适应激光雷达点云校正与定位方法，包括以下步骤：

S1.扫地机器人在运动时，通过搭载在机器人上的激光雷达采集一帧新的数据，假设上一帧数据发布出去后，时间戳为t_s，扫地机器人在k-1时刻的位姿为T_k-1，发出当前激光帧之后的k时刻位姿为T_k，位姿变换为T_k，k-1。位姿变换关系如图3所示。

S2.同时通过轮式里程计不断采集距离d和旋转角度θ，直到采集激光数据帧一个周期结束，时间戳为t_e，使用机器人航迹推测模型估计机器人在每个里程计时间戳对应的位姿。

选取扫地机器人坐标X轴方向为激光雷达扫描角度为零时扫地机器人的指向，Z轴指向机器人上方，XYZ构成右手系。选取机器人本体作为世界坐标系，测量多组激光扫描点P在机器人下的位置，从而通过定点标定的方式得到坐标变换T_bl，计算T_bl的目的在于把激光点云数据转换到机器人坐标系下，便于后期构建栅格地图。

轮式里程计的工作频率为200Hz，激光雷达的工作频率为5Hz～10Hz，机器人从位姿T_t-1:[x_t-1 y_t-1 θ_t-1]^T运动到T_t:[x_t y_t θ_t]^T，扫地机器人的运动距离为d，旋转角度为θ，用一段圆弧r模拟这一段运动过程，r＝d/θ，如图2所示。根据几何关系，航迹推测方程为：

S3.考虑到误差的存在，同时使用直线运动模型和曲线运动模型对机器人位姿拟合，计算拟合结果后的均方差。

直线运动模型的拟合方程为：

使用Ransac算法估计机器人位姿[x y θ]，其中，a,b,c,d,e,f为待定参数，使用Ransac算法有利于排除噪声较大的里程计数据干扰，曲线运动模型的拟合方程为：

使用最小二乘拟合算法估计机器人位姿[x y θ]，其中，a,b,c,d,e,f,g,h,i为待定参数，误差函数为：

由方程可知，y是关于x的二次曲线，求解方法为高斯-牛顿法，且同时开启两个线程分别计算。

上述的拟合直线或曲线必过T_k-1，由于航迹推测模型的假设累积误差及打滑等情况，随着时间的增加，估计结果可信度下降，因此对于每个里程计位姿赋予权重w，且w随时间递减：

其中σ为方差、n表示估计的位姿个数。

S4.选取产生较小均方差的模型作为这一个周期内的运动模型，计算每个激光点云相对于T_k-1的坐标P′_k-1。

如图3所示，假设选取的是曲线模型，拟合曲线为l，假设相邻激光点云之间的时间间隔为δt，估计出来的机器人位姿为T_k-1,i，对应的激光点云坐标为[ρ_k-1,i θ_k-1,i]^T，转换成笛卡尔坐标系为P_k-1,i＝[ρ_k-1,icosθ_k-1,i ρ_k-1,isinθ_k-1,i]^T，转换到T_k-1下，P′_k-1为：

P′_k-1,i＝T_k-1,iP_k-1,i。

S5.获取扫地机器人的位姿T_k上的激光点云坐标，并把相邻两帧数据进行ICP匹配，获取ICP匹配数据集，计算T_k，k-1，进而得到扫地机器人的位姿T_k。

ICP匹配数据集为：

P_k-1＝{P_k-1,i,i＝1...n},P_k＝{P_k,i,i＝1...n}

最小化误差函数为：

先去中心化，后采用SVD分解即可求解得。其中，R，t是从第k-1帧到第k帧的机器人位姿变换。迭代的初始R，t来自于步骤S4所选取的运动模型估计值。最终求得

表示二范数。

附图中描述位置关系的用语仅用于示例性说明，不能理解为对本发明的限制；

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之。

Claims (10)

1.基于扫地机器人的自适应激光雷达点云校正与定位方法，其特征在于，包括以下步骤：

S1.扫地机器人在运动时，通过搭载在机器人上的激光雷达采集一帧新的数据，假设上一帧数据发布出去后，时间戳为t_s，扫地机器人在k-1时刻的位姿为T_k-1，发出当前激光帧之后的k时刻位姿为T_k，位姿变换为T_k，k-1；

S2.通过里程计不断采集运动距离d和旋转角度θ，直到采集激光数据帧一个周期结束，时间戳为t_e，使用机器人航迹推测模型估计机器人在每个里程计时间戳对应的位姿；

S3.使用直线运动模型和曲线运动模型对机器人位姿拟合，计算拟合结果后的均方差；

S4.选取产生最小均方差的模型作为这一个周期内的运动模型，计算每个激光点云相对于扫地机器人的位姿T_k-1的坐标P′_k-1；

S5.获取扫地机器人的位姿T_k上的激光点云坐标，并把相邻两帧数据进行ICP匹配，获取ICP匹配数据集，计算位姿变换T_k，k-1，进而得到扫地机器人的位姿T_k。

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

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

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

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

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

其中a、b、c、d、e、f表示待确定的线性方程系数，t表示时刻；

误差模型为：

其中w_i为i时刻位姿的置信权重；[x_i y_i θ_i]表示机器人在i时刻的位姿；

使用Ransac(Random Sample Consensus)算法估计机器人位姿[x y θ]，曲线运动模型的拟合方程为：

其中a、b、c、d、e、f、g、h表示待确定的线性方程系数，t表示时刻；

误差函数为：

其中：

其中σ为方差、n表示估计的位姿个数；

使用最小二乘拟合算法求解机器人位姿[x_i y_i θ_i]，求解方法为高斯-牛顿法，且同时开启两个线程分别计算。

7.根据权利要求1所述的基于扫地机器人的自适应激光雷达点云校正与定位方法，其特征在于，步骤S3拟合的机器人位姿曲线经过扫地机器人的位姿T_k-1，对于每个里程计位姿赋予权重w，且w随时间递减。

8.根据权利要求1所述的基于扫地机器人的自适应激光雷达点云校正与定位方法，其特征在于，步骤S4中，假设相邻激光点云之间的时间间隔为δt，估计出来的机器人位姿为T_k-1,i对应的激光点云坐标为[ρ_k-1,i θ_k-1,i]^T，转换成笛卡尔坐标系为P_k-1,i＝[ρ_k-1,icosθ_k-1,iρ_k-1,isinθ_k-1,i]^T，转换到扫地机器人的位姿T_k-1下的坐标P′_k-1为：

P′_k-1,i＝T_k-1,iP_k-1,i。

9.根据权利要求1所述的基于扫地机器人的自适应激光雷达点云校正与定位方法，其特征在于，步骤S5所述的ICP(Iterative Closest Point)匹配数据集为：

P_k-1＝{P_k-1,i,i＝1...n},P_k＝{P_k,i,i＝1...n}

最小化误差函数为：

先去中心化，后采用SVD(SingularValue Decomposition)分解求解，其中，R和t是从第k-1帧到第k帧的机器人位姿变换的旋转矩阵和平移向量，

表示二范数。

10.根据权利要求1所述的基于扫地机器人的自适应激光雷达点云校正与定位方法，其特征在于，步骤S5扫地机器人的位姿T_k如下：

其中，迭代的初始R和t来自于步骤S4所选取的运动模型估计值。

Images