0 引　言

在无人艇的广泛应用中，路径规划始终是其核心技术^[1]。无人艇工作海域通常含有较多未知目标，其运动信息通常无法预先掌握，需通过艇载的各类传感器（如雷达、红外、电视等）进行目标探测录取，获取实时局部周边动态。因此，常见的全局路径规划算法（如基于格栅图的REA*算法^[2]、基于采样方法的RRT*算法^[3]、智能算法AG^[4-5]等）并不能与实时变化的场景相适应。而传统动态窗口法（DWA）作为局部路径规划算法，可根据局部态势变化进行机动避障。同时DWA相较于其他局部路径规划算法（如人工势场法PFM、时间弹性带法TEB^[6]、向量场直方图法VFH^[7]），能充分结合无人艇本身欠驱动特性，划定可行的速度组合生成路径，在编队集结运动中拥有更强的适应性。

Chang等^[8]将强化学习的方法应用于传统DWA算法，通过周围环境变化动态调整评价函数权值，进一步提高机器人路径规划能力。王永雄等^[9]提出一种参数自适应DWA算法，通过计算机器人周围障碍物距离及障碍物密集度动态变化评价函数权值，较好穿越稠密障碍物。常路等^[10]通过修正传统DWA算法中3个评价指标函数和添加2项新评价函数的方法进行改进，增强了机器人导航及全局搜索能力。刘渐道等^[11]将COLREs规则融合于动态窗口法，提高了无人艇在海上复杂交通环境中自主避碰能力，进一步保证了在机动过程中的航行安全。

综合以上研究发现，目前DWA在多无人艇编队集结中的应用较少，绝大多数是对单一机器人的避障机动能力进行改进优化。在无人艇编队集结运动中，对各无人艇之间存在运动中相互避碰的情况，而传统DWA算法对移动目标的避障有3项缺陷：一是采样窗口选择策略不够合理，在避障过程中易陷入局部最优，导致目标减速甚至停止；二是障碍物距离计算不够全面，存在碰撞风险；三是未考虑障碍物移动的情况，避障能力不够全面。因此，本文针对传统DWA这3个方面的问题，对算法进行改进，综合提高无人艇对移动障碍避障能力，使其适用于无人艇编队集结任务。

1 传统DWA算法

传统DWA算法由Fox等^[12]提出，通过机器人自身物理限制的极限速度和极限加速度约束，结合客观环境的障碍物约束，得到一定时间窗口内可行速度组合 $ （v,w） $ 。其中， $ v $ 为线速度， $ w $ 为角速度。3类约束具体表达为：

$ {V_s} = \left\{ {\left( {v,w} \right)|{v_{\min }} \leqslant v \leqslant {v_{\max }},{w_{\min }} \leqslant w \leqslant {w_{\max }}} \right\}，$

(1)

$ \begin{split} {V_d} =& \left\{ \left( {v,w} \right)|{v_c} - \dot v\Delta t \leqslant v \leqslant {v_c} + \dot v\Delta t,\right.\\ & \left.{w_c} - \dot w\Delta t \leqslant w \leqslant {w_c} + \dot w\Delta t \right\}，\end{split} $

(2)

$ {V_a} = \left\{ {\left( {v,w} \right)|{v_a} \leqslant \sqrt {2 \cdot {\rm{dist}}\left( {v,w} \right) \cdot \dot v} ,{w_a} \leqslant \sqrt {2 \cdot {\rm{dist}}\left( {v,w} \right) \cdot \dot w} } \right\} 。$

(3)

式中： $ {V_s} $ 为自身极限速度约束； $ {V_d} $ 为自身极限加速度约束； $ {V_a} $ 为障碍物约束； $ {v_c} $ 为当前时刻线速度； $ \dot v $ 为线加速度； $ {w_c} $ 为当前时刻角速度； $ \dot w $ 为角加速度； ${\rm{dist}}\left( {v,w} \right)$ 为轨迹与最近障碍物的距离。

这3类约束交集，即为可行速度范围：

$ {V_r} = {V_s} \cap {V_d} \cap {V_a} 。$

(4)

根据设定精度采样所有可行速度组合 $ （v,w） $ ，对其所形成的预测轨迹进行评价，取分值最高者为理想轨迹，评价函数定义如下：

$ E\left( {v,w} \right) = \sigma \left( {\alpha \cdot {\rm{head}}\left( {v,w} \right) + \beta \cdot {\rm{dist}}\left( {v,w} \right) + \gamma \cdot {\rm{vel}}\left( {v,w} \right)} \right)。$

