﻿ 基于改进动态窗口法的无人艇编队集结研究
 舰船科学技术  2023, Vol. 45 Issue (23): 91-95    DOI: 10.3404/j.issn.1672-7649.2023.23.016 PDF

Research on USV formation aggregation based on improved DWA algorithm
WEI Ge-an, ZHANG Jian-qiang
Ordnance Engineering College, Naval University of Engineering, Wuhan 4300331, China
Abstract: An improved dynamic window method is presented to solve the problems of traditional DWA, such as long avoidance time due to falling into local optimum during obstacle avoidance, incomplete distance sampling points leading to obstacle avoidance failure, and poor effect on obstacle avoidance. First, by modifying the decision rules and evaluation functions of the sampling window, more excellent tracks are retained, obstacle avoidance paths are optimized, and obstacle avoidance time is shortened. Secondly, by further improving the obstacle distance calculation during the whole pre-trajectory process, the collision trajectory is eliminated and the success rate of obstacle avoidance is improved. Thirdly, the obstacle avoidance ability is enhanced by introducing the obstacle motion track prediction model. Finally, the simulation results of unmanned vehicle formation assembly based on the improved algorithm using Matlab show that the improved algorithm can improve the obstacle avoidance ability of unmanned vehicle and realize the formation assembly of unmanned vehicle.
Key words: DWA algorithm     USV     obstacle avoidance     formation aggregation
0 引　言

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

1 传统DWA算法

 ${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_r} = {V_s} \cap {V_d} \cap {V_a} 。$ (4)

 $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)

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

 图 1 采样窗口分析 Fig. 1 Sampling window analysis

 $\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)

 图 2 评价函数分析 Fig. 2 Evaluation function analysis

 $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函数计算范围

 图 3 传统dist函数计算 Fig. 3 Traditional dist function calculation

 图 4 改进dist函数计算 Fig. 4 Improved dist function calculation

2.3 障碍物运动轨迹预测

 图 5 传统DWA速度窗口 Fig. 5 Traditional DWA speed window

 图 6 改进DWA速度窗口 Fig. 6 Improve DWA speed window

2.4 改进DWA算法具体流程

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

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

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

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

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

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

3 仿真实验与分析

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

 图 7 优化前后对比图 Fig. 7 Comparison chart before and after optimization

3.2 完善dist函数计算验证

3.3 USV编队集结仿真

 图 8 各种队形图 Fig. 8 Various formation charts

4 结　语

 [1] THOA M T, COPOT C, TRAN D T, et al. Heuristic approaches in robot path planning[C]// The Proceedings of 2016 IEEE Robotics an Autonomous Systems, 2016: 13–28. [2] ZHANG An, LI Chong, BI Wenhao. Rectangle expansion A* pathfinding for grid maps[J]. Chinese Journal of Aeronautics, 2016, 29(5): 1385-1396. DOI:10.1016/j.cja.2016.04.023 [3] QURESHI A H, AYAZ Y. Potential functions based sampling heuristic for optimal path planning[J]. Autonomous Robots, 2016, 40(6): 1079-1093. DOI:10.1007/s10514-015-9518-0 [4] 柳长安, 鄢小虎, 刘春阳, 等. 基于改进蚁群算法的移动机器人动态路径规划方法[J]. 电子学报, 2011, 39(5): 1220-1224. [5] 王晓燕, 杨乐, 张宇, 等. 基于改进势场蚁群算法的机器人路径规划[J]. 控制与决策, 2018, 33(10): 1775-1781. DOI:10.13195/j.kzyjc.2017.0639 [6] ROESMANN C., FEITEN W., WOESCH T., et al. Bertram. trajectory modification considering dynamic constraints of autonomous robots[J], ROBOTIK 2012; 7th German Conference on Robotics, 2012, pp. 1–6. [7] STATHEROS T, HOWELLS G, MCDONALD-MAIER K. Autonomous ship collision avoidance navigation concepts, technologies and techniques[J]. The Journal of Navigation, 2008, 61: 129-142. DOI:10.1017/S037346330700447X [8] CHANG L, SHAN L, JIANG C, et al. Reinforcement based mobile robot path planning with improved dynamic window approach in unknown environment[J]. Autonomous Robots, 2021, 45(1): 51-76. DOI:10.1007/s10514-020-09947-4 [9] 王永雄, 田永永, 李璇, 等. 穿越稠密障碍物的自适应动态窗口法[J]. 控制与决策, 2019, 34(5): 927-936. DOI:10.13195/j.kzyjc.2017.1497 [10] 常路, 单梁, 戴跃伟, 等. 未知环境下基于改进DWA的多机器人编队控制[J]. 控制与决策, 2022, 37(10): 2524-2534. DOI:10.13195/j.kzyjc.2020.1817 [11] 刘渐道, 刘文, 张英俊, 等. 基于改进动态窗口法的无人水面艇自主避碰算法[J]. 上海海事大学学报, 2021, 42(2): 1-7. DOI:10.13340/j.jsmu.2021.02.001 [12] FOX D, WOLFRAM B, SEBASTIAN T. The dynamic window approach to collision avoidance[J]. IEEE Robotics Autom, 1997(4): 23–33.