基于动态窗口法的无人艇局部路径规划

高 宇，赵嵩郢

应用研究

基于动态窗口法的无人艇局部路径规划

高 宇1，赵嵩郢2

(1. 渤海造船厂集团有限公司，葫芦岛 125000；2. 武汉船用电力推进装置研究所，武汉 430064)

本文设计了一种基于动态窗口法的无人艇局部路径规划方法。首先建立无人艇运动学模型，结合艇体机动性能给出无人艇运动方程。然后根据无人艇在短时间间隔内能达到的速度设置搜索空间，进而组成动态窗口。在动态窗口中确定速度产生的安全轨迹，设置动态窗口法轨迹评价函数选出下一时刻无人艇航行的最优轨迹，并在Matlab中进行仿真验证。结果显示，本文所设计基于动态窗口法的无人艇局部路径方法可以有效结合水面无人艇运动学参数，并充分考虑避碰规则以及障碍物因子，在其趋向目标点的同时也有效躲避障碍物，提高水面无人艇的自主避障能力。

水面无人艇 局部路径 动态窗口法

0 引言

无人水面艇(unmanned surface vessel，USV)作为一种智能化小型水面运动平台，在海事监管、军事作战及环境检测等领域充当了重要角色[1]。水面无人艇具有反应快速、机动性强、自动化水平较高的特点，可以快速有效地完成指定任务[2]。路径规划是无人艇智能航行模块的重要组成部分，也是无人艇绕开海面障碍物安全抵达目标点的先决条件[3]。

海洋环境复杂，根据对环境地图的理解差异，水面无人艇的路径规划主要可分为两种：基于电子海图的全局路径规划，以及基于传感器信息的局部路径规划[4-7]。全局路径规划侧重于在已知全部地图后的无人艇全局移动路径搜索，其路径精度与地图的分辨率直接相关[8～10]；局部路径规划侧重于对周围环境的感知及障碍物避让，重点在于培养水面无人艇自主避障能力[11-12]。

水面无人艇实际运行过程中，由于电子海图范围较大，且无人艇体型较小，所形成适航环境模型不精确，所以其航行过程中需设置局部路径规划来进一步修正航线，从而保证水面无人艇能够正确避开障碍物，安全抵达目标点。水面无人艇局部路径规划中，主要是通过艇载传感器探测周围环境信息，建立局部地图，并结合局部地图和原来规划的全局路径方向进行局部的路径规划[13]。水面无人艇能否在行进过程正确避开动态障碍物，首先基于其运动学分析，再采用动态规划的方式进行路径规划，及时有效地调整速度或方向，保证安全航行。

1 水面无人艇运动学建模

水面无人艇的运动学模型与动力学模型的建立是其运动规划与控制的基础。由于海况复杂，水面无人艇在航行过程中受风、浪、流影响，导致无人艇的垂荡、横摇和纵摇。其中，横摇对无人艇航行安全的影响最大。由于风、浪、流等环境干扰因素在建立无人艇运动学及动力学模型过程中产生过多不确定因素，特别在对水面无人艇进行闭环控制时，无人艇本体及周围环境信息存在测量误差，且测量信号受随机噪声的影响。因此，在本文方法中，为保证所建立数学模型的准确性和控制系统的有效性，将水面无人艇的航行过程，近似看作一种服从于二维平面的平面运动，忽略垂荡和纵摇等三维不定因素的影响和各运动状态间的概合，仅考虑横摇的影响[10]。

式中，为无人艇艇体总质量。xG和zG分别是无人艇重心在轴和轴上的坐标，Ixx和Izz分别是其相对轴和轴的惯性矩，Ixx是其在平面的平面惯性积。和分别是无人艇相对轴和轴的运动动量，和是其在轴和轴上的动量矩。

2 水面无人艇速度空间

在完成了艇体运动学建模后，水面无人艇的局部运动轨迹主要通过速度空间(,)确立。动态窗口法的搜索策略为无障碍最优方向，通过速度空间可以快速得出下一时刻规划轨迹的最优解，从而将纵向与横向两个维度的优化简化成一个维度的优化，使其在趋向目标节点的同时也能躲避障碍物。由于在速度(,)的二维空间中存在无数种速度组合，从而对应无数种运动轨迹。为控制采样速度范围，减少冗余计算量，通常速度空间(,)需受最大最小速度约束、动力学约束以及安全约束。

如图2所示构建水面无人艇速度空间，横坐标为前行角速度，纵坐标为前行线速度，故水面无人艇的最大最小速度空间V为图示整个长方形区域。V表示在控制周期内考虑动力学性能无人艇可达的速度空间。图中浅色区域为无人艇可安全行驶的空间V，深色区域则表示无人艇在该速度下可能与障碍物产生碰撞，白色矩形区域则为上述集合的交集V，即最终确定的采样空间窗口，也称为无人艇在该时刻的动态窗口。

图2 水面无人艇速度空间

式中，min、max为无人艇前行线速度的最大值及最小值，min、max为前行叫速度的最大值及最小值。

2）动力学性能约束：由于艇体动力限制以及环境因素影响，无人艇速度空间受艇体最大加速度影响。V为水面无人艇动力学可达的速度空间：

3）碰撞安全约束：为保证无人艇在与障碍物发生碰撞前速度需降为零。V为探测器安全的速度空间：

满足上述约束条件，则V为水面无人艇的安全速度空间，即有效动态窗口：

因间隔时间短，为便于简化计算避免冗余，设定无人艇在每个时间周期内速度恒定。在连续的速度空间Vr中，对速度采样点进行离散处理，以此得到无人艇在单个时间周期内每个采样点的运动轨迹。

