大型自主水下機器人全局靜態與局部動態融合路徑規劃方法

梁世勛，丁寧寧，劉 健

(1.中國科學院沈陽自動化研究所機器人學國家重點實驗室，遼寧 沈陽 110016；2.中國科學院機器人與智能制造創新研究院，遼寧 沈陽 110169；3.中國科學院大學，北京 100049)

0 引 言

深入探索海洋需要越來越多的水下設備部署。當前以蓄電池為動力源的中小型自主水下機器人，續航能力普遍在10～40 h 之間。大型自主水下機器人可攜帶小型水下設備進行部署、回收；建立水下移動基站，為小型機器人補充能源；能夠同時承擔多種復合任務，水下設備的作業時間與作業范圍能夠大大增加。未來大規模水下機器人聯網、協同對于高續航的大型自主水下機器人的安全性與自主性要求越來越高。機器人尺寸、重量的增加，其操縱性、機動性必然隨之降低，靈活度勢必減弱，這些問題使得大型機器人的環境模型更加復雜、機動性約束更加嚴格、路徑選擇更加困難，因此需要求解能力更強的全局靜態與局部動態路徑規劃算法。

粒子群優化（PSO）優化算法具有算法簡單、收斂迅速等優點，但各粒子間缺少信息交流，“搜索”能力與“收斂”能力難以完美平衡，局部最優解的“陷阱”難以完全避開。在利用PSO 算法規劃全局路徑時，大量文獻在粒子群算法中，通過借鑒其他算法進行融合改進，并提高了粒子群算法的性能，取得良好效果。在全局靜態路徑規劃研究中，文獻[1]結合隨機采樣與均勻變異的方法來更新粒子，在高機動救援機器人的路徑規劃中生成了高質量的最優路徑，但在保證收斂的前提下，其搜索能力仍需提高以保證機器人獲取更大的安全活動空間；文獻[2]提出了環境選擇與匹配選擇策略，在迭代過程中逐漸減少隨機性，收斂速度大大提高，但其落入局部最優的可能性同樣增大；文獻[3]通過引入模擬退火算法，利用退火算法暫時接受少許劣質解的特性,優化全局搜索能力，提高局部搜索精度，但算法參數量的增加使得計算量、復雜性也同樣增加；文獻[4]通過差分進化過程，借鑒遺傳算法的交叉變異過程，于粒子群算法中生成新的粒子，增加粒子群的信息交互能力，但其過多的變異增加了不必要的計算量；文獻[5]在PSO 算法中引入同性因子，結合魚群算法的“跳躍”過程提升了算法的求解能力，較好平衡了搜索與收斂能力，滿足AUV 在復雜海域航行時的全局路徑規劃需求，但其過于依賴先驗環境，對于實時環境信息融合海圖后的動態環境模型缺少驗證；在動態規劃研究中，文獻[6]提出基于改進的雙向快速擴展隨機搜索樹（RRT）算法，優化搜索樹之間的連接方式，并設計新的動態步長策略，能夠較優規劃局部避障路徑，但其算法隨機性強沒有得到改善，路徑的平滑連接依舊困難；文獻[7]通過基于全局路徑規劃的引導，通過障礙物運動預測與滾動優化方式進行動態避障，但其對規劃結果過于依賴障礙運動預測方式，且存在規劃路徑遠離既定路線的可能；文獻[8]改進了速度障礙法并用于動態避障,通過碰撞與避障的時間判定避障動作的始末時間,但其對速度條件過分依賴，如若障礙物速度過慢，可能發生碰撞危險。

基于上述問題，考慮實際海域環境，構建海域環境模型、目標函數以及優化約束條件，通過目標函數加權方式，將多目標函數轉化為單目標函數，并通過調整加權權重突出機器人對各目標函數的重視程度；提高算法求解和收斂能力對加權后目標函數優化，得到最優或次優解，從而完成全局靜態與局部動態相結合的路徑規劃任務。

1 基于改進PSO 算法的全局靜態規劃

1.1 障礙物處理與極坐標建立

