移动机器人基于激光点云定位建图的汽车宽度与方位估计

， ，霄远，

(上海交通大学机械与动力工程学院机械系统与振动国家重点实验室， 上海 200240)

0 引言

在移动机器人抓取搬运作业中，识别待操作目标物体的宽度和摆放方位是成功的关键。文献[1]采用二维图像进行物体检测和定位。随着深度传感器的发展，基于三维点云的物体参数识别方法越来越多，相比于基于二维图像的方法，其更适合于识别物体的几何参数。基于三维点云识别物体几何参数，首先需要构建整体环境的三维点云结构，然后从包含地面和周边环境信息的数据中分离出物体，最后识别其宽度和摆放方位。

文献[2]采用旋转的二维激光雷达生成三维点云构建三维环境地图；文献[3]采用三维激光雷达实时建立高精度三维点云地图，但是需要借助相机传感器。文献[2-3]主要研究定位建图技术，并没有识别物体几何参数。在物体与环境信息分离方法方面，文献[4-6]提取环境中的平面，实现物体与地面、墙面或桌面的分割。文献[7]首先将物体三维点云投影到二维平面，然后采用连通成分分析进行物体分割。在物体几何参数识别方面，文献[8]先建立汽车点云的包围盒, 然后估计包围盒的方位和尺寸参数。文献[9]提出建立边缘点凸包，然后通过最小化凸包顶点与包围盒各个边的平均距离确定出包围盒。文献[10]采用迭代端点算法拟合出边缘折线，以最长边缘折线的2个端点作为包围盒的相对顶点，然后以距离两者连线的最远点作为第3个顶点来最终确定包围盒，进而估计物体尺寸参数。

在此，针对室外环境下停放的汽车物体搬运作业需求，提出一种识别待搬运汽车宽度和方位的方法。

1 移动机器人坐标系定义

移动机器人的坐标系定义如图1所示。移动机器人本体坐标系为Or-XYZ，其中原点Or为两侧驱动轮轴向连线的中点，X轴水平指向机器人右侧，Y轴指向机器人正前方，Z轴垂直地面竖直向上。机器人前上方安装有三维激光雷达传感器，用来采集周围环境的信息，该传感器测量坐标系为Os-XYZ，其3个坐标轴与机器人本体坐标系平行，两者之间存在1个平移变换T。世界坐标系Ow-XYZ与初始时刻机器人本体坐标系Or-XYZ重合。

图1 移动机器人坐标系定义

图2 移动机器人运动中地图构建过程

2 基于激光点云的定位建图

2.1 定位建图中主要算法

ICP(iterativeclosestpoint)[11]算法是Besl和McKay在1992年首次提出的，主要用来计算两帧具有部分或者全部重叠点云之间的位姿变换关系。GICP(generalized-ICP)作为改进算法之一，凭借其出色的速度和鲁棒性能被广泛使用。

标准ICP算法主要分为2个部分，一是计算两帧点云之间的匹配关系；二是计算使得具有匹配关系的点对之间欧氏距离最小的变换矩阵。标准ICP算法过程如下所述。

设有两帧点云数据A={ai}，B={bi}，i=1，2，…，N，两者之间的变换矩阵为T0。

a.令T=T0。

b.对于i=1∶N，搜索mi∈A使得‖mi-T·bi‖最小。

GICP算法考虑到点云中每个点的概率模型，通过对点云中的点进行权重分配来使得整个算法更加精细化，同时又不增加算法复杂度，其与标准ICP主要不同在于求解T时加入了协方差矩阵，即

(1)

Ci为距离的协方差矩阵。

因子图[12]是同时定位与建图方法中后端优化架构，该框架将待优化的变量表示为顶点，将变量之间的约束关系表示为因子。因子图求解定位与建图问题可以转化为求解最大后验估计的问题，即通过调整待优化变量的值使得所有因子乘积达到最大。对于待优化的图是不断增长的情况，因子图可以通过精细化的处理，对于新添加进来的节点和边，近似认为只有与新添加进来的直接关联的节点需要调整更新，而不必对整个图进行重新计算，以节省掉大量的计算[13]。

