基于滚动速度障碍法的AUV动态避障路径规划

章 飞, 胡春磊

基于滚动速度障碍法的AUV动态避障路径规划

章 飞, 胡春磊

(江苏科技大学 电子信息学院, 江苏 镇江, 212003)

针对自主水下航行器(AUV)在动态环境中的路径规划及避障效果不佳的问题, 提出一种可行的滚动速度障碍方法。该方法将滚动窗口法与速度障碍法相结合, 在混合避障结构下基于滚动窗口法设计合适的三维模型预测控制器, 通过优化目标函数实现对参考路径的稳定跟踪; 窗口滚动的同时构建三维碰撞锥与速度障碍锥模型, 若满足触发避碰条件则计算得到临界碰撞点, 通过对临界碰撞点的跟踪引导AUV安全避障, 若避碰结束则引导AUV完成轨迹恢复。仿真结果表明, 该方法可有效提高AUV在实时路径规划时的动态避障能力。

自主水下航行器; 路径规划; 动态避障; 滚动窗口法; 速度障碍法

0 引言

自主水下机器人(autonomous undersea vehicle, AUV)作为一种轻型的水下探测工具, 因其体积小、操控灵活及智能性能高等优点, 已逐渐成为实施海洋资源探索, 增强国家海军实力的重要设备。在复杂多变的海洋环境中, AUV的安全避障技术是其航行与工作的保障, 也是其导航控制技术的重要组成部分。随着各国对海洋探索程度的日益加深, 如何进一步提升AUV在复杂海洋环境中的动态避障和路径规划能力, 已成为制约AUV发挥功效的关键[1]。

AUV避障规划方法通常借鉴于传统移动机器人路径规划方法, 主要分为两大类[2], 一类是依赖于已知环境信息的全局路径规划方法, 如文献遗传算法[3]、A*算法[4]、蚁群算法[5]及神经网络[6]等, 此类方法虽可取得不错的规划结果, 但忽略了环境中未知障碍物对机器人产生的威胁。另一类是依赖于对周围环境感知的局部路径规划方法, 如文献人工势场法[7]、速度障碍法[8]及滚动窗口法[9]等, 此类方法的主要缺陷是缺乏对全局信息的把握, 导致规划所得的路径往往不是全局最优(如最短路径、最小能耗), 严重时会出现目标不可达现象。针对此, Campbell等[10]提出混合避障结构的思想, 预先通过全局规划得到一条参考路径, 机器人按照参考路径航行, 当探测到未知障碍物体时, 机器人在参考路径的基础之上调整方向完成避障。这样, 既充分利用全局地图信息, 又有效提高了机器人的动态避障能力。AUV的动态避障规划可借鉴该思想, 其中全局参考路径的获取前人已多有研究, 文中研究的重点将为如何实现对参考路径的稳定跟踪和对未知障碍物的有效躲避。

窗口滚动优化控制(滚动时域控制)属于一种有效的预测控制方法, 机器人基于实时的环境信息对下一时刻的运动状态进行预测, 以此得到控制对象的最佳输入量, 从而实现对机器人的优化控制, 该方法已广泛应用于机器人在不确定环境中的避障研究。张广洁等[11]采用滚动优化控制的方法, 能够在二维地图内使机器人在线跟踪直线参考路径, 同时避开环境中的未知障碍物, 但这种方法仅适用于二维地图的直线路径, 且不能有效避开环境中较大的或者动态障碍物。速度障碍法是另一种预测控制方法, 能够直接利用机器人和障碍物的速度与位置信息进行动态避碰。如Fiorini等[12]利用速度障碍法构建碰撞锥, 根据机器人、障碍物的相对速度与碰撞锥的关系, 判断碰撞是否发生, 该算法在避障方面简易、高效, 但存在目标不一定可达的致命缺陷。Mingeuk等[13]将速度障碍法应用于多障碍物环境中机器人的动态避障, 通过引入关于速度安全性的成本函数对无碰撞速度进行评估, 从中选择最佳的速度方案, 该方法有效提高了机器人在复杂环境中的动态避障能力, 但由于缺少对全局信息的把握, 依然不能确定规划所得路径的全局最优性。

综上, 文中在混合避障结构的框架下融合改进的滚动窗口与速度障碍法, 提出一种适用于AUV的动态避障方法: 滚动速度障碍法。首先将参考路径进行分段处理, 通过窗口滚动优化控制实现对参考路径的分段跟踪, 整个过程中将二维路径的跟踪控制提升为三维; 窗口滚动的同时建立三维碰撞锥与速度障碍锥模型, 在触发避碰条件下, 重新规划并跟踪新路径实现避碰; 仿真试验结果验证了该算法的有效性。

1 AUV运动及控制模型

