具有實時避障能力的UUV 編隊技術設計與研究

佟盛，李大輝

(1. 中國船舶集團公司第七一四研究所，北京 100101；2. 齊齊哈爾大學，黑龍江 齊齊哈爾 161000)

0 引 言

目前智能體協同控制成為國內外研究的熱點之一，協同控制技術被應用于許多領域[1-2]，智能體編隊協同控制技術取得了大量科研成果，如通過控制多個機器人以特定方式自動運輸各種貨物；如多個自主運動小車被廣泛應用于軍事邊防編隊巡邏、資源地理勘測、偵察救援、掃雷或衛星的姿態運動協調及空間探測；如在警務領域的多個移動機器人編隊成一個弧形包圍網抓捕入侵者等，形成了以人工勢場法[3-5]、虛擬結構法[6-7]、基于行為控制法[8]、跟隨領航者法為主要代表的多種編隊控制方法。

而在UUV 集群編隊航行控制中，利用人工勢場法實現了集群無碰隊形變換技術，雖然也實現了集群的避障和避碰功能，由于人工勢場法是通過引入與障礙物間的斥力來防止碰撞，容易產生局部最小值而造成無法運動到終點結果[9]，導致避障方法的實時性差問題。同時在障礙物分布稀松的情況下，由于多個候選方向造成最優路徑丟失現象，導致避障方法的自主性差問題。

針對UUV 集群編隊航行的安全性需求，本文在人工勢場法[10]（Artificial Potential Field，APF）完成編隊控制基礎上，引進圓形擴張法（Circle Sector Expansions，CSE+）和鴿子群優化算法（pigeon-inspired optimization，PIO）尋找下一時刻位置和方向，解決實時避障問題。

1 理論基礎

1.1 CSE+方法

CSE+方法的基本思想為進行圓的擴張找到障礙物，如圖1 所示。在由圓形障礙物隨機填充的環境中，找到到達目標位置的可行路徑，首先在圖1（a）中，智能體從起點開始膨脹尋找障礙物，其中細箭頭轉向為圓的膨脹方向。然后在圖1（b）中，沿與障礙物1 的圓心方向膨脹確定障礙物2。再在圖1（c）中，穿過障礙物1 和障礙物2 構成的通道，繼續向前膨脹確定障礙物3。最后在圖1（d）中，根據與目標的方位角選擇向左或向右運動方向，重復這一過程而不斷接近目標位置。

圖 1 CSE+尋徑示意圖Fig. 1Diagram ofCSE+ method

CSE+方法在障礙物分布密集情況下，利用根據Apollonius 相切問題確定相切圓，移動智能體運動到相切圓圓心為下一步位置，但對于障礙物分布稀疏情況時，確定下一步位置效果很差。

1.2 PIO 方法

PIO 方法通過建立地圖和指南針算子以及地標算子，（見圖2），在安全范圍內尋找下一目標位置，而后進行轉向判斷。

圖 2 PIO 尋徑示意圖Fig. 2Diagram ofPIO method

首先利用地圖通過膨脹尋找新的障礙物，然后建立指南針算子以及地標算子，不斷重復這一過程，最后直到智能體運動到達終點。PIO 方法較好地解決了UUV 集群在稀疏環境空間中的實時避障算法設計[9,11]。

1.3 APF 方法

APF 方法通過智能體個體之間引力、斥力求得合力，就是“遠吸近斥”原理控制智能體的位置和方向。

首先對UUV 集群編隊建立Leader-Follower 模型，完成初始化隊形，如圖3（a）和3（b）所示。當UUV 間的距離較遠時，吸引力發揮作用，縮編隊隊形。當UUV 間的距離較近時，排斥力發揮作用，稀疏編隊隊形。在UUV 間的距離合理范疇內時，不受任何力的影響，實現UUV 編隊無碰隊形變換和保持。

圖 3 編隊隊形模型及恢復示意圖Fig. 3The model of cluster keep and resume

2 算法設計

2.1 UUV 集群無碰撞編隊算法設計

步驟1給定當前Learder 及狀態 ( x1,v1)；

步驟2定義拓撲結構，求得受控UUV 集合 Ni；

步驟3受控上UUV 集合 Ni的下一時刻狀態輸出()，得下一時刻Follower 的理想控制輸入()；

步驟4由型式得下一時刻Follower 的的實際狀態(xi,yi,hi,Vi,ψi,λi)；

