在移动机器人的各项应用中, 路径规划一直是核心技术之一[1-2], 其方法可分为全局和局部路径规划. 常见的全局路径规划算法包括基于栅格图的方法(REA*)[3]、基于采样的方法(RRT*)[4]和一些智能算法(AG)[5]等. 此类算法通常基于精确的全局地图, 而在实际场景中往往难以提前掌握. 局部路径规划算法指机器人根据各类传感器(2-D激光雷达[6]、相机[7])数据进行实时规划, 更能适应未知的工作环境. 常见的局部路径规划算法中, Bug算法通过绕障碍轮廓运动实现避障. 一些方法通过检测路径中“突变点”(E-bug)[8]或参考全局地图信息(K-bug)[9]等方式优化路径长度和平滑度. 人工势场法(APF)通过虚拟势场驱动机器人运动, 大范围环境下易陷入局部最优. 一些方法通过设计新型势场函数[10]或引入“旋转力”引导机器人脱离死锁位置[11]. 此类算法一般难以考虑机器人运动学模型, 易使生成的路径无法跟踪. 动态窗口法(DWA)由可行速度组合生成轨迹并评分, 直接得到最优速度, 无需路径跟踪, 也能充分考虑机器人的物理限制、环境限制和当前状态[12].

Chang等[13]基于强化学习动态调整DWA参数, 提高了规划成功率. Ballesteros等[14]提出BDWA, 使DWA能够模仿真实行人轨迹. 王永雄等[15]设计了自适应规则动态调整DWA评价函数的权重, 使机器人更安全、高效地穿越稠密障碍物.

目前, DWA依然存在两个主要问题: 1)传统DWA采样窗口较为保守, 3个原始评价函数也存在不合理之处, 导致机器人在障碍物前犹豫许久以及在目标点附近运动缓慢, 缺少对DWA现有机理的改进; 2)为改善DWA局部最优问题, 部分算法参考全局路径将DWA作为轨迹跟踪方法, 但未知环境下参考路径难以获得, 此类算法将无法运行, 需要设计新机制增强DWA单独全局规划的能力.

多机系统较单机器人可完成更复杂的任务, 其编队算法包括领航-跟随法、虚拟结构法和基于行为法等. 领航-跟随法中各成员所处环境不同, 使运动状态难以统一且避障表现欠佳. Ahmed等[16]提出一种线性动态反馈策略, 在无障碍环境下保持了队形的稳定. Xiao等[17]提出了一种集中式领航-跟随编队方法, 考虑无障碍环境下的变机器人数量编队问题.

虚拟结构法在无障碍时可证明编队稳定性.Mehrjerdi等[18]结合图论, 实现了各机独立运动和编队两种模式. 虚拟结构法与领航-跟随法类似, 避障一般需结合其他算法, 如潘无为等[19]借助APF实现了简单障碍下的多AUV编队及避障。

基于行为法分别控制各编队成员行为, 适用于具体任务, 但不易保持队形. Yang等[20]定义了编队和避障两种行为, 设计模糊控制率进行行为切换. 与本文思路类似, Gamage等[21]设计奔向目标、沿墙运动、避障等行为, 建立离散事件模型(DES)切换各行为, 但需要预知障碍类型和全局参考路径.

未知环境中, 无法获知参考轨迹及预测成队情况, 需要编队具有全局导航、局部避障和保持队型能力. 借鉴现有方法思想, 领航者主要承担全局规划任务, 而基于行为法可兼顾避障及成队需求. 因此, 本文对传统DWA机理进行分析和修改, 将领航-动作法与基于行为法相结合, 研究基于改进DWA的单机器人路径规划和多机器人编队问题.主要工作如下：

1) 针对传统DWA算法机制不合理、易陷入局部最优等缺陷, 提出一种改进DWA算法, 对其中采样窗口和评价函数部分进行分析和修改, 提高单机器人的通行效率并增强未知环境下的全局搜索能力.

2) 针对现有多机编队算法难以适应未知环境的问题, 提出一种领航-跟随法与基于行为法相结合的编队控制算法. 将改进DWA用于领航者增强编队全局规划能力, 加入等待机制加快队形形成和恢复. 跟随者具有跟踪、避障和等待3种行为模式, 基于改进DWA和社会力模型(SFM)[22]获得控制信号, 实现未知环境下编队的整体导航和队形灵活变换.

考虑未知环境, 仅具有一张空的全局地图记录机器人和目标位置, 机器人的初始位姿和终点已知, 环境中障碍未知, 机器人及其编队的任务为安全到达其终点. 针对双轮差分驱动机器人, 仅考虑其旋转和向前运动, 控制信号为线速度v和角速度ω.

算法所需数据均来自机器人搭载的传感器(陀螺仪、激光雷达、编码器等), 给出如下假设.

假设1  单机器人在起点及终点的位置可达, 多机器人编队在起点及终点的队形可以形成.

假设2  每个时刻, 所有机器人的朝向、位置和速度以及全局目标点位置已知.

假设3  每个时刻, 对于每个机器人, 其周围每个角度上最近障碍物的距离已知, 此数据由激光雷达采集, 距离大小不超过其检测上限.

