基于改进动态窗口法的无人船目标跟踪运动规划方法

马金帅，刘兰军，金久才，陈明志

（1.中国海洋大学工程学院，山东 青岛 266100；2.自然资源部第一海洋研究所，山东 青岛 266061；3.自然资源部海洋遥测技术创新中心，山东 青岛 266061）

无人船（Unmanned Surface Vehicle，USV） 是一种新型海上智能平台，具有集成度高、运营成本低等优势，同时具备体积小、隐蔽性好、机动性强等特点，可搭载不同载荷完成环境监测、海上维权等多种任务[1-4]。利用无人船对可疑船只进行快速跟踪、监视，同时回传目标船只信息，可以有效减少人员安全问题。在多浅滩、礁石和渔船的复杂环境下执行运动目标跟踪任务，如何兼顾避障和目标跟踪任务，是一个现实问题。针对目标跟踪任务进行运动规划，合理选择有效的航向航速控制指令，是多障碍物环境下完成目标运动跟踪任务的关键。

针对运动目标跟踪问题，国内外学者开展了大量的研究，研究方向主要包括跟踪制导律研究、路径规划算法研究和人工智能目标跟踪方法研究。在跟踪制导律研究方面，针对无障碍物环境下单目标跟踪问题，挪威科技大学FOSSEN T I[5]系统性研究了3 种常用的无人船目标跟踪制导律，包括纯跟踪法（Pure Pursuit，PP）、方位不变法（Constant Bearing，CB）和视线制导法（Line of Sight，LOS）。针对高速直线行驶目标船只跟踪问题，挪威科技大学MORTEN B 等[6]采用CB 制导律，设计了新型速度控制器，实现了对目标船只的跟踪。韩国首尔大学JONGHOEK K[7]提出了一种新颖的目标跟踪制导律和环绕监视制导律，完成了目标跟踪和环绕监视任务，并从理论上证明了目标跟踪距离的收敛性。哈尔滨工程大学周鸿伟[8]提出了一种模仿跟踪制导方法，无人船实时模拟被跟踪目标的速度和航向，但存在着无人船和目标之间的距离无法缩短的问题。在目标跟踪路径规划算法方面，ŠVEC P 等[9]使用蒙特卡洛采样方法估计目标未来姿态，结合A*算法采用轨迹跟踪方式，完成了目标跟踪任务。ERIKSEN B[10]提出了基于航海雷达的动态窗口法，将LOS 制导律和执行器的特性融合到动态窗口法来跟踪船只目标。自然资源部第一海洋研究所汪栋[11]针对多礁石、渔船等近海复杂环境下静态目标点的跟踪问题，提出了一种基于有限状态机模型的无人船局部路径规划算法。在人工智能无人船目标跟踪研究方面，海军工程大学曹世杰等[12]提出了一种基于强化学习（Q-learning）的无人船目标跟踪规划方法，通过改进学习策略，为静态场景下动静态目标跟踪提供了无模型解。大连海事大学郑帅[13]采用DDPG（Deep Deterministic Policy Gradient） 深度强化学习算法训练优化无人船驾驶决策模型，利用Gazebo 物理仿真器生成模型训练的学习样本，实现了无人船自主追踪漂移遇险目标。

本文针对多障碍物环境下运动目标跟踪问题，提出了一种适用于无人船平台的目标跟踪运动规划方法，将自适应制导律、危险度评估因子引入传统动态窗口法（Dynamic Window Algorithm，DWA）的目标函数中，同时优化了速度采样空间，可快速完成无人船运动目标跟踪任务，并实时规避动静态障碍物。

1 无人船跟踪规划模型

1.1 无人船运动模型构建

本文运动规划研究主要集中于目标跟踪任务中无人船航速和航向的规划，只考虑水平面内三自由度的运动，而忽略无人船的横摇、纵摇和垂荡3 个自由度。坐标系采用北东地（North East Down，NED）坐标系。无人船的运动模型表示如下。

式中，X（t）=[x（t），y（t），v（t），θ（t）]为无人船的状态变量；[x（t），y（t）]为无人船的位置坐标；v（t）为无人船的航速；θ（t）为无人船的航向角。

1.2 障碍物膨化模型