步驟5狀態轉換為 ( Xi,vi)，并作為模型輸入；

步驟6返回步驟1，直至達到仿真終了條件。

2.2 UUV 集群實時避障算法設計

步驟1從起點出發向前膨脹直到找到障礙物1，如圖1（a）所示。沿障礙物1 與起點方向延長線進行膨脹確定障礙物2，如圖1（b）所示。若構成的通道不可通行，即 d12〈dw，調整膨脹方向重新尋找障礙物2。確定障礙物1 和障礙物2 后執行步驟2。

步驟2從障礙物1 和障礙物2 所構成的通道中穿過進行膨脹尋找障礙物3，如圖1（c）所示。確定由障礙物構成的安全范圍A 的大小。當sA＜st時進行步驟4，否則將進行步驟5。

步驟3經過障礙物1 和障礙物2 所圍成的通道到達與3 個障礙物相切的相切圓的圓心，后執行步驟7。

步驟4初始化鴿群優化算法的參數，如空間維度D、地圖羅盤算子R、種群數量 Np、最大迭代次數NC1max和 NC2max[12]，并確定鴿群優化算法的適應度值計算函數threat-count，隨機初始化鴿群的速度與位置，將執行步驟5。

步驟5以安全范圍重心作為現有最優結果，運算地圖羅盤算子，多次迭代更新（直到迭代次數NC1=NC1max）鴿群的速度V 與路徑X。根據適應度值fitness 的大小確定每只鴿子的最優位置 Xp，以及全局最優位置XR[13-17]。

步驟6根據適應度值淘汰20%的鴿子，再利用地標算子進行迭代更新直到迭代次數NC2=NC2max，調整鴿群的速度V 與路徑X，以當前全局最優X 作為下一位置，并移動到這一位置，而后執行步驟7。

步驟7判斷是否可直接達到目標位置，不可以時進行步驟9。

步驟8記錄當前位置與障礙物信息至搜索樹中。

步驟9進行轉向判斷。若可進行轉向，選擇構成當前通道的2 個障礙物作為新的障礙1 和障礙2，并進行步驟3；若不可轉向，將進行步驟10。

步驟10標記當前通道不可行，機器人掉頭返回上一層搜索樹，選擇另一條通道，若仍不可行，則機器人再一次掉頭并返回搜索樹更上一層再次重新執行步驟10；若可行將進行步驟2。

3 測試與仿真

3.1 UUV 集群無碰撞編隊算法Matlab 仿真實驗

首先完成4 個Follower 和1 個Leader 初始化，即位置pose，速度V，加速度控制量control 三個量，然后設置Follower 相對Leader 的位置，再設置位置偏差線速度調節5、位置偏差角速度調節5、朝向偏差角速度調節0，效果如圖4 所示。

3.2 UUV 集群實時避障算法Matlab 仿真實驗

1）障礙物分布稀疏情況下的APF 方法避障實驗

參數設置為計算引力需要的增益系數1，計算斥力的增益系數2，障礙影響距離100，當障礙和車的距離大于這個距離時，斥力為0，即不受該障礙的影響，障礙個數8，步長5，迭代次數200，UUV 個數為4，效果如圖5（a）所示。

圖 4 無碰撞編隊算法Matlab 仿真實驗Fig. 4Simulation of the contact-free cluster algorithm

2）障礙物分布密集情況下的CSE+方法避障實驗

在PIO 參數設置為地磁導航次數90，地標導航次數15，種群數30，維數2 維，地磁因數0.3，經過28次迭代，UUV 完成避障進入穩定狀態，效果如圖5（b）所示。

4 結 語

本文完成UUV 編隊無碰隊形變換技術和UUV編隊實時自主避障技術的設計與仿真，達到集群編隊的解散和恢復，100%實現原始隊形的無碰撞保持。本算法在ROBO-MAS 即桌面微型移動機器人系統進行測試，系統的測試環境為射頻通信芯片進行組網，最高傳輸速率可達2 Mbps，在小型實驗場地內通過光電投影定位技術，實現機器人位置感知和數據通信，完成本文算法的測試，效果達到預期效果。

本文實時避障算法可以應用在蜂群UUV 作戰中，在水下通信延遲與丟包狀態下，UUV 集群編隊實現無碰保持與變換效果。

圖 5 實時避障算法matlab 仿真實驗Fig. 5Simulation of real-time obstacle avoidance algorithm