DWA算法将路径规划问题转化为带约束的速度优化问题, 传统方法对速度组合$ (v, \omega) $设计了3种约束得到采样窗口[12]. 其中速度和加速度约束考虑机器人运动学模型, 障碍约束要求机器人在碰到障碍物前停止, 其形式为

其中$ {\rm dist}(v, \omega) $为该速度组合对应的预测轨迹离障碍物的最短距离.

根据速度组合生成预测轨迹, 通过下式的评价函数进行评分, 输出最高分轨迹对应的速度组合:

其中: $ {\rm heading}(v, \omega) $为导航函数, 倾向于末端位姿朝向目标的轨迹; $ {\rm obdist}(v, \omega) $为避障函数, 倾向于远离障碍物的轨迹; $ {\rm velocity}(v, \omega) $评价机器人线速度, 鼓励其迅速通行; $ w_1 $、$ w_2 $、$ w_3 $为3种子评价函数的权重; σ表示归一化.

针对传统DWA速度窗口保守评价函数机制不合理、机器人易陷入局部最优等问题, 本节提出一种改进DWA算法, 对速度窗口和现有的3个评价函数进行修正, 并添加了两个新评价函数以解决环境未知等复杂场景下的路径规划问题.

传统DWA中, 式(1)的$ \operatorname{dist}(v, \omega) $项是整条预测轨迹离障碍物的最短距离, 预测轨迹的时间一般取数十个时间间隔, 期间速度视为恒定, 而机器人的速度在每个时刻都将重新选取, 故式(1)计算离障碍物距离的方式较为保守. 此外, 式(1)中加速度项a和α为机器人正常行驶时的值, 该约束要求机器人及时停车, 应使用刹车加速度更为合理, 而低速行驶的小车几乎可以立刻刹车, 故其加速度的取值也较为保守.

基于以上两点, 式(1)的障碍约束过于保守, 会排除一些实际可行的速度, 从而影响通行效率.因此, 本文移除该约束, 仅根据原速度和加速度约束得到速度窗口.对于障碍约束, 将在3.3节以另一种方式体现.

该函数为导航函数. 考虑图 1所示的情况, 轨迹1的转弯趋势更为理想. 而按照原函数的评价方式以轨迹末端位姿计算角度可知$ \theta_{o, 2} R_{\rm safe};\\ {\rm discarded}, D_{\min}(r_{n_{\Delta t, o}}, {\rm obs})\leqslant R_{\rm safe}. \end{cases} \end{align} $ (5)

其中: $ D_{\min}(r_{n_{\Delta t, o}}, {\rm obs}) $为预测轨迹上起点至$ n_{\Delta t, o} $个时间间隔后机器人位置与障碍物的最短距离, $ R_{\rm safe} $为机器人安全半径, $ \operatorname{\min}(\cdot) $为取最小值函数.

该函数使机器人快速移动, 评分仅与线速度大小呈线性关系. 一般而言, 角速度波动易导致震荡, 角速度过大易导致扭曲. 考虑图 3所示的情况, 两种轨迹线速度相同, 轨迹1的角速度较大, 显然轨迹1更为理想. 考虑到机器人在低速行驶时角速度较大不会影响安全性, 添加对角速度的评分机制如下所示:

其中: $ w' $为角速度评分, $ \omega_{t-2} $、$ \omega_{t-1} $和$ \omega_{t} $分别为机器人前两时刻、前一时刻、当前角速度, $ k_1 $、$ k_2 $为系数. 式(6)中第1项评价角速度波动情况, 第2项基于当前线速度评价角速度大小. 该函数修正后的形式为

新增$ {\rm oscillation}(v, \omega) $函数帮助机器人逃离局部最优, 增强DWA在未知环境下的全局搜索能力. 该函数评价预测轨迹和历史轨迹的相似程度, 将全局地图栅格化, 每个栅格有1个代价值记录历史轨迹与该栅格的靠近程度. 代价值增量计算方式如图 4(a)所示. 图中: P为机器人在t时刻的位置; $ R_{\rm rec} $为代价值更新半径; $ d_{ij} $为坐标$ (i, j) $的栅格与P的距离; $ w_{ij} $为栅格$ (i, j) $代价值的增量, 计算如下:

其中$ v_t $为机器人当前速度. 添加速度项是为了避免当机器人低速行驶时周围栅格的代价值激增.

图 5(a)为机器人在简单障碍地图上的轨迹及代价, 颜色越深的栅格代价越高. $ {\rm oscillation}(v, \omega) $函数依据轨迹扫过的所有栅格的总代价值进行评分, 在图 5(b)中即为所有红色栅格的总代价值.

另外, 观察图 4(b), 角速度较大的预测轨迹总会被机器人当前位置高度覆盖. 由于此函数非速度函数, 为消除此影响, 栅格代价值的更新具有0.5 s的延时, 即每次更新使用0.5 s之前机器人的位置和速度.

考虑图 6所示的情况, 两种轨迹线速度相同, 轨迹1更理想. 仅依据传统评价函数, 有$ \theta_A