障碍物检测是无人船运行过程中实时避障的关键技术，本文假设无人船传感器系统可以提供无人船和周围环境中障碍物的位置和航向航速信息，控制器性能足够好，不考虑海洋环境对无人船运动造成的影响。

为了简化计算量，将跟踪区域内的动静态障碍物做外接圆处理，将无人船的半径和传感器不确定度作为膨化因子对障碍物进行膨化处理。对于“狭长”的障碍物，使用多个外接圆进行处理。此时忽略无人船尺寸，将无人船视为质点进行处理。

图1 等效障碍物示意图

2 改进动态窗口法

2.1 传统动态窗口法

传统动态窗口法是一种基于采样的反应式局部路径规划方法，其借鉴了预测控制中滚动优化的思想，通过在可行速度、角速度空间中进行采样，模拟机器人在一定时间内的多组运行轨迹，根据目标函数选取最优轨迹对应的速度、角速度来驱动机器人运动。

2.1.1 速度采样空间

无人船在二维空间水平面内运动，由于无人船自身动力学限制、动态障碍物和静态障碍物影响，需要对无人船的实际速度和角速度的选取进行约束。

约束1：无人船最高航速限制、最大角速度限制，决定了速度采样空间动态窗口的上下限。

式中，Vs为约束1 下的无人船速度采样空间；vmin和vmax为无人船的最小航行速度和最大航行速度；ωmin和ωmax为无人船的最小航行角速度和最大航行角速度。速度、角速度参数由无人船的自身性能决定。无人船的航速和角速度的采样必须要在该区间内选取才是合理的采样速度。

约束2：无人船的线加速度和角加速度约束。

式中，Vd为约束2 下的无人船速度采样空间；va和ωa为无人船当前的速度和角速度；v˙和ω˙分别是无人船的最大线加速度和最大角加速度。

约束3：安全避障距离约束，即保证无人船在当前速度情况下进行最大加速度制动，可避免和最近的障碍物碰撞。

式中，Va为约束3 下的无人船速度采样空间；v˙为无人船最大线加速度。

综上，取三者的交集作为动态窗口的速度采样空间。

2.1.2 预测轨迹和目标函数

动态窗口法通过选择合适的向前预测时间，基于速度采样空间中可行速度和可行角速度，生成多条预测轨迹（图2），选取最优轨迹对应的速度和角速度作为控制量，发送给控制层。

图2 无人船轨迹预测图

预测轨迹的评估利用评价函数完成，通常评价函数设计如下。

式中，heading（v，ω）为制导评价函数；dist（v，ω）为避碰评价函数；velocity（v，ω）为速度评价函数；α、β和γ分别是制导函数权重、避障函数权重和速度函数权重。heading（v，ω）制导评价函数表征当前航向角与期望航向角之间的偏差。dist（v，ω）避碰评价函数表征无人船和最近的障碍物之间的距离，当无人船与障碍物之间的距离超过一定值时，该函数值设置为常数。velocity（v，ω）速度评价函数表征无人船的运行速度情况，在满足约束条件的情况下，无人船航速越快越好。heading（v，ω），dist（v，ω）和velocity（v，ω）函数均为归一化之后的函数。

2.2 改进动态窗口法

2.2.1 自适应目标跟踪制导律

为解决动态窗口法在避障时兼顾目标跟踪任务，设计了一种自适应切换制导律，实现纯跟踪（Pure Pursuit，PP） 制导和方位不变法（Constant Bearing，CB） 制导自适应切换。纯跟踪制导律简单、直接，适用于无障碍下快速跟踪动态目标；方位不变法制导律在避障方面更有优势。根据障碍物环境和目标运动状态，自适应切换跟踪制导律，可保障避障的同时，改善跟踪效果。自适应制导律示意图如图3 所示。

纯跟踪制导律属于二点制导方式，在目标跟踪时只考虑无人船和目标的位置信息。此时无人船的期望航向由无人船指向船只目标（图3），期望航向设置如下。

图3 自适应切换制导律示意图

式中，PT为目标位置；PU为无人船的位置；kpp为系数。

将预测轨迹的航向与期望航向的差值作为评价函数的一部分，纯跟踪制导律下，headingpp（v，ω）函数设置如下。CB 制导也被称为平行制导，船员广泛使用CB制导规则来避免船舶之间的碰撞。CB 制导律适合在障碍环境下对目标船只进行避碰跟踪。CB 制导律下的无人船期望航向vnd设置如下。