(5)

式中： $ \alpha $ 、 $ \beta $ 、 $ \gamma $ 为权重系数； ${\rm{head}}$ 函数为轨迹末端航向角与目标方位角之差； $ {\rm{dist}} $ 函数为目标与最近障碍物距离； $ {\rm{vel}} $ 函数为线速度大小； $ \sigma $ 为归一化处理。

2 改进DWA算法 2.1 修改采样窗口和评价函数

用传统DWA算法中，对采样窗口的障碍物约束 $ {V_a} $ 仅基于障碍物距离进行判断，未同时结合障碍物距离、方位进行全面判断，存在一定局限性。导致采样窗口选择过于保守，丢弃部分可行轨迹。如图1所示。

图中，障碍物碰撞半径为R，角速度 $ {w_0} \geqslant {w_1} $ ，预测轨迹 $ （v{}_{0},{w}_{1}） $ 终点与障碍物相切，此时障碍物处于正横位置，预测轨迹 $ （v{}_{0},{w}_{0}） $ 终点与障碍物距离为 $ R + \Delta d $ 。基于传统DWA算法，若此时预测轨迹 $ （v{}_{0},{w}_{0}） $ 恰好符合障碍物约束 $ {V_a} $ ， $ \Delta d $ 为临界刹车距离，则角速度窗口大于 $ {w_0} $ ，因此预测轨迹 $ （v{}_{0},{w}_{1}） $ 将被剔除。但从实际情况分析，预测轨迹 $ （v{}_{0},{w}_{1}） $ 仅需保持当前运动状态即可实现避障，是可行轨迹，因此理想的角速度窗口应大于 $ {w_1} $ 。综上，需对采样窗口策略中的障碍物约束进行优化调整，使采样窗口保留更多可行的预测轨迹，优化后的 $ {V_a}' $ 约束表达式为：

$ \begin{split} {V_a}'= & \left\{ \left( {v,w} \right)|{v_a} \leqslant \sqrt {2 \cdot {\rm{dist}}\left( {v,w} \right) \cdot \dot v} \cap |{\rm{cour}}|\geqslant\right.\\ & \left. \arcsin \left( {R/{\rm{dist}}\left( {v,w} \right)} \right),{w_a} \leqslant \sqrt {2 \cdot {\rm{dist}}\left( {v,w} \right) \cdot \dot w} \right\}。\end{split} $

(6)

式中， $ {\rm{cour}} $ 函数为障碍物方位角于末端航向角之差。

传统DWA算法评价函数 $ E\left( {v,w} \right) $ 由head、dist、vel这3项指标构成，分别代表自身航向与目标方位愈接近、与障碍物保持安全距离愈大、移动速度愈快的预测航迹能得到更高的评分。但dist评价指标在某些情况下将更优秀航迹赋予更低分数，影响避障效率，具体如图2所示。

图中，假设需避开障碍物并抵达目标点，从全局路径规划上看，轨迹 $ （v{}_{0},{w}_{1}） $ 更为理想，其在满足避障要求的同时花费的路程更短。但在传统DWA算法评价体系中，轨迹 $ （v{}_{0},{w}_{0}） $ 的 $ {\rm{dist}} $ 评价指标评分更高，可能导致最终选择的最优预测轨迹为 $ （v{}_{0},{w}_{0}） $ 。综上，需对评价函数 $ E\left( {v,w} \right) $ 评价内容进行修改，删除 $ {\rm{dist}} $ 评价指标，修改后的评价函数 $ E'\left( {v,w} \right) $ 为：

$ E'\left( {v,w} \right) = \sigma \left( {\alpha \cdot {\rm{head}}\left( {v,w} \right) + \gamma \cdot {\rm{vel}}\left( {v,w} \right)} \right)。$

(7)

2.2 完善dist函数计算范围

传统DWA算法中，对dist函数计算为预测轨迹终点到障碍物的距离。当与障碍物距离与碰撞半径相近且预测轨迹长度大于碰撞半径时，部分情况下，采样窗口会将发生碰撞的无效轨迹纳入评价范围，最终可能导致无效轨迹被评价为最优可行轨迹，造成避碰失败。具体如图3所示。

图中， $ {w_3} \leqslant {w_2} $ ，预测轨迹 $ （v{}_{2},{w}_{2}） $ 终点与障碍物距离 ${\rm{dist}}（{v}_{2},{w}_{2}）$ 符合障碍物约束 $ {V_a} $ ，该轨迹在传统采样窗口判定下为可行轨迹，但其已与障碍物发生碰撞，该速度下的实际角速度窗口应小于 $ {w_3} $ 。

