移动机器人障碍物地图构建研究

程天明，陈元电，苏成悦,，徐胜，杨尚儒，刘拔

（1.广东工业大学信息工程学院，广州510000；2.广东工业大学物理与光电工程学院，广州510000）

0 引言

移动机器人行驶过程中对路面不平和障碍物的检测是环境感知技术领域的重要研究方向。研究包括基于立体视觉的方法[1-2]、基于激光雷达的方法[3-4]和基于神经网络的方法[5-6]。室外检测的主要目标为建筑物、行人和车辆等，室内检测的主要目标为墙壁和桌椅等。实际环境中的障碍物种类繁多，如减速带和凹坑等处于移动机器人盲区不能被识别。有学者利用图像和结构光对室外水体、凹凸障碍物特征进行分析[7]，但易受光照等环境因素影响。3D激光雷达分布式安装进行凹障碍物特征检测经济性和运算量都存在问题[8]。上述研究均只能处理检测而不具备构建凹凸障碍物地图的算法和能力。

移动机器人同步定位与地图构建（SLAM）主流有激光SLAM和视觉同步定位与地图构建（VSLAM）两种，激光SLAM又分2D和3D，其中2D激光SLAM利用2D激光雷达直立安装检测同水平面内的障碍物并构建环境地图，但它存在路面盲区，忽略了地面平整性。3D激光SLAM和VSLAM都用于构建三维点云地图[9-10]，但点云地图的目的是提高定位精度，避障导航则是使用点云地图提取大型障碍物信息转换而成2D栅格地图，去掉了地面点云的平整性信息[11]。3D激光雷达和深度相机或双目相机与2D激光雷达相比运算开销大，行内多采用成熟的2D激光雷达技术[12]。目前无论哪种技术都没有实现路面实时平整性检测和凹凸障碍物地图。

针对现有方法实时性弱、可靠性差和缺乏平整性信息等问题，本文提出一种用激光雷达实时检测路面平整性和凹凸障碍物地图构建方法。通过对地面探测的数据进行空间变换、标签并滤波获得有效数据，对路面平整性进行预测，结合惯性测量单元（IMU）和视觉里程计的最优位姿，构造凹凸障碍物地图。

1 单线激光投影变换

平整性检测使机器人在不同位姿下，通过多线激光测距进行路面检测，实时计算路面平整性用于即时避障，并提供构建凹凸障碍物地图的数据。多线激光是单线激光进行刚性变换后的扩展，以单线激光检测作为最小检测单元，建立数学模型计算理论距离与实际测量的比值，判断测量点是凹陷障碍物还是凸起障碍物。假设世界坐标系为(Xw,Yw,Zw)，机器人坐标系为(Xr,Yr,Zr)；单线激光测距的激光束为世界坐标系中两点P0(x0,y0,z0)和P1(x1,y1,z1)所确定的一条射线P0P1，其中P0为射线的起始点，P1为预测的测量点，如图1所示。

图1 算法应用示意图

当机器人坐标系与世界坐标系之间的投影仅有绕Zw轴旋转和XwYw轴平移时，射线P0T P1T在XwYw平面上的投影长度不变，此时LP0P1=LP0TP1'。机器人前轮最低点所在的XwYw平行面视为平整路面，雷达安装高度z0为固定值，Zw轴平移量为0，只需计算XwYw方向上的旋转变换，旋转角由IMU测量获得。

令P0P1变换后为：

由此确定射线P0TP1T的参数方程：

式（2）中，P为未知量，k为参数值。求射线P0TP1T与XwYw轴所成平面相交的点P1'T=k[P0T-P1T]+P1T|z=0（当z=0时求得的值），求得平坦时的理论距离

设实际测得的距离为Lmeas。①Lmeas>LP0TP'1时，机器人前方路面为凹陷路面；②Lmeas=LP0TP'1时，机器人前方路面为平坦路面；③Lmeas

图2 单线投影算法流程

2 多线检测与构图

在上文提出基本单元的平整性检测算法基础上，扩展成多线激光测距模块或激光雷达，结合IMU和视觉里程计位姿建立凹凸障碍物地图。上文的P0和P1点为参考点和其他激光束存在变换关系Aj：

由于IMU二次积分导致变换矩阵误差大，所以构图中需结合视觉里程计获取变换矩阵T。令第i时刻的IMU位姿xpose(i)为预测数据，视觉里程计位姿zobs(i)为观测数据，利用非线性最小二乘计算最优位姿。令f(xpose(i))=zobs(i)，i=0,1,2,3,…，目标为最小化预测和观测的差：

假设误差服从高斯分布，令其对应的信息矩阵为Ωi，观测值误差的平方定义为：

非线性最小二乘的目标函数为：

求解目标函数得到各时刻的最优位姿，Topt(i)代表求得的第i时刻的最优位姿，结合点云数据[PmeasT]i投影到世界坐标中以获得点云集合：