图1 AUV三维运动模型示意图

AUV在三维空间内的状态信息变化遵循如下关系

2 滚动窗口跟踪控制器设计

图2 曲线化为折线示意图

式中: 为AUV状态预测量与参考路径上跟踪点之间的误差, 主要用于对参考路径的稳定跟踪; 为AUV与速度障碍法获得的避障引导点之间的误差, 主要用于AUV的动态避障; 为布尔量, 当避障发生时其值为1, 否则为0。取使得目标函数最小时的控制输入量, 结合当前状态与参考路径点得到下一时刻状态, 算法主要流程如图3所示。

3 参考路径稳定跟踪

将式(7)与式(8)代入式(6), 得

4 改进速度障碍法

4.1 碰撞锥模型建立

在滚动窗口跟踪控制模型中引入速度障碍法能够高效且及时地避开未知环境中的动静态障碍物, 该方法需要先建立相应的碰撞锥模型, 为此文中在二维碰撞锥模型的基础之上建立适用于AUV的三维碰撞锥模型。

图5 三维碰撞锥模型

4.2 避障方向引导点确定

图6 三维速度障碍锥模型

图7 AUV与障碍物碰撞点预测模型

5 避障引导与轨迹恢复

6 仿真试验与分析

6.1 静态未知障碍物

图9 AUV避障轨迹(静态障碍物)

图10 参考路径偏移量的AUV轨迹(静态障碍物)

6.2 动态未知避障物

图11 AUV避障轨迹(规则运动障碍物)

图12 AUV到障碍物距离变化(规则运动障碍物)

图13 参考路径的偏移量的AUV轨迹(规则运动障碍物)

为进一步验证文中算法在处理各种动态障碍物的普遍适用性, 首先在上述条件下使2的运动方向发生变化, 速度不变(运动轨迹见图14), 其他环境设置同上, 仿真结果如图14~图17所示。

图14 AUV避障轨迹(不规则运动障碍物)

图15 AUV到障碍物距离变化(不规则运动障碍物)

图16 AUV轨迹与参考路径的偏移量(不规则运动障碍物)

图17 AUV艏向角与纵倾角的变化(不规则运动障碍物)

可见, AUV能够成功到达指定目标点, 且整个过程中与障碍物的距离始终大于其威胁半径(4 m)。图17显示出AUV艏向角与纵倾角随时间的调整变化值均满足实际运动特性的限制要求。

7 结束语

为提升AUV在动态环境中的路径规划及避障能力, 文中在混合避障结构框架下融合改进的滚动窗口与速度障碍法, 通过设计的滚动窗口跟踪控制器实现对参考路径的稳定跟踪, 窗口滚动过程中通过建立碰撞锥与速度障碍锥模型, 获得避障方向引导点, 引导AUV进行避障。

图18 AUV避障轨迹(多个运动障碍物)

图19 AUV到障碍物距离变化(多个运动障碍物)

图20 AUV轨迹与参考路径的偏移量(多个运动障碍物)

该方法优点在于既能充分利用已知的全局地图信息, 保证AUV在接近最优的轨迹上航行, 提升AUV的航行效率, 有效避免了航行目标不可达现象的出现; 又能使AUV具备动态避障的能力, 解除了环境中的未知障碍物对AUV航行安全的威胁, 且在避障完成后AUV能够自动完成轨迹恢复, 继续向目标点航行。

图21 AUV艏向角与纵倾角的变化(多个运动障碍物)

通过设置相应的仿真试验, 验证了该算法的有效性。为使该方法更具实用价值, 下一步的研究工作将着重于增加环境的复杂程度, 提升算法的可靠性。

[1] Mcmahon J, Plaku E. Mission and Motion Planning for Autonomous Underwater Vehicles Operating in Spatially and Temporally Complex Environments[J]. IEEE Journal of Oceanic Engineering, 2016, 41(4): 1-20.

[2] 朱大奇, 颜明重. 移动机器人路径规划技术综述[J]. 控制与决策, 2010, 25(7): 961-967.Zhu Da-qi, Yan Ming-chong. Survey on Technology of Mobile Robot Path Planning[J]. Control and Decision, 2010, 25(7): 961-967.

[3] Song B, Wang Z, Sheng L. A New Genetic Algorithm Approach to Smooth Path Planning for Mobile Robots[J]. Assembly Automation, 2016, 36(2): 138-145.

[4] 陈实, 刘纯武, 黄芝平, 等. 基于稀疏A*算法的AUV全局路径规划[J]. 鱼雷技术, 2012, 20(4): 271-275.Chen Shi, Liu Chun-wu, Huang Zhi-ping, et al. Global Path Planning for AUV Based on Sparse A* Search Algorithm[J]. Torpedo Technology, 2012, 20(4): 271-275.