真實海洋環境障礙大小形狀各異，為縮短全局規劃時間而簡化障礙模型。模型簡化前后滿足以下三點：障礙物“阻擋”效果不發生改變；盡量減少可行路徑損失以及方便計算，提高計算效率。分析海岸、船只、浮標、游魚、暗礁以及漂浮物等動、靜態障礙，選取矩形或圓形的障礙物包圍方式。對于長寬比小于1.5 的障礙物采用圓形包圍；長寬比大于1.5 的障礙物采用有向包圍盒（OBB，即具有方向性的矩形）包圍。考慮大型水下機器人航行狀態調整緩慢，操縱性與機動性較弱，障礙模型外側通過增加圓形直徑或增加矩形邊長的方式，膨脹一個安全閾值ε，模型與安全閾值范圍內區域皆設為危險區域。安全閾值的取值與機器人尺寸正相關，與機動性能負相關。

以起始點為極點，起始點與終止點連線為極軸建立極坐標系，順時針方向為負，逆時針方向為正。以極點為圓心，圓形障礙物的極徑為半徑建立路徑同心圓。規劃的路徑用路徑集合點P=P0,P1,P2,···,Pi,Pi+1表示。P0,Pi+1分 別代表起始點和終止點，i為路徑節點的數量，與路徑同心圓數量相同。路徑節點位于路徑同心圓上，路徑節點信息用極徑Pm和極角Pa表示。

矩形障礙物位置由4 個頂點確定，全部頂點建立路徑同心圓會增加計算量，降低計算效率，選擇2 個頂點建立路徑同心圓既能較好反應障礙信息，又能減少計算量。根據矩形頂點所在位置的極徑長短有2 種選取方式：極徑最長和最短的2 個頂點；極徑次長和次短的2 點。

如圖1 所示，選取極徑最長和最短的頂點會出現路徑節點之間有障礙物“突出”現象。靠近障礙物調整航向角是對機器人回轉性能的巨大挑戰，在嚴格的機動性約束下大型水下機器人不具備此種回轉條件的能力。因此，采取圖2 所示的方式建立路徑同心圓。

圖1 極徑最長和最短頂點建立路徑同心圓Fig.1 Path concentric circle established by the longest and shortest vertices of polar radius

圖2 極徑次長和次短頂點建立路徑同心圓Fig.2 Path concentric circle established by the second longest and the second shortest vertices of polar radius

由起始點、障礙物信息建立極坐標，采用弧形虛線表示路徑同心圓，同心圓圓心為起始點；圓形、矩形實線表示障礙物；圓形、矩形虛線代表障礙物膨脹安全閾值 ε后的邊界。極坐標系下全局靜態環境信息如圖3 所示，其中path1，path2 表示路徑同心圓上存在的路徑節點，在路徑節點集合中將以P1，P2表示。

圖3 極坐標系下的障礙物模型Fig.3 Obstacle model in the polar coordinate system

1.2 適應度函數

在自主水下機器人航行過程中，在線獲取環境信息有限，由先驗知識規劃的路徑信息十分重要。通常為得到全局最優路徑，機器人會根據障礙物模型構建目標函數，將規劃問題轉化為多目標函數優化問題[9]。在全局路徑規劃中，需要在工作空間中搜索出一條從起始點到終止點的最優路徑。最優路徑不僅滿足機器人自身速度、加速度以及角速度約束，同時還在滿足安全條件下保證路徑最短。執行層中的避障問題可以視為機器人在可行區域內的優化搜索，通過多目標函數優化求解找到可行區域的解。

建立路徑長度函數J1計算機器人從起始點Start 到終止點End 的路徑長度，如下式：

其中，L（.，.）為兩路徑點的歐式距離；

建立路徑可行度函數J2，表示航行中路徑可行程度，路徑可行度函數J2如下式：

為防止出現路徑點位于障礙內部或閾值區域內的情況，設立危險度函數D(i)，如下式：

式中：D(i)表示第i-1 個路徑點與第i個路徑點之間的危險度；OB表示危險區合集。若路徑與危險區相交，則危險度為無窮大；若不相交，則危險度為0。

建立回轉可行度函數J3，表示機器人航行過程中的回轉角度可行性，如下式：

建立回轉評估函數W(i)如下式：

其中：ω(i)代 表機器人于第i個路徑點處回轉角度；ωl(i)為機器人最大回轉角度。由于自主水下機器人無法迅速調整首向，因此用J3約束機器人的回轉角度。避免出現回轉角度過大，機動性、操縱性不能滿足的情況。

