基于激光测距的月球探测重载六足机器人自主避障控制

刘宇飞，丁 亮，高海波，刘 振，胡艳明，何玉庆，邓宗全

(1. 哈尔滨工业大学机器人技术与系统国家重点实验室，哈尔滨 150080； 2. 中国科学院沈阳自动化研究所机器人学重点实验室，沈阳 110016)

0 引 言

足式机器人可以自主行走在崎岖地形下，在月球探测[1]、抢险救灾[2]等领域内都体现了腿式机器人的优势。在多任务环境下，足式机器人的自主导航和避障是移动机器人研究中非常重要的部分[3-4]。AMBLER[5]是由美国卡内基梅隆大学与喷气推进实验室共同研制的用于星球探测的六足移动机器人，机器人可以在无人帮助的环境下通过崎岖地形。NASA-JPL研制的ATHLETE机器人被誉为最先进的六足机器人[6-7]，其用途是在月球表面运送人员和装备，具有很强的地形适应能力。

足式机器人在一个结构地形下，可以通过本体传感器对地形做一个初步估计并指导足端落足点选择；当机器人遇到小障碍物时，可以选择踏过去，但当机器人面临一个极度崎岖或不可逾越的障碍物时，若不借助视觉信息是无法顺利通过的。在移动机器人实际工作中，自主的躲避障碍物是非常必要的任务之一，这涉及地形识别、步态、路径规划以及落足点选择等任务。

在未知环境中辨别障碍物是避障运动的前提，一般大部分机器人利用相机信息[8]或激光测距传感器数据[9]建立地形高程图，利用栅格表示环境模型起源于美国CMU大学，Borenstein曾采用栅格表示环境，提出矢量场矩形法控制机器人行走方向[10]。激光测距仪扫描精度较高[11]，一种基于激光测距仪的自适应调节阈值求可行方向的方法被提出[12]，此方法根据移动机器人的行驶方向与参考方向的偏差值作为对机器人前进方向线速度的限制，并在ASR机器人上进行了实验验证，使机器人可以安全平滑地通过障碍区域。针对单一视觉传感器无法实现复杂空间作业问题，文献[13]提出基于单目相机与激光雷达融合的位姿估计方法，实现了尺度模糊下的位姿快速估计。本文采用激光测距仪获取地形信息，并基于障碍物与机体之间的距离信息得到最优可行方向和最短运动距离。

足式机器人的运动是一个复杂的轨迹规划过程，当机器人直线行走的时候，可以在考虑能耗和流量的前提下去规划足端轨迹[14]。根据机器人本体传感器和视觉信息，Belter[15]采用A*规划算法去实现基于地形高程图的机器人运动规划研究，基于地形引导的RRT算法找到了精确的可行方向，通过辨识出地形的可通过性指标，完成了机器人的自主避障运动。文献[16]针对自由漂浮空间机器人，提出一种FFSR的避障规划，实现了FFSR的避障控制并保证其环境适应性。针对月球车的避障技术，文献[17]提出了虚拟主体避障技术，实现了月球车在障碍图中的避障学习与控制。避障运动过程中，本文采用了环境中静止的物体作为障碍物，且障碍物的高度高于机器人本体，即视为不可逾越的物体。

目前，在足式机器人中，针对自主避障运动的研究和实验验证较少。本文以电驱动六边形对称分布的六足机器人为研究对象，模拟月壤崎岖地面，提出了基于虚拟机体模型的自主避障策略，得到了机器人偏航运动过程的摆动相和支撑相轨迹，通过实验证实了六足机器人可以自主的通过障碍地形。

1 地形建模

为了实现机器人安全的在未知复杂环境下自主行走，需要机器人预先检测地形并建模，从而帮助机器人避开行走轨迹上的障碍物。针对腿部对称分布的六足机器人，采用基于机体质心的地图建模方法。在机器人行走过程中，根据检测得到的地形实时距离和高度信息，生成更新的地形高程图。通过机器人本体上的激光传感器可以准确得到实时更新的机体与障碍物之间的距离，根据障碍物的形状和距离，决定机器人下一步的运动状态。

