基于滾動速度障礙法的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-), 男, 博士, 副教授, 主要研究方向為機器人導航與控制, 傳感器網絡目標跟蹤.

(責任編輯: 楊力軍)