综上，计算预测轨迹终点到障碍物距离的方法存在缺陷，dist函数应完善为计算整条轨迹与障碍物距离的集合，并通过该集合中最小值与碰撞半径进行比较，剔除发生碰撞的无效轨迹。具体如图4所示：

图中，改进后 $ {\rm{dist}}（{v}_{2},{w}_{2}） $ 集合中，最小值为轨迹 $ （v{}_{2},{w}_{2}） $ 与障碍物最小距离，由于该距离小于障碍物碰撞半径，因此无效轨迹 $ （v{}_{2},{w}_{2}） $ 被剔除。

2.3 障碍物运动轨迹预测

传统DWA算法基于对静止障碍物进行局部路径规划，未考虑对移动障碍物的避碰。因此，为使该算法能适用于存在移动障碍物的路径规划环境，需对障碍物的移动速度 $ V $ 进行实时录取，以自身预测轨迹的时间为范围，根据障碍物当前时刻的位置、速度外推障碍物的预测轨迹，将障碍物预测位置作为避障中心进行速度窗口计算。具体实例如图4和图5所示。

通过图4和图5对比，使用障碍物预测终点的改进方法计算的速度窗口 $ {V_r}' $ 较传统DWA速度窗口 $ {V_r} $ 发生较大改变。此改进方法可使速度窗口选择更加合理，能够有效提高USV的避障能力。

2.4 改进DWA算法具体流程

综合对传统DWA算法改进完善，改进后的DWA算法流程如下：

1）设定目标点位置，并根据传感器信息获取周围障碍物实时位置、速度等信息，初步形成总体态势；

2）根据当前时刻USV自身运动性能及预测时间，得到合适大小的原始速度窗口、预测障碍物位置及运动状态；

3）根据预测障碍物位置，利用改进后的速度窗口选择策略剔除无效航迹；

4）通过改进后的评价函数对所有可行航迹进行评分，选择最优航迹 $ （v,w） $ ；

5）根据最优航迹的线速度、角速度执行至下一周期；

6）检查是否到达目标点，若是，则结束运行，若否，则返回第2步。

3 仿真实验与分析

为验证改进后的DWA算法可进一步提升USV避障能力，实现USV编队集结，在Matlab 2017a环境下对传统DWA算法和改进DWA算法进行对比仿真实验，并对结果进行分析。USV运动学参数及DWA算法参数如表1和表2所示。

3.1 改进评价窗口及评价函数验证

根据图1和图2所示场景，结合表1和表2参数进行建模仿真，结果如图7所示。

由图7可知，经优化后的DWA算法，在相同的评价函数内容和权重系数的情况下，速度窗口更加合理，使无人艇在满足避碰距离的前提下，执行路程更短的避碰轨迹，提高了避障效率。

3.2 完善dist函数计算验证

以表1和表2数据为基础，将预测轨迹时间提高至200 s、权重系数 $ \beta $ 降低至0.04，由此进行建模仿真，结果如图7所示。

可知，由于传统算法 $ {\rm{dist}} $ 函数仅计算预测轨迹终点是否满足避障要求，因此在提高预测轨迹时间和降低距离评价权重系数的情况下，预测航迹与碰撞区域相交，评价窗口内出现避障错误的预测航迹，导致了避障失败。而优化后 $ {\rm{dist}} $ 函数即便不纳入轨迹评价指标，仍可准确剔除避障错误的预测航迹，引导USV成功实现避障抵达目标点。

3.3 USV编队集结仿真

依据表1和表2的参数，同时模拟4艘USV同时出发并按指定位置集结组成特定队形，具体如图8所示。

可知，经优化后的DWA算法可在多USV的环境下成功实现编队集结，且任一USV在集结过程中能在自身机动能力范围内对其他USV做出正确的避让机动，各USV航行期间始终保持预先设定的安全距离，未发生碰撞。

4 结　语

本文通过分析传统DWA算法，对避碰过程中预测轨迹窗口存在的问题进行分析。通过修改采样窗口范围和评价函数指标，保留更多的优秀轨迹；通过完善 $ dist $ 函数计算，剔除将发生碰撞的错误轨迹。针对多USV集结过程中需对移动目标进行避障的环境，引入障碍物轨迹预测模型，提高USV对移动目标的避障能力。仿真验证表明，改进后的DWA算法可更好的去劣择优，提高USV的避障能力和避障效率，完成USV编队集结任务。