[5] Cao J. Robot Global Path Planning Based on an Improved Ant Colony Algorithm[J]. Journal of Computer & Comm- unications, 2015, 4(2): 11-19.

[6] Bharadwaj H, Vinodh K E. Comparative Study of Neural Networks in Path Planning for Catering Robots[J]. Pr- ocedia Computer Science, 2018, 133: 417-423

[7] 罗强, 王海宝, 崔小劲. 改进人工势场法自主移动机器人路径规划[J]. 控制工程, 2019, 26(6): 1091-1098.Luo Qiang, Wang Hai-bao, Cui Xiao-jin. Improved Artificial Potential Field Method for Autonomous Mobile Robot Path Planning[J]. Control Engineering of China, 2019, 29(6): 1091-1098.

[8] Fiorini P, Shiller Z. Motion Planning in Dynamic Environments Using the Relative Velocity Paradigm[C]//IEEE International Conference on Robotics & Automation. At- lanta, USA: IEEE, 1993: 560-565.

[9] 张纯刚, 席裕庚. 动态未知环境中移动机器人的滚动路径规划及安全性分析[J]. 控制理论与应用, 2003, 20(1): 37-44.Zhang Chun-gang, Xi Yu-geng. Rolling Path Planning and Safety Analysis of Mobile Robot in Dynamic Uncertain Environment[J]. Control Theory & Applications, 2003, 20(1): 37-44.

[10] Campbell S, Naeem W, Irwin G W. A Review on Improving the Autonomy of Unmanned Surface Vehicles through Intelligent Collision Avoidance Manoeuvres[J]. Annual Reviews in Control, 2012, 36(2): 267-283.

[11] 张广洁, 严卫生, 高剑. 基于模型预测控制的欠驱动AUV直线路径跟踪[J]. 水下无人系统学报, 2017, 25(2): 86-92.Zhang Guang-jie, Yan Wei-sheng, Gao Jian. Straight Line Tracking of Underactuacted AUV Based on Model Predictive Control[J].Journal of Unmanned Undersea Sys- tems, 2017, 25(2): 86-92.

[12] Fiorini P, Shiller Z. Motion Planning in Dynamic Environments Using Velocity Obstacles[J]. The International Journal of Robotics Research, 1998, 17(7): 760-772.

[13] Mingeuk Kim, Jun-Ho Oh. Study on Optimal Velocity Se- lection Using Velocity Obstacle(OVVO) in Dynamic and Crowded Environment[J]. Autonomous Robots, 2016, 40(8): 82-90.

[14] 王怿, 祝小平, 周洲. 3维动态环境下的无人机路径跟踪算法[J]. 机器人, 2014, 36(1): 83-91.Wang Yi, Zhu Xiao-Ping, Zhou Zhou. UAV Path Foll- owing in 3-D Dynamic Environment[J]. Robot, 2014, 36(1): 83-91.

Research on AUV Dynamic Obstacle Avoidance Path Planning Based on the Rolling Speed Obstacle Method

ZHANG Fei, HUChun-lei

(School of Electronic Information, Jiangsu University of Science and Technology, Zhenjiang 212003, China)

To address the problem of poor path planning and the obstacle avoidance effect of autonomous undersea vehicles(AUVs) in dynamic environments, a feasible rolling speed obstacle method is proposed in this study. This method combines the rolling window method with the speed obstacle method and designs a suitable three-dimensional model predictive controller based on the rolling window method under a hybrid obstacle avoidance structure. Stable tracking of the reference path is achieved by optimizing the objective function. A three-dimensional collision cone and speed obstacle cone model is constructed while the window is rolling. If the collision avoidance condition is met, the critical collision point is calculated, and the AUV is guided to avoid obstacles safely by tracking the critical collision point. If collision avoidance ends, the AUV trajectory recovery is guided. Simulation results show that this method can effectively improve the dynamic obstacle avoidance ability of AUVs in real-time path planning.

autonomous undersea vehicle(AUV); path planning; dynamic obstacle avoidance; rolling window method; speed obstacle method

TJ630.33; U675.9

A

2096-3920(2021)01-0030-09

10.11993/j.issn.2096-3920.2021.01.005

章飞, 胡春磊. 基于滚动速度障碍法的AUV动态避障路径规划[J]. 水下无人系统学报, 2021, 29(1): 30-38.

2020-01-09;

2020-04-29.

国家自然科学基金项目(11574120); 江苏省高校高技术协同创新项目(1634871601-18); 江苏省高校自然科学基金(14KJD510002).

章 飞(1979-), 男, 博士, 副教授, 主要研究方向为机器人导航与控制, 传感器网络目标跟踪.

(责任编辑: 杨力军)