粒子群算法通過適應度函數來判別粒子優劣，是逼近最優解的決定性因素，適應度函數的選擇將直接影響算法性能。在靜態障礙環境中，尋求與障礙物不相交、路徑長度最短且滿足最大回轉角限制的可行路徑。

采用線性加權方法將J1，J2，J3轉化為單目標函數，建立適應度函數如下式：

若路徑符合航行條件，則適應度為路徑總和；若路徑不符合航行條件，則適應度為無窮大。為比較粒子優越性，制定如下粒子擇優條件：

1）如Fit(i)＜Fit(j)，那么粒子i優于粒子j；

2）如Fit(i)=Fit(j),ω(i)＜ ω(j)，則粒子i優于粒子j。

1.3 改進粒子群算法

粒子群算法以其簡單、參數少、快速收斂等特性被廣泛應用在機器人各領域[10]。PSO 算法是一種隨機搜索算法，該算法設計無質量的粒子群來模擬鳥群，鳥群中的粒子僅有2 個屬性：速度 V和 位置 X，其基本更新公式[11]如下式：

學習因子c1，c2，慣性權重w等參數的選取和控制在一定程度上決定PSO 算法的性能。學習因子是粒子具備自我經驗總結和學習能力的體現，c1保持較大，粒子搜索范圍大但收斂慢；c2保持較大，能夠學習群體經驗使收斂迅速但搜索范圍小[12]。算法需要在前期大范圍搜索，后期快速收斂，因此需要在搜索前期c1較大，搜索后期c2較大，c1，c2由下式表示：

其中，run表 示當前迭代次數；runmax表示最大迭代次數；r1，r2為在0 和1 之間均勻分布的隨機數。

慣性權重w影響全局與局部尋優能力，合適的w取值能夠平衡全局與局部搜索能力，以實現算法初期搜索能力強，后期收斂快，以較少的迭代次數得到最優解。因此采用線型遞減慣性權重如下式：

其中：wmax為最大慣性權重；wmin為最小慣性權重；run是當前迭代次數；runmax為迭代最大次數。

粒子最終收斂位置由群體最佳位置和個體歷史最佳位置決定[13]。本文定義：如果粒子群經過若干代更新后，所有粒子都未發現比歷史最優更優的值，則算法處于輕度收斂狀態。引入遺傳算法“變異”過程使得最佳位置發生“變異”。由于粒子群陷入局部“陷阱”是逐漸形成的，因此需要結合求解階段對群體最佳位置進行變動，即采取不同的變異方式引導種群向有利方向繼續搜索。

定義粒子k到當代最佳位置的維度距離：

其中：d為算法維度，在全局靜態規劃中為路徑節點i；維度距離可將粒子聚集度反映到一維區間比較。如果算法適應度值較理想，且粒子聚集度較高，說明在收斂過程中尚未發現更優位置，此時變異應在小范圍內進行，以免重復搜索過程；如若適應度不高，或者粒子聚集度較低，說明尚在搜索過程，且對空間搜索不充分，因此變異在大范圍內進行，提高全局搜索能力，避免陷入局部極值。當算法處于輕度收斂狀態時，變異后的最佳位置由原來最佳位置與變異范圍共同決定，且變異范圍根據收斂原因不斷調整。

定義 γ (t)為最佳位置變異范圍；

式中：rand() 表 示均勻分布在 (0，1)區間上的隨機數；gb(t) 表示原來最佳位置。通過最佳適應度Fitbest來控制變異范圍的大小；最佳位置越接近理想值，適應度值越小，位置變異程度也越小，反之則越大。

1.4 改進粒子群算法流程

全局路徑規劃的流程圖如圖 4 所示。分為如下步驟：

圖4 靜態環境改進PSO 算法流程圖Fig.4 Flowchart of improved PSO algorithm in static obstacle environment

1）初始化種群，包括設置粒子的個數、維度，各個粒子的位置、速度，最大迭代次數runmax，初始化慣性權重w、學習因子c1，c2。

2）計算粒子適應度。

3）由粒子擇優規則找出個體最優解和群體最優解。

4）更新學習因子與慣性權重，更新粒子的速度和位置。

5）經過指定代數更新后，如若粒子仍未產生新的群體最優解，則根據式（11）判斷是否處于輕度收斂狀態。若處于輕度收斂狀態，則根據式（12）對最優解進行變異并轉入第2 步；否則轉入第6 步。