激光测距传感器可以得到地形环境下的点云信息，通过对点云信息的分析和处理，区分平坦地形和障碍地形。下一步机器人根据障碍的距离和高度，在运动过程中的安全裕度范围内，在大地坐标系下沿着安全路径自主行走。为更准确的描述地形信息，建立了基于地形建模的4个坐标系系统，如图1所示。分别为大地坐标系OW，本体坐标系OB，传感器坐标系OV和腿部坐标系OL。其中障碍物高为h，宽度为w。

相对于大地坐标系，机体坐标系的坐标为BP(BPX,BPY,BPZ)，腿部坐标系为LiP(LiPX,LiPY,LiPZ)，传感坐标系为VP(VPX,VPY,VPZ)。为了建立基于机体质心的地形信息，将激光测距传感器信息转换到机体坐标系下：

(1)

(2)

由于在高度测量过程中，物体在两个坐标系统下的横纵坐标不变，因此矩阵P=[0 0 1]。高度测量的标准差如下所示：

(3)

其中ΣV代表激光传感器模型的协方差矩阵，ΣP,q是传感器坐标系旋转的协方差矩阵。JV是传感器测量的雅克比矩阵，Jq是传感器坐标旋转的雅克比矩阵。

针对地形高程图的云图更新，采用扩展卡尔曼滤波的方式对更新数据进行数据融合[19]。扩展卡尔曼滤波方程中的状态变量等同于地图信息中的高度信息。扩展卡尔曼滤波更新方程如方程(4)-(6)所示。

(4)

(5)

(6)

(7)

(8)

通过这种方式，可以更新实时的地形高度信息和高度的标准差，从而得到实时的地形高程图。图2是通过此方法得到的地形信息测试。

2 障碍检测及避障算法

基于地形建模得到的高程图，可以通过相邻网格判断其是否为障碍物。需要通过障碍检测方法得到障碍物在大地坐标系下的位置。下一步根据障碍物和机器人之间的位置关系，实现机器人自主避障。

2.1 障碍检测

(9)

其中栅格的高度信息表示为h[p,q]，代表了部分地形的平均高度。

(10)

其中k属于栅格中点云的数量，利用h[p,q]去评价地形的平坦程度，判断是否是障碍物。

(11)

2.2 避障算法

地图上的每个栅格存储了地图上每个点的高度值，首先对地图上的整个区域进行评价，得到平均高度最低区域，即为可通过性最好区域。本文提出一种基于虚拟机体模型的自主避障的方法，利用激光测距仪获得的地形信息得到地形通过性最好区域，最后计算得到最优的可行方向，并实现运动距离最短完成避障运动。由于六足机器人是六边形对称结构，因此在任意方向都可以当作前进方向，并不需要做转向运动，可以沿着给定的方向实现全向运动。基于本体传感器得到机器人本体及各腿的位置信息，建立虚拟机体模型，通过虚拟机体模型可以最大限度的包络整个机器人的运动范围，防止机器人和障碍物相碰。通过虚拟机体和障碍物的几何模型，可以实现高效简便的计算得到机器人与障碍物的距离及最优的运动可行方向。机器人的避障运动可以分为以下几个步骤：

第一步：基于本体传感器信息，建立一个以机体质心为圆心，机体质心到足末端位置距离为半径的虚拟机体几何模型，如图3所示。

第二步：以虚拟机体模型构成的圆形区域作为每一步的采样评价区域，其最大包络尺寸为以机器人原点为圆心，半径为1430 mm的圆形区域。在机器人行走避障过程中实时更新激光扫描获得的栅格信息，对机器人前进方向的采样区域进行评价。根据激光测距仪得到地图上每个栅格点的高度值，对采样点评价区域内的所有栅格点做加和取平均值，得到平均高度最低区域，即为可行方向最优。计算得到机器人质心到障碍物的垂直距离dOV和质心到最优方向点T的距离dOT，根据距离dOT和机器人虚拟模型的包络范围可以确定机器人的偏航方向，并保证运动距离最短，实现最快速的避障运动。