如图3所示，矩形方框表征水面无人艇，途中曲线为无人艇前行可选择的轨迹。其中，轨迹b和轨迹a为最大线加、减速度和最大角加、减速度下的极限速度轨迹，轨迹v为无人艇当前速度轨迹。

3 动态窗口法轨迹评价优化

基于所得无人艇可行轨迹，下一步需选择最优轨迹并给定无人艇与其对应的速度空间()。动态窗口法的轨迹评价函数主要考虑三个方面，即所得轨迹与参考路径的切合程度、所得轨迹与参考路径终点的距离以及所得轨迹上是否途径障碍物。

由此可设定动态窗口法轨迹函数为：

上式中，为距离最近障碍物与所得轨迹的距离，为无人艇与障碍物需保持的最小距离，若所得轨迹上不经过障碍物，可将设置为一个较大的常数，从而避免其在评价中权重过大，影响评价标准。

从而，所得的动态窗口法轨迹评价函数可以满足无人艇局部路径规划需求，即保持较高航速下完成对于障碍物的及时规避，其算法流程如图5所示：

其具体实现步骤如下：

1）根据艇载传感器探测的局部环境信息构建局部栅格地图；

3）根据公式（7）对速度空间里的所有轨迹进行评价；

4）选择无人艇局部路径规划的最优轨迹；

5）输出轨迹，重复迭代，直至到达目标点。

图5 动态窗口法算法流程

图6 动态窗口法寻路过程示意

如图6所示，在局部栅格地图中无人艇依据动态窗口法进行局部路径规划。其中，黑色栅格表示不可通行障碍物；白色栅格代表无人艇可通行区域。曲线w为无人艇运行的过往轨迹曲线，黑色直线为无人艇当前航向，曲线u集合为无人艇下一时间间隔中的轨迹空间，所设置评价函数将从中选出最优轨迹并输出执行。

4 仿真实验对比与分析

为评价动态窗口法应用于无人艇局部路径规划场景中的应用效果，本文基于Matlab2018b搭建仿真场景，对动态窗口法应用效果进行验证，设置的算法初始参数如下：

表1 动态窗口法参数设置

局部栅格地图尺寸大小为10 m×10 m，水面无人艇的出发点为(0, 0)，目标点为(10, 10)。

图7 动态窗口局部寻路仿真结果

5 结果分析

本文所设计基于动态窗口法的无人艇局部路径方法可以有效结合水面无人艇运动学参数，并充分考虑避碰规则以及障碍物因子，在其趋向目标点的同时也有效躲避障碍物，能有效提高水面无人艇的自主避障能力。本文研究也存在不足之处，即对于动态障碍的考虑较少，后续将结合多通道的传感器信息实现局部栅格地图的实时更新，并基于真实环境对本文方法进行进一步的验证及优化。

[1] 杨文韬.世界无人水面艇发展综述[J]. 现代军事, 2014(10): 58-60.

[2] 胥凤驰, 王伟, 李哲等. 无人艇系统的设计实现与未来展望[J]. 舰船科学技术，2019, 41(23): 39-43.

[3] 刘建. 水面无人艇路径规划技术的研究[D]. 江苏科技大学, 2014.

[4] Mousazadeh H, Jafarbiglu H, Abdolmaleki H, et al. Developing a navigation,guidance and obstacle avoidance algorithm for an Unmanned Surface Vehicle (USV) by algorithms fusion[J]. Ocean Engineering, 2018, 159: 56-65. Aided Desigh, 1993,25(11): 801-811.

[5] 曲毅, 潘民子. 无人艇路径自主规划方法及策略研究综述[J]. 信息通信, 2019(08): 278-280.

[6] Wang H, Wei Z. Stereovision based obstacle detection system for unmanned surface vehicle[C]. IEEE International Conference on Robotics and Biomimetics. IEEE, 2014: 917-921.

[7] Bibuli M, Singh Y, Sharma S, et al. A two layered optimal approach towards cooperative motion planning of unmanned surface vehicles in a constrained maritime Environment[J]. IFAC-Papers On Line, 2018, 51(29): 378-383.

[8] 范云生, 赵永生, 石林龙, 等. 基于电子海图栅格化的无人水面艇全局路径规划[J]. 中国航海, 2017, 40(01): 47-52+113.

[9] 庄佳园, 万磊, 廖煜雷, 孙寒冰. 基于电子海图的水面无人艇全局路径规划研究[J]. 计算机科学, 2011, 38(09): 211-214+219.

[10] 饶森. 水面无人艇的全局路径规划技术研究[D]. 哈尔滨工程大学, 2007.

[11] 张凤, 谈大龙. 动态未知环境下基于相对坐标系的移动机器人实时运动规划[J]. 机器人, 2004, 26(5): 434-438.

[12] 戴光明. 避障路径规划的算法研究[D]. 华中科技大学, 2004.

[13] 杜开君, 茅云生, 向祖权, 周永清, 宋利飞, 刘彬. 基于海事规则的水面无人艇动态障碍规避方法[J]. 船海工程, 2015, 44(03): 119-124.

[14] 张洋洋, 瞿栋, 柯俊等. 基于速度障碍法和动态窗口法的无人水面艇动态避障[J]. 上海大学学报(自然科学版), 2017, 23(01): 1-16.

Local path planning of unmanned vehicle based on dynamic window method

Gao Yu1, Zhao Songying2

(1. Bohai Shipyard Group Limited Company, Huludao 125000, Liaoning, China; 2. Wuhan Institute of Marine Electric Propulsion, Wuhan 430064, China)

TP332

A

1003-4862（2022）07-0050-05

2022-02-11

高宇（1995-），男，助理工程师。研究方向：自动化。E-mail: 15872427429@163.com