张瑞雷，丑永新，丁 卫

（常熟理工学院 电气与自动化工程学院，江苏 常熟 215500）

1 引言

多机器人路径规划［1］在未知环境探索、灾后地形重构等特定任务中发挥着越来越大的作用，在交通运输、军事、航空航天、工业生产等领域有着广阔的应用前景. 例如在军事和国家安全领域可以用于侦查搜寻、排雷反恐、区域巡逻以及物资运输等任务；在仓储管理中可用于包裹分拣、分类摆放；农业上可用于农作物区域协作播种、灌溉、农药喷洒和收割等. 多机器人系统路径规划研究内容［2］主要包括在环境中行驶路径的规划、队形形成和保持以及队形切换等.

目前，传统机器人路径规划方法主要有模版匹配图、势场法、地图构建法等，智能路径规划算法有粒子群、蚁群等［3-8］，但主要侧重于单机器人系统的路径规划. 对于多机器人系统还必须考虑编队中机器人之间的冲突和协作等. 研究队形形成和保持的主要方法有领航跟随法、虚拟结构法、基于行为法等［9］. 相比较而言，由于领航跟随法具有数学描述简单、易于分析等优点，应用更为广泛. 然而在许多应用场合由于环境约束和任务需要等［10］原因，需要多机器人系统在前行过程中改变队形形状. 队形在环境和任务中的调整在现有文献中研究较少，还没有统一的模式和方法来解决这个问题. 研究队形切换的主要方法有固定队形切换和队形自适应选择等，但都要求标准的环境模型.

如需将编队控制应用于具体的任务中，就必须设计合适的协作体系结构，把路径规划、编队保持和编队切换等功能有机统一起来. 本文先简要介绍编队应用的场合，然后设计多机器人系统的全局和局部路径规划，并在此基础上详细描述此动态算法，最后通过多机器人在起伏地形环境下编队运输行驶的两个具体任务，验证所提方法的有效性.

2 问题描述

2.1 机器人模型

车式移动机器人对起伏地形的适应能力要强于两轮差动的移动机器人. 本文采用四轮车式移动机器人进行路径规划仿真研究. 具体运动学模型如图1所示，其对应公式为式（1）.

其中后轮速度方向与机器人运动方向一致，前轮绕垂轴转动，l表示前轮与后轮间的轴间距，机器人的位姿向量为p=[x,y,θ,φ]T. 其中，(x,y)T为后轮轮轴中心C的坐标，θ为车身方向与x轴的夹角，φ为前轮转向角，v为后轮前进速度，ω为前轮转向角速度，(v,ω)T为机器人的控制输入.

图1 车式移动机器人模型

2.2 地形环境

本文主要研究地面移动机器人，而地面移动机器人通常在地形表面运动. 在地理学上，通常采用等高图来描述起伏地形. 由等高图可以转换得到含有高度信息的地形图，如图2所示. 然后将地形导入到PhysX物理引擎中绘制并生成三维起伏地形环境.

起伏地形通常用于仿真野外环境，如山地、土坡以及陨石坑等，用于测试机器人在起伏地形环境下如何克服地形对编队控制以及路径规划带来的影响. 平缓起伏地形主要用来测试地形崎岖起伏对机器人运动性能以及编队精度的影响. 陡峭起伏地形主要用来测试编队对环境的适应能力，如不可通过区域队形切换以及路径规划的算法有效性. 平缓起伏地形设计如图3所示，高度单位为m.

地形可设置多种参数，其中包括还原系数（指物体碰撞后速度和碰撞前速度的比值）、静摩擦力系数、滑动摩擦力系数等，用于近似模拟实际实验中不同材质的地形特性，如干燥木板的还原系数0.2～0.4，静摩擦力系数为0.4～0.6，滑动摩擦力系数为0.2～0.5.

图2 起伏地形模型

图3 平缓起伏地形

2.3 双向RRT路径规划算法

双向RRT规划算法如图4所示. 受到经典双向搜索理论的启发，可以通过两个RRTs增长（一个来自xinit，另一个来自xgoal）改善系统性能. 当这两个RRTs相遇时，一条可行的路径就规划出来了. 在复杂二维环境下RRT与双向RRT路径规划曲线如图5所示. 起伏地形环境下RRT算法将结合仿真进行说明.

图4 双向RRT算法

3 路径规划方法

3.1 全局路径规划

为了更好适应起伏地形，机器人选择四轮车式移动机器人.同时，在地形设计时需要考虑起伏地形的典型特征，需要具备凸起和凹下两种陡峭地形. 设计的两种典型测试地形的等高线图和高度图如图6所示，高度标尺单位为m. 地形1狭窄通道宽于地形2.地形1用于测试固定队形编队运输行驶，地形2用于测试切换队形编队运输行驶.

图5 地形2的RRT路径规划流程

图6 地形设计

在完成地形设计后，需要依据地形和编队运输行驶任务设置的起点和终点规划行驶参考路径. 全局路径由任务规划模块生成. 以地形2为例，在起伏地形环境下的RRT路径规划过程如图6所示.

在起伏地形环境下，经过平滑和优化后的参考路径如图7所示. 在仿真初始化时，由任务规划模块将路径传输给领航机器人作为行驶参考.

3.2 局部路径规划

无障碍空旷地形如图8所示. （a）图为视觉信息，（b）图为经过处理的深度信息，颜色越深表示障碍物或地面离机器人越近. 在这种特征地形下，领航机器人不改变原有路径方向，按照规划路线行驶，跟随机器人保持原有队形行驶.