6）判斷迭代次數是否達到runmax，滿足時候結束算法；否則，轉第 2 步。

2 基于速度障礙法的局部動態規劃

2.1 速度障礙法

機器人航行過程中，遭遇游魚、漂浮物以及船舶等動態障礙時，可以改變首向繞過障礙，也可調整航速躲避障礙。當機器人遭遇動態障礙阻擋時，如需調整航速為Vnew，則要求Vnew盡可能接近最佳航速。

在局部動態避障的規劃中，構建障礙物模型進行避障規劃時，由導航精度帶來的障礙物位置偏差會導致避障效果不理想，因此選建障礙物相對機器人的速度坐標系。利用機器人搭載的聲吶系統采集數據構建障礙物相對機器人的地圖系統，其中包括障礙物相對機器人方位Sobψ和距離Sob。以機器人聲吶位置為原點(0,0)建立笛卡爾坐標系Os，機器人體坐標系 (Xa，Ya)，定義機器人航向為Y軸，水平處置于航向為X軸。假定在時間 Δt內，機器人的首向角 ψr，航速 υr為固定值。機器人航行過程中，笛卡爾坐標系Os隨機器人位姿更新。當機器人以固定首向角 ψr以及航速 υr航行時，坐標系原點在X軸、Y軸的移動量表示為 (Δx，Δy)：

假定在時間 Δt內，障礙物的運動方向 ψo、航速υo為固定值。聲吶測得障礙物位置相對于當前坐標系位置 (x1，y1)，障礙物新測得的位置 (x2，y2)。則上一時刻障礙物相對當前坐標系的位置為：

障礙物運動速度為：

障礙物運動方向為：

根據聲吶圖像的障礙物輪廓，對障礙物長寬比判別后進行包圍處理。如圖5 所示，機器人A與障礙物B均按照固定航向與航速運動。

圖5 機器人與障礙物運動情況Fig.5 The motion of vehicle and obstacle

將機器人A視為質點，并結合機器人尺寸與安全閾值對障礙物B進行膨脹處理。計算A相對B的相對速度VBA=VA-VB，定義從一點P出發沿V方向的射線為：

當從A點發出沿VBA方向的射線 λBA與障礙物B相交，即射線 λBA落在障礙物B相對于機器人A的2 條切線夾角內時，二者將會發生碰撞，如圖6 所示。

圖6 碰撞條件判定Fig.6 Determination of collision condition

由速度障礙法，定義碰撞范圍為：

如不改變機器人A的航行狀態，機器人將在某一時刻t1與障礙物發生碰撞，為逃離碰撞范圍，需要調整位置于安全區域。由碰撞范圍可得：的補集即是安全航行區域[15]。

2.2 適應度函數

建立機器人-障礙物相對速度坐標系如圖7 所示。

圖7 機器人-障礙物相對速度坐標系Fig.7 Relative velocity coordinate system of vehicle-obstacle

設全局坐標系(X，Y)，機器人以速度VA、航向角α運動。機器人體坐標系(Xa，Ya)。O處為障礙物，(X，Y)坐標系中速度為VO，方向角 βO。LMO，LNO為機器人障礙物兩側切線；LO是以機器人為起點，到障礙物邊緣任意點CO的 有向線段。θO是LO與 X 軸正向夾角，γAO為VAO與LO夾角。定義避碰角為機器人躲避障礙物，VAO的調整角度。Δγaolow，Δγaoup分別為VAO旋轉到LMO，LNO的角度，設逆時針為正向。

機器人在時間區間 [tk，tk+Δt] 內 躲避某障礙物Oi，則在該段時間內VAOi應該偏離碰撞范圍，即如下不等式之一成立：

假設 [tk，tk+Δt]區間內障礙物速度恒定，則

其中，ΔθOi=VAO=sinγAOi/LOi，文獻[16]對上式有詳盡推導證明。由式（2 2）可以看出避碰角由(ΔVA,VAΔα)確定。由此可知調整機器人的航速與航向是避障的2 種有效行為，且僅僅采取其中一種基本就能完成避障要求。針對子目標段的運動障礙物，粒子群維度數是固定的二維：機器人的航速和航向，且其優化調整變量即為這2 個值。