第三步：根据建立的虚拟圆几何模型，得到圆切点M距离障碍边界点N的连线，并计算得到机器人的避障偏航角φ。

(12)

其中dOT可以根据本体传感器及激光传感器的测量值计算得到。

以此方法可以保证机器人沿着最短距离达到目标位置O′，当机器人在运动过程中，根据激光测距信息实时调节避障偏航角φ，从而实现最快速度的避障运动。

在避障过程中保证机器人的运动直径l′

3 基于自主避障的运动规划

机器人的运动是根据预先设定好的轨迹实现的，当有激光测距传感器反馈时，机器人按照给定的方向角和步长实现运动，完成自主避障运动，因此机体运动规划是实现运动的重要一环。根据机器人本体的运动规划，得到机器人支撑腿的运动轨迹，从而规划出各腿摆动相的落足点，最终实现足端支撑相和摆动相的轨迹规划。

3.1 六足机器人机身运动规划

机器人机体运动轨迹的时间函数由六个变量组成，选取机器人质心在大地坐标系平面内的行进速度，机体支撑高度，转弯半径和三个姿态角的时间函数。记为：(vB(t),hB(t),rB(t),α(t),β(t),γ(t))T。机器人的行进速度，支撑高度，俯仰角和滚转角都可以直接线性给出，转弯半径和偏航角根据激光测距仪检测得到的信息实时给定，从而达到最优的偏航轨迹。机体的偏航角度的变化与转弯半径有关，根据图5所示，当机器人沿着转弯半径进行规划时，其转向半径可以表示为：

rB=S/(2sin(rwπ/4))

(13)

其中，S为机体质心距运动速度点的距离，rw为转向参数，在避障运动过程中给定。

当机器人保持直线匀速运动过程中，机体质心运动速度可以通过多项式方程拟合得到：

(14)

(15)

其中，λ是足端运动步长。根据机体质心距运动速度点的距离和前向运动速度vB，可以得到机体的偏航速度为：

vp=vB/(Ssec(rwπ/4))

(16)

当得到机器人本体的运动轨迹和速度之后，可以根据机体质心的运动实现对足端轨迹的实时规划。

3.2 六足机器人足端轨迹规划

由于足式机器人的移动是通过各腿足的交替运动实现，基于规划好的机器人本体运动，将机体规划转化为各腿有序的行走。采用二步态的步序，如图6所示，将六足腿分成两组，实现机器人最快速度的行走及避障运动。

足端轨迹规划过程中需要先对足端轨迹进行规划，后通过逆运动学转换为各关节的协调运动。将足端运动规划分为支撑相和摆动相轨迹。

(17)

根据足端目标位置和机体期望的偏航速度，基于前进方向的目标位置，速度和加速度，对足端摆动相轨迹进行规划：

(18)

其中矩阵A可以被写为：

(19)

通过参数辨识，得到避障运动过程中多项式中的各参数：

(20)

根据机器人的偏航速度，得到足端的支撑相轨迹方程为：

(21)

根据机器人质心的运动速度和实时改变的运动方向，通过多项式拟合的方式，实现了机器人的摆动相和支撑相轨迹。通过机器人逆运动学方程得到跟关节角α，髋关节角β和膝关节角γ：

(22)

(23)

(24)

表1 六足机器人参数Table 1 Parameters of hexapod robot

通过计算得到各腿关节角之后，通过控制器实现对规划的足端速度和位置的跟踪，实现机器人自主避障运动。

4 试验验证

通过提出的避障策略和运动规划方法，可以让机器人在未知的环境下找出一条安全的路径。为验证所提出方法的可行性，在ElSpider机器人上对控制策略进行了试验验证，其六足机器人系统的实现如图7所示。

六足机器人的试验系统包括内部和外部传感器系统，其内部传感器包括运动控制器，驱动器，电机；外部传感器包括足底六维力传感器，安装在本体上的姿态传感器IMU，以及激光测距传感器Velodyne，其采样频率达到10 Hz。六足机器人传感器信息见表2所示。