2.2 激光点云定位建图

激光点云定位建图算法架构如图3所示。该方法采用单一的三维激光传感器，通过帧与帧之间的重叠关系，采用GICP算法估计传感器的运动里程计；采用基于关键帧的策略来避免冗余；后端采用构建因子图的方式进行优化，调整三维点云关键帧的空间位置使其更好地满足约束关系；采用回环检测策略提供额外的约束添加到因子图中，减小运动过程中由帧间匹配造成的累积误差。

图3 激光定位建图算法

里程计部分将每一帧新读取到的点云先与上一帧进行GICP迭代，初步估计出当前帧机器人位姿。然后根据初始估计出的位姿结果在地图中寻找出与当前帧每一点最近的三维点组成新的点云，将当前帧与从地图中取出的点云再次进行GICP迭代，得到更加精确的当前帧位姿。将此估计结果添加进后端优化因子图中，执行优化。根据优化后的位姿平移量的大小确定是否生成关键帧。若生成关键帧则将关键帧插入到回环关键帧序列中，并将其对应点云加入地图中。

回环优化部分对每个新生成的关键帧，都会在已经存在的回环关键帧序列中寻找出与当前帧相似的关键帧，通过同样的GICP方法使得两帧之间具有额外的位姿约束关系，并添加到因子图中。每次回环成功即进行1次全局因子图优化，通过调整变量节点的取值来满足所有的因子节点约束关系：

(2)

X为待估计的移动机器人关键帧处位姿；Z为帧间匹配和回环检测得到的关键帧之间位姿约束。

3 汽车宽度和方位估计

3.1 点云分割

激光定位建图算法得到的三维点云中混有地面和环境信息，下一步需要进行汽车点云与环境的分割，从而只针对汽车数据进行处理。主要包括2步，点云地面去除和多个汽车之间点云分离。

由于激光传感器相对于移动机器人的变换矩阵T已知，可以根据变换矩阵T将地面点云数据从传感器坐标系转换到机器人坐标系下，这样地面点云在机器人坐标系下Z轴方向坐标已知，因此地面点云数据的分割就比较容易，只需要将机器人坐标系下Z坐标为-r的点云去除即可，r为机器人驱动轮半径。

地面分割后的物体数据中包含多个目标点云，为此需要进行目标之间的分割，从而分别识别其方位和宽度信息。同一物体的点云总是紧邻的，不同物体的点云之间存在间距。为此，可以根据空间点的欧氏距离采用点云聚类算法进行物体分割，然后分别对不同的类别进行处理。对于P中的2组点云集PI和PJ，其中PI∩PJ=∅，且令d为分类距离阈值，若满足

(3)

则2组点云集可认为分属不同类别。聚类过程中需要根据距离对数据点进行大量的临近点搜索，采用遍历方法效率太低，因此采用构建K-D树的数据结构的方法加快搜索速度，其中K代表数据点的维数，本文中K=3。

3.2 汽车宽度和方位估计

在对汽车进行宽度识别前先将三维的汽车点云投影到二维，这样使得汽车减少了一个不重要的高度信息，也很好地保留对搬运作业比较关键的长宽和方位信息，如图4所示。

图4 汽车宽度和方位识别方法

识别算法如下所述。

设二维投影后的汽车点云为C(共N个点)；第i个点的坐标为Pi(xi，yi)；汽车点云的包围盒长度为L，宽为W；P0(x0,y0)为包围盒距离机器人坐标系原点最近的点；包围盒4条边分别为l1,l2,l3,l4,其对应的法向量分别为n1,n2,n3,n4,其中l1与X轴的夹角为θ。

方形框4条边的直线方程分别为：

(4)

每条边的法向量分别为：

(5)

Pi与4条边的距离分别为：

(6)

目标函数为：

(7)

