佟盛，李大輝

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

0 引 言

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

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

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

1 理論基礎

1.1 CSE+方法

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

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

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

1.2 PIO 方法

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

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

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

1.3 APF 方法

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

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

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

2 算法設計

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

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

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

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

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

步驟5狀態(tài)轉(zhuǎn)換為 ( Xi,vi)，并作為模型輸入；

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

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

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

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

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

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

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

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

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

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

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

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

3 測試與仿真

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

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

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

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

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

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

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

在PIO 參數(shù)設置為地磁導航次數(shù)90，地標導航次數(shù)15，種群數(shù)30，維數(shù)2 維，地磁因數(shù)0.3，經(jīng)過28次迭代，UUV 完成避障進入穩(wěn)定狀態(tài)，效果如圖5（b）所示。

4 結 語

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

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

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