梁世勛，丁寧寧，劉 健

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

0 引 言

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

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

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

1 基于改進PSO 算法的全局靜態(tài)規(guī)劃

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

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

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

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

如圖1 所示，選取極徑最長和最短的頂點會出現(xiàn)路徑節(jié)點之間有障礙物“突出”現(xiàn)象?？拷系K物調整航向角是對機器人回轉性能的巨大挑戰(zhàn)，在嚴格的機動性約束下大型水下機器人不具備此種回轉條件的能力。因此，采取圖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

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

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

1.2 適應度函數(shù)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

1.3 改進粒子群算法

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

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

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

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

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

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

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

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

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

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

1.4 改進粒子群算法流程

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

圖4 靜態(tài)環(huán)境改進PSO 算法流程圖Fig.4 Flowchart of improved PSO algorithm in static obstacle environment

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

2）計算粒子適應度。

3）由粒子擇優(yōu)規(guī)則找出個體最優(yōu)解和群體最優(yōu)解。

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

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

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

2 基于速度障礙法的局部動態(tài)規(guī)劃

2.1 速度障礙法

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

在局部動態(tài)避障的規(guī)劃中，構建障礙物模型進行避障規(guī)劃時，由導航精度帶來的障礙物位置偏差會導致避障效果不理想，因此選建障礙物相對機器人的速度坐標系。利用機器人搭載的聲吶系統(tǒng)采集數(shù)據(jù)構建障礙物相對機器人的地圖系統(tǒng)，其中包括障礙物相對機器人方位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)。則上一時刻障礙物相對當前坐標系的位置為：

障礙物運動速度為：

障礙物運動方向為：

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

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

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

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

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

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

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

2.2 適應度函數(shù)

建立機器人-障礙物相對速度坐標系如圖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的角度，設逆時針為正向。

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

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

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

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

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

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

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

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

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

動態(tài)障礙環(huán)境下路徑規(guī)劃于路徑節(jié)點間進行規(guī)劃，由當前路徑點向下一個路徑點航行過程中進行避障操作，其算法具體流程如圖9 所示。

圖9 動態(tài)環(huán)境改進PSO 算法流程圖Fig.9 Flowchart of improved PSO algorithm in dynamic obstacle environment

3 實驗結果

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

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

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

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

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

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

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

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

圖12 動態(tài)環(huán)境下規(guī)劃結果Fig.12 The result of path planing in dynamic obstacle environment

4 結 語

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