(8)

即确定了汽车轮廓的长度和宽度，以及汽车在移动机器人坐标系下的摆放方位，对于搬运作业只需要从宽度面进行规划搬运路径，因此只针对宽度进行处理。

4 实验验证

4.1 移动机器人实验平台

实验设备为差动轮移动机器人，如图5所示。该机器人下位机为工控机，位于机器人本体内部，主要负责接收上位机的指令控制机器人运动。上位机为笔记本电脑，配置为 Core i5 四核处理器、主频 2.3 GHz、4 GB内存，主要负责接收激光雷达数据，并完成定位建图及汽车宽度和方位估计的任务。

图5 移动机器人实验平台

三维激光传感器为VLP-16激光雷达，位于机器人顶部。它支持16个通道，每秒向四周360°发射激光束，采集30万个三维点，并将采集到的数据通过百兆以太网与上位机通信。VLP-16激光雷达垂直视角为±15°。

4.2 实验结果

实验场景如图6所示。移动机器人在运动中实时构建周围环境的三维点云地图，并识别出各个汽车宽度信息和摆放角度。三维点云图结果如图7所示，测量结果与真实值对比如表1所示。由表1可知，该方法识别结果宽度误差约为0.007 m。汽车停放角度测量值如表2所示，角度误差约为1°，可以满足室外搬运作业精度要求。

图6 实验场景

图7 三维点云图

表1 汽车宽度测量结果 m

表2 停放角度测量结果 (°)

5 结束语

提出的基于激光点云定位建图的汽车宽度和方位估计方法，适用于室外环境下移动机器人汽车搬运作业。该方法通过构建点云地图将汽车外形部分重构出来，然后对点云进行分割，提取出汽车点云，最后通过投影处理后进行包围盒估计，进而识别出汽车的宽度和方位信息。通过实验验证了该方法的识别效果。

参考文献：

[1] Calonder M, Lepetit V, Strecha C, et al. BRIEF: binary robust independent elementary features[C]// European Conference on Computer Vision,2010:778-792.

[2] Zhang J, Singh S. LOAM : lidar odometry and mapping in real-time[C]// Robotics: Science and Systems Conference,2014.

[3] Zhang J, Singh S. Enabling aggressive motion estimation at low-drift and accurate mapping in real-time[C]// IEEE International Conference on Robotics and Automation, 2017：5051-5058.

[4] 孙小凯.基于RGB-D信息的物体定位与识别[D].杭州：浙江大学, 2014.

[5] 柯翔. 面向室内环境的服务机器人物体检测与识别研究[D].合肥:中国科学技术大学, 2013.

[6] 鄢武. 基于ROS和点云库的室内三维物体识别与姿态估计[D].广州: 广东工业大学, 2016.

[7] Marton Z C, Goron L C, Rusu R B, et al. Reconstruction and verification of 3D object models for grasping[C]// Robotics Research - the International Symposium, 2009:315-328.

[8] Azim A, Aycard O. Detection, classification and tracking of moving objects in a 3D environment[C]//Intelligent Vehicles Symposium, 2012:802-807.

[9] Börcs A, Nagy B, Baticz M, et al. A model-based approach for fast vehicle detection in continuously streamed urban LIDAR point clouds[C]//Asian Conference on Computer Vision, 2014:413-425.

[10] Ye Y, Fu L, Li B. Object detection and tracking using multi-layer laser for autonomous urban driving[C]// IEEE International Conference on Intelligent Transportation Systems, 2016:259-264.

[11] Besl P J, McKay N D. A method for registration of 3-D shapes[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence,2002, 14(2):239-256.

[12] Dellaert F, Kaess M. Factor graphs for robot perception[J].Foundations and Trends in Robotics 2017, 6(1/2):1-139.

[13] Kaess M, Johannsson H, Roberts R, et al. iSAM2: incremental smoothing and mapping using the Bayes tree[J].International Journal of Robotics Research, 2012, 31(2):216-235.