同理，设置方位不变法制导律headingcb（v，ω）函数如下。

综上，构造heading（v，ω）函数如下。

式中，λ为制导律切换因子。当无人船需要进行避碰时，λ为0，否则λ设置为1。

2.2.2 采样空间优化

动态窗口法只模拟一段时间内的轨迹，而无人船搭载的障碍物检测传感器包括航海雷达等可以在较远的距离检测到障碍物。为保证无人船的航行安全，本文采用最近会遇点（Closest Point of Approach，CPA）方法对会船时可能发生的碰撞进行检测，剔除掉可能会发生碰撞的轨迹。最近会遇点方法主要包括DCPA（Distance to Closest Point of Approach）和TCPA（Time of Closest Point of Approach）。其中，DCPA 是指船舶会船时相互之间的最近距离，TCPA 是指驶抵最近会遇点的时间。DCPA和TCPA计算公式如下。

式中，x0和y0为无人船的位置；xt和yt为障碍物船舶的位置。若TCPA小于0，则表明无人船和障碍物在相互远离，无碰撞风险。若TCPA大于0，则表明无人船和障碍物正在相互靠近，DCPA为两船保持当前速度会遇时最近的距离。通过计算预测轨迹的最近会遇点，将可能产生碰撞的轨迹提前剔除。优化采样空间后的轨迹如图4 所示。

图4 优化后的采样轨迹

2.2.3 考虑碰撞危险度的目标函数构建

无人船在执行目标跟踪任务时，需要完成避障任务。原始动态窗口法只考虑动态障碍物的位置而不考虑动态障碍物的速度，容易出现距离动态障碍 物过近的现象，对动态障碍物避障效果差。为防止出现距离动态障碍物过近的现象，不仅要考虑无人船距离障碍物的当前距离，还需要对两船会遇时可能发生碰撞的危险进行评估。根据式（13）、式（14）计算TCPA和DCPA，可以对动态船舶障碍物碰撞危险度进行评估。

针对“久航490”无人船，设计危险度函数risk（v，ω）如下。

式中，boundary 为最大会遇距离阈值。当无人船与障碍物船只最大会遇距离超过boundary 时，risk（v，ω）函数取值为常数。当TCPA小于0 时，表明无人船和运动的障碍物船只完成了会船，两船处于相互远离的状态。

根据以上，构建新的目标函数如下。

式中，heading（v，ω）为自适应制导律制导评价函数；risk（v，ω）为动态船舶碰撞危险度评估函数；δ为动态船舶碰撞危险度评估函数的权重。

3 仿真结果及分析

3.1 无人船参数和目标跟踪流程

3.1.1 无人船参数

为验证算法的有效性，针对自然资源部第一海洋研究所研发的“久航490”无人船[14]（图5），利用MATLAB 2019b 平台进行仿真实验。根据“久航490”无人船实际动力学性能，设置无人船仿真参数见表1。

图5 “久航490”无人船

表1 “久航490”无人船仿真参数表

3.1.2 目标跟踪算法流程

（1）初始化无人船和障碍物状态：无人船的状态通常由定位系统、惯导等提供，包括无人船的位置、航向、航速等信息。障碍物的信息通常由航海雷达、AIS、激光雷达等障碍物检测传感器提供，本文仿真中假设动静态障碍物都在无人船的探测范围内，为已知信息。

（2）自适应制导律选取：根据无人船和障碍物的状态信息，自适应选择跟踪制导律，从而计算期望航向。

（3）优化速度空间：根据无人船性能和当前状态，确定动态窗口，利用最近会遇点方法优化动态窗口。

（4） 利用改进的目标函数对预测轨迹进行评价，选取最优轨迹对应的控制量作为控制指令，更新无人船和障碍物的状态。如果无人船距离目标船只距离在50 m 以内，则视为完成目标跟踪任务。否则，继续执行以上步骤。

3.2 仿真验证及结果分析

针对本文提出的改进动态窗口法进行验证，采用原始动态窗口法与改进算法进行对比。采用静态障碍物模拟航海雷达检测到的礁石等障碍物，动态障碍物模拟航行的船舶。图6 中黑色船只为运动的目标船只，红色线代表目标船只的航行轨迹。白色双体船代表无人船，绿色线代表无人船航行轨迹。黑色圈代表膨化处理后的静止障碍物，蓝色圈代表膨化处理后的动态船只障碍物。