建立機器人-目標區域相對速度坐標系如圖8 所示，定義與機器人-障礙物相對速度坐標系相同。

圖8 機器人-目標點相對速度坐標系Fig.8 Relative velocity coordinate system of vehicle-target

假設機器人子路徑終點位于可行區域G內，CG為終止點，則期望VAG始 終指向CG，避碰角盡可能接近VAG與LG之間的夾角，保證目標函數式（23）極小：

為了減少偏航時間，使機器人盡快回歸正常航線，建立最小時間函數。具體表示為在避障前航線方向上，當機器人航向分量在此方向速度最大時，能使偏航時間最短，則

利用線性加權方法分別將目標函數JG1，JG2歸一化，轉化為單目標函數，得到的單目標函數如下式：

其中，?1，?2分 別為JG1，JG2的權重。考慮到動態避障過程中，對回轉角度要求更高，對航速變化要求較小，在選擇參數的值時使 ?1＞ ?2，且?1+?2=1。

動態障礙環境下路徑規劃于路徑節點間進行規劃，由當前路徑點向下一個路徑點航行過程中進行避障操作，其算法具體流程如圖9 所示。

圖9 動態環境改進PSO 算法流程圖Fig.9 Flowchart of improved PSO algorithm in dynamic obstacle environment

3 實驗結果

根據建立的環境模型與適應度函數，分別采用遺傳算法，傳統粒子群算法和改進粒子群算法對機器人全局靜態路徑規劃進行仿真實驗。實驗結果對比表明，本文的改進粒子群算法在全局靜態規劃中有更好的路徑規劃結果。最后，驗證局部動態環境下路徑規劃效果。

為滿足算法的穩定性并保證收斂，選取線性遞減慣性權重，其中wmax=0.85，wmin=0.15；學習因子c2=c2=2 ；粒子數100；最大迭代次數runmax=100。利用傳統 PSO 算法、遺傳算法和改進 PSO 算法各實驗20 次，全局靜態規劃結果如表1 所示，圖10 為實驗結果，圖11 為目標函數值的迭代曲線圖。

表1 實驗結果Tab.1 The result of experiment

圖10 同側路徑規劃結果Fig.10 The result of path planing in the same side

圖11 目標函數值的迭代曲線Fig.11 Iterative curve of objective function value

分析表1，由20 次實驗結果所得路徑長度平均值升序排序，分別為改進粒子群算法、遺傳算法和粒子群算法。如圖10 所示，同側實驗結果比較表明，3 種方法都能夠完成全局靜態路徑規劃任務，且機器人航行全程無危險；與遺傳算法、傳統粒子群算法相比，改進粒子群算法的規劃效果更好，規劃路徑長度更短。

圖11 為選取某次實驗的函數值迭代曲線。結果表明，改進粒子群算法的收斂效果和求解能力優于傳統 PSO算法和遺傳算法，且算法的跳出局部最優解的能力優于二者。改進后的粒子群算法在前期搜索效果弱于傳統粒子群算法和蟻群算法的情況下，能夠利用粒子的“變異”加快搜索速度，快速逼近最優解。在后期收斂中，由于“變異”粒子的引入，能夠避免陷入局部最優解。由以上分析可知，改進的粒子群算法在做機器人全局靜態路徑規劃時，具有較強的搜索和收斂能力。

同時，本文驗證了局部動態路徑規劃能力。圖12為全局靜態最優規劃路徑中，加入局部動態障礙后所得到的最終規劃結果。結果表明，針對不同的局部動態障礙，機器人能夠安全、高效的進行躲避，且避障動作小、路徑損失少，能夠在全局路徑的基礎上規劃出最優避障路徑。

圖12 動態環境下規劃結果Fig.12 The result of path planing in dynamic obstacle environment

4 結 語

本文依據大型自主水下機器人路徑規劃需求，在嚴格的機動性約束條件下構建了路徑長度、路徑可行度和回轉可行度函數；通過引入最優位置變異過程增強了算法在全局靜態路徑規劃過程中的求解能力；面對局部動態規劃，引入速度障礙法以獲得碰撞范圍和安全區域。實驗結果表明，全局靜態與局部動態相融合的路徑規劃方法，在全局靜態規劃中路徑更短、求解能力更強，局部動態規劃能夠規劃出最優避障路徑，高效且穩定。