单侧不可通过：凹下和凸起地形分别如图9和图10所示. 机器人可以从另一侧通过，领航机器人左转或右转一定角度避开突然凸起和下凹地形，跟随机器人保持队形并避开危险地形. 由于RRT路径规划器路径有可能贴近这些地形边缘，为了避免意外，需要依据地形调整行驶路径.

中间可通过：两侧凸起和凹下地形分别如图11和图12所示. 领航机器人沿着中心通过该地形，跟随机器人切换为柔性队形，跟随领航机器人，不保持原有队形，防止跟随机器人跌落和碰撞等.

上下坡：如图13和图14所示，深度信息相似，结合姿态传感器综合判断地形，适当减速以减少行驶过程的颠簸，激活地形行驶策略以减少打滑、卡住等故障.

不可通过地形：正常的路径规划一般不会遇到这种情况. 但是为了防止环境中存在的不确定性等，对凸起和凹下不可通过的地形（如图15和图16所示），领航机器人必须停止行驶、倒退、右转或者左转绕行，但在全局路径规划中应该通过参数优化、路径优化等，尽可能避免出现这种情况.

图7 参考行驶路径

图8 空旷地形

图9 单侧凹下地形

图10 单侧凸起地形

4 仿真结果与分析

4.1 固定队形多机器人编队运输任务

4个车式移动机器人初始位姿分别为：

期望队形参数：

编队中机器人由初始位置先完成菱形编队，然后保持队形沿着预定参考轨迹行驶到目标位置. 环境中地形由图7（a）导入MRDS生成，可见环境地形起伏明显，其中高坡和峡谷地形是机器人前进过程中需要尽可能避免的. 沿着参考轨迹在较为平缓的地形上行驶，由于仿真采用物理引擎，多机器人编队行驶仿真过程中存在随机的环境噪声、CPU占用率、分布式线程响应等因素影响，以及仿真最大速度设置等不同，每次完成时间不同. 在同一最大速度指标下，误差小于3 s. 只要编队中有一个机器人卡死或翻车就判定任务失败. 在编队运输过程中还会出现运输车辆碰撞陡坡和滑落深坑等问题，但概率很小.

各机器人的轨迹如图17所示，其中a，b，c，d 4个点的编队仿真场景如图18所示. 虽然参考路径选择平缓地形优先进行路径规划，但是由于行驶地面崎岖不平，机器人行驶仍然比较颠簸，地形颠簸的程度可以由领航机器人的姿态曲线（图19）看出. 其中α为俯仰角，β为倾斜角，机器人姿态角度大部分小于10°，可知全局路径规划使机器人编队基本在平缓起伏的路面上行驶. 跟随机器人的位置误差如图20所示，可以看出编队以较小误差沿着预定轨迹从起点安全行驶到终点.

如图18所示，在编队两侧的机器人由于行驶路面狭窄容易发生卡住、翻车等问题，需要通过灵活队形切换策略更好解决这类问题.

4.2 调整队形多机器人编队运输任务

图11 两侧凹下地形

图12 两侧凸起地形

图13 上坡地形

图14 下坡地形

图15 陡峭凸起地形

图16 陡峭凹下地形

图17 编队各机器人实际轨迹图

图18 行驶过程场景

机器人初始位姿分别为：

期望队形参数：

编队中机器人由初始位置先完成矩形编队，然后保持队形沿着预定参考轨迹行驶到目标位置. 环境中地形由图7（b）导入MRDS生成，可见环境中地形起伏明显，行驶过程遇到狭窄地形，需要切换为柔性队形. 只要编队中有一个机器人卡死或翻车就判定任务失败. 任务失败如图21所示，机器人R4在通过狭窄通道进行队形调整过程中，由于坡度较陡峭，出现翻车故障，但是其他机器人仍然能够保持编队行驶. 由于地形环境限制，机器人队形进行了8次队形调整. 每次队形调整，由于机器人当前位置远离配置位置，都会有较大的初始误差，但都很快收敛. 编队切换过程如图22所示.

图19 领航机器人姿态

图20 跟随机器人位置误差

图21 编队切换失败

图22 编队切换过程

图23 编队路径轨迹图

图24 领航机器人姿态

图25 跟随机器人位置误差

由于地形起伏并且机器人行驶速度较快，对队形切换速度要求较高，不能采用平缓队形切换策略. 跟随机器人通过参考全局路径信息，结合地形对队形进行动态调整. 由于起伏地形行驶过程较为颠簸，机器人配置的深度视觉传感器会受到干扰，所以每次队形切换时间会有差别.

各机器人的轨迹如图23所示. 虽然参考路径选择平缓地形优先进行路径规划，但是由于行驶地面崎岖不平，地形颠簸起伏的程度可以由领航机器人的姿态曲线（图24）看出. 跟随机器人的位置误差如图25所示，可以看出编队以较小误差沿着预定轨迹能够自主调整队形从起点安全行驶到终点.

5 总结与展望

本文研究了多机器人在起伏地形环境下的路径规划问题. 首先， 依据坡度对地形特征进行处理得到可规划路径的区域，并应用双向RRT算法进行全局路径规划. 对于可规划区域边缘易出现事故等问题，利用深度视觉传感器，将局部行为策略和全局路径规划相结合，并通过两个典型的起伏地形下多机器人路径规划仿真，验证了所提方法的有效性. 但是本文仅考虑环境中地形起伏，没有考虑有乱石、树木等障碍物情况，需要进一步研究. 同时也需要提高仿真中路径规划的成功率及切换队形的效率等.