表2 六足机器人传感设备信息Table 2 Equipments of the ElSpider robot

为最大限度保证激光雷达的扫描范围，将激光雷达倾斜10°安装在机器人本体上，以便可以最大限度看到前面落足位置并扫描到前方物体，如图7所示。

通过激光测距传感器信息，设定一个长3 m×宽8 m的地形图，地形图上每一个栅格尺寸均为0.3 m的正方形。

在机器人避障运动过程中，通过避障策略得到最优航向角度，通过UDP通信方式将实时角度发送给机器人运动规划。为了验证机器人可以正确、实时响应最优航向角度，通过机器人本体传感器测量机体的偏航角度并与激光测距信息得到的偏航角度对比，如图8所示。

试验曲线中的红色实线是根据3.2节中通过激光测距传感器得到的最优可行偏航角度信息；蓝色星线代表通过本体传感器计算得到的机体实时偏航角度，通过对比发现机器人可以实时跟踪激光传感器给定的最优偏航角度，但是由于六足机器人的足端运动是非连续运动，在运动过程中会有一定的滞后，导致无法完全跟踪偏航角度。

从曲线中可以看出在开始时机器人距离障碍物很近，因此通过计算得出机器人的偏航角度为66.8°，当机器人向偏航方向运动时，偏航角度逐渐变小，到272 s时，机器人完全避开了障碍物，开始朝向前进方向运动，因此偏航角度变为0°。

通过本体传感器数据采集得到机器人各足的足端位置曲线，其足端轨迹曲线是基于机体坐标系绘制的，可以体现出机器人的运行方向，通过足端轨迹验证机器人可以准确沿着偏航方向运动，由于在运动过程中采用二步态实现机器人的步态运动，因此分别绘制足1和足2的轨迹曲线，如图9和图10所示。

从图9和图10中可以看出在机体坐标系下机器人的足端轨迹曲线完成了摆动相和支撑相的循环运动，并时刻沿着偏航的方向行走，两组不同的腿可以相互协调的完成偏航运动过程。当检测到障碍物时，足端沿着前进方向和水平方向的夹角运动，当避开障碍物以后，足端沿着前进方向运动，完成整个的避障过程。

针对六足机器人完成的避障运动，为验证关节运动的连续性，图11和12所示为腿1和腿2的跟关节，髋关节和膝关节曲线。

通过曲线11和12可以看出机器人的腿1和腿2关节运动连续，并根据足端轨迹曲线完成了相应的避障运动。六足机器人的避障运动过程见图13所示，可以根据地图信息近似得到机器人距离障碍物的距离，地图中标亮的地形为障碍物，可以根据地图尺寸信息和机器人尺寸信息得出机器人质心距离障碍物的近似距离为2.6 m。最终，依据实时更新的地图信息，机器人实现了自主避障运动。

5 结 论

基于激光测距信息的避障运动是机器人在复杂环境下作业的重要手段之一，通过模拟月壤环境并实现地形建模，解决了机器人在未知环境下的自主避障和导航问题，本文对其地形建模和自主避障策略进行了研究，有以下结论：

(1) 基于激光测距仪信息，完成了实时的地形建模，并得到了障碍和机器人本体几何信息的映射，提出基于虚拟机体模型的自主避障策略，得到机器人避障运动过程中的最优可行方向和最短运动距离。

(2) 针对电驱动六边形对称分布的六足机器人，建立了其运动学模型，规划了实时避障的机体和足端运动轨迹，完成了摆动相和支撑相的运动规划，使机器人可以根据给定的可行方向实现运动。

(3) 进行了六足机器人样机避障运动的实验研究，实现了六足机器人在未知环境下的自主避障运动。实验结果表明，六足机器人可以实时、准确跟踪避障策略中给定的偏航角度，避障过程中的摆动相和支撑相的运动轨迹验证了避障策略的有效性和实时性。

在未来的工作中，考虑到六足机器人会在复杂凸凹不平的地形下行走，此时需要对机器人足底的地形进行建模，并根据实时的地形信息为机器人提供安全的落足点选择是进一步需要研究的问题。