式（7）得到了原始的点云地图。将所有激光测距的初始点[P0T,P1T]j，j=0,1,2,3,…根据各时刻的最优位姿进行单线投影变换计算出每个点云数据的理论距离LP0TP'1T实际距离Lmeas的比值，将该数据点定义为凹陷、凸起、平坦三种情况，并对每一个数据添加凹陷、凸起或平坦的标签属性。根据标签筛选出所有的凹陷和凸起障碍物，获得凹凸障碍物地图。其算法流程如图3所示。

图3 多线构图算法流程

3 实验

3.1 平整性检测

本研究用于实时检测凹凸障碍物时，室内的门槛、地面线槽和向下楼梯等可类比室外凹坑减速带等凹凸障碍物，检测原理一致。基于此原则，本研究在测试平台上运行Gazebo仿真软件加载测试模型，添加实验障碍物，进行仿真实验，测试平台硬件信息如表1所示。

表1 测试平台硬件信息

根据单线投影变换原理标志后得到激光点云数据，利用标签进行滤波，保留凹凸障碍物信息，其效果图4所示。

图4 投影变换效果图

激光数据投影只提取表征凹凸障碍物距离的激光数据。图4（b）是对图4（a）的原始数据效果，经过平整性检测和滤波后的数据如图4（c）。进一步对不同类型的凸起障碍物进行检测，结果如下所述。

（1）对图5（a）（b）（c）凸起障碍物检测试验结果如图6（a）（b）（c）所示，本研究能检测出移动机器人前进方向中处于激光雷达安装位置下方的凸起障碍物，包括图6（a）木板、图6（b）圆台障碍物和图6（c）小球。此木板等类障碍物类比门槛、地毯和玩具等室内障碍物，亦可类比减速带和石头等室外障碍物。

图5 Gazebo凸起障碍物仿真场景

图6 凸起障碍物可视化检测效果

（2）对图7（a）（b）凹陷障碍物检测效果如图8（a）（b）所示，本研究能检测出路面凹陷障碍物。选取的图7（a）长方体凹陷、图7（b）半球体凹陷类比凹坑和路面断层等室外凹陷障碍物。

图7 Gazebo凹陷障碍物仿真场景

图8 凹陷障碍物可视化激光检测效果

3.2 构建地图

（1）本研究在Gazebo的大型和凹凸障碍物仿真场景地图构建效果如图9（a）（b）所示。在如图9（a）仿真环境中，根据本文方法进行凹凸障碍物检测后构建的地图包含所有物体，如图9（b）。

图9 本算法建图效果

（2）在相同仿真场景下直立安装的2D激光雷达的地图构建效果，如图10（a）（b）所示。在图10（b）的障碍物地图中仅存在一个障碍物，不能表征整个仿真环境。

图10 直立安装2D激光雷达效果

（3）在仿真场景下深度相机的地图构建效果，如图11（a）（b）（c）所示。在图11（b）的3D点云地图中可见两种凸起障碍物的模型，两种凹陷障碍物存在形状上的空缺，没有点云数据。在图11（c）障碍物地图中也仅包含一个障碍物，不能表征整个仿真环境。

图11 深度相机效果

各方法的建图实验结果如表2所示。

表2 实验结果对比

2D激光雷达传感器基于gmapping算法创建的障碍物地图，只包含雷达安装平面的大型障碍物，缺失了地面小型和凹陷障碍物。深度相机基于rtabmap在3D点云地图中包含了门和球形两类障碍物体，缺少凹陷障碍物，转换成障碍物地图时，由于球形障碍物体积小，凹陷障碍物点云缺失，只保留了大型障碍物。本算法的障碍物地图除包含大型障碍物外，还包含了地面小型凹凸障碍物。实验结果表明，前面两传感器和它们的算法不具备构建包含小型和凹陷障碍物的导航地图的能力；本算法应用于机器人平整性检测和地图构建时，具备构建小型和凹陷障碍物的导航地图的能力，比其他算法含有更多的障碍物信息，具备更强大的导航能力。

4 结语

在实际应用中，移动机器人在建图导航时仅考虑XY坐标和偏航（Yaw），缺乏地面平整性和凹凸障碍物信息，这对机器人移动过程中的位姿将产生重要影响。室内的门槛和往下楼梯等区域不包含在导航地图中，对移动机器人存在威胁；室内外的不平路面对机器人移动位姿的干扰同样也会影响建图精度。本文从移动机器人不同姿态时对地面凹凸障碍物检测的数学原理和计算流程开展研究，首次提出构建凹凸障碍物地图并实现路面实时平整性预测。与其他方法相比，本研究使机器人在复杂的环境中能够感知更多的障碍物信息，增加了小型和凹型障碍物的信息，并直接纳入导航地图，使移动机器人可依靠障碍物地图避开危险区域。本研究提供的方法计算量少，经济性和实时性好，很好地解决了导航地图的信息缺失问题。