图6 原始动态窗口目标跟踪仿真轨迹

由图6（a）和图6（b）分析可知，原始DWA 无人船躲避静态障碍物Os1 和Os2 向目标船只抵近。由图6（c）分析可知，原始DWA 在T为50 s 开始躲避动态船只Od1 时。由于原始DWA 只考虑了无人船和船只障碍物之间的距离，而没有考虑动态船只的速度，在距离船只较近时才开始避障，距离动态船只障碍最近距离只有20 m，安全风险极大。由图6（d）分析可知，面对动态障碍物Od2，无人船并未采取避让行为，保持直线航行，对动态障碍物避障效果较差。由图6（e）和图6（f）分析可知，在T为150 s时，无人船期望航向指向目标船只，导致原始方法选择从Os3 和Os4 障碍物之间的狭小缝隙中穿过，无人船在躲避Os4 障碍物航向变化较大，且和动态障碍物之间的距离过近。穿越狭窄空间后，无人船航向和目标船只的航向差值较大，无人船需要一定的调整时间，错过了跟踪可疑机动目标的时机。

由图7（a）、图7（b）和图7（c）可知，改进DWA由于考虑了将来碰撞的风险，在T为30 s 时对动态船只障碍物Od1 采取避让行为，转向时机早，与动态障碍物Od1 保持了足够的安全距离。由图7（d）可知，面对动态障碍物Od2 时，无人船适当向左转向，保持足够的安全距离。由图7（e）可知，面对较为密集的障碍物Os3 和Os4，无人船自适应选择跟踪制导律，从Os4 的右侧驶过，既与两个静态障碍物保持了足够的安全距离，也充分考虑了目标船只的速度和航向，未错过目标跟踪时机。由图7（f）可知，无人船检测到右侧动态船只障碍物时，采取了向左转向保持安全距离的行为，同时跟踪目标船只，逐渐缩短和目标船只之间的距离。

图7 改进动态窗口法目标跟踪仿真轨迹

由图8 分析可知，原始DWA 距离最近障碍物的平均距离为50.8 m，改进DWA 距离最近障碍物的平均距离为63.8 m，安全距离增加了25.6%。原始动态窗口法完成目标跟踪任务用时276 s，改进方法用时184 s，运动目标跟踪时间缩短了33.3%。改进方法在目标跟踪快速性和安全性方面都得到了提高。

图8 目标距离和障碍物距离对比图

由图9 分析可知，采用改进DWA 方法的无人船在目标跟踪过程，航向由0 m/s 逐步增加到允许的最大值5 m/s 后，可保持最高航速完成目标跟踪与避障任务。而原始DWA 方法由于躲避动态障碍物不及时，出现了两次降速现象，在140 s 附近由最高速度5 m/s 降速到1 m/s，易错过跟踪机动船只目标的时机。

图9 跟踪过程航速对比图

综合对比复杂场景下无人船目标跟踪的过程可知，原始DWA 未优化采样空间，一些当前不会产生碰撞但是将来可能产生碰撞的轨迹也被考虑，导致了传统DWA 较为“短视”，容易出现距离障碍物较近时才采取避障行为的情况。改进方法在处理动态障碍物时，通过考虑未来碰撞危险度，采用提前避障行为，不会出现航向大幅度改变的情况，自适应跟踪律的加入让改进方法在跟踪过程中期望航向的选取更加合理，减小了无人船因为避障导致跟丢机动目标的可能性。

4 结 论

针对多浅滩、礁石和渔船的复杂环境下可疑船只目标运动跟踪的问题，本文提出了一种改进无人船目标跟踪运动规划算法。改进DWA 充分考虑了无人船和被跟踪目标的状态，引入了自适应目标跟踪制导律，优化了采样空间和目标函数。通过对比原始DWA 与改进DWA，证明优化后的采样空间可以使无人船提前转向，目标跟踪路径更加平滑。改进方法考虑障碍物的速度信息，可有效躲避动态障碍物，在目标跟踪安全性和快速性方面具有明显的优势，下一步将该算法应用于自研发的无人船系统中，以验证算法的实用性和效果。