基于改進人工勢場法的無人船動態路徑規劃算法研究

陳凱翔,周姝婧,許 強,閔柏成

(中國船舶集團有限公司第七二三研究所,江蘇 揚州 225101)

0 引 言

無人船是一種可在復雜水域環境中,利用感知裝置進行探測,并完成既定任務的小型水面運動平臺。相較于傳統船舶,無人船擺脫了對船員歷史經驗教訓的依賴,綜合了環境感知、信息融合、人工智能、規劃決策和自動控制等技術,能夠智能化完成特殊任務需求,使船舶更為經濟、環保、安全和便捷。無人船航行路徑規劃是保障無人船自主、安全航行的關鍵技術之一,如何及時、有效地避碰更是其中的重點和難點[1]。

路徑規劃是綜合考慮無人船機動性能及水面環境約束等因素而設計出的一條從起點快速抵達目標位置的最優路徑[2]。目前比較常見的有柵格法、A*算法、蟻群算法、Voronoi圖法、神經網絡算法和人工勢場法等。其中柵格法和A*算法是通過啟發式信息估算下一步行進的最優節點位置,常用于全局路徑規劃,它一般需要大量的環境數據和先驗知識作為支撐,計算繁雜,并行性差,實時性弱,與環境粒度的大小密切相關[3]。蟻群算法具有很好的算法收斂和并行分布式,可做到局部最優,但所規劃出的路徑卻并不平滑,且隨著地圖范圍的擴大,所需計算的工作量和時間也將急劇增加[4]。Voronoi圖法容易構造實現,時效性較強,計算量較小,具有很好的環境兼容性,但特征元素較難選取,所規劃出的路徑不夠平滑[5]。神經網絡算法具有很好的分類準確度,可并行分布處理,具有較強的魯棒性和容錯能力,但它通常需要大量的樣本環境數據進行學習,高昂的計算成本和更長的開發時間也很難令人滿意[6]。人工勢場法的數學機理簡單,計算量小,硬件要求低,運算迅速,且滿足實時控制需求,規劃路徑較為平滑,但它往往容易陷入局部極小值點而造成局部振蕩和目標不可達現象,導致算法停滯[7-8]。

本文便是基于人工勢場法(APF)的思想,引入了碰撞危險度、虛擬引力和偏航引力,提出了一種動態路徑規劃算法,使無人船在船舶動力學約束條件下能夠快速跳出局部振蕩和目標不可達的怪圈并抵達目標終點。仿真結果表明,該方法在不同環境下都可以規劃出一條安全、可靠、平滑且經濟的路徑。

1 傳統的人工勢場法

眾所周知,在磁場中,2個同性物體之間存在著排斥力,該力會阻止兩者的靠近,被稱為斥力;2個異性物體之間存在著吸引力,該力會不斷促使兩者相互靠近,被稱為引力。而人工勢場法便是借鑒了這一思想,將無人船所處的環境抽象成一個勢力場,障礙物看成同性物體,目標終點看作異性物體,那么無人船在航線上所受到的作用力就可以認為是引力和斥力的合力疊加,其下一時刻的行駛方向便由合力方向所決定。

如圖1所示,將無人船看做一個質點,在某一時刻其行駛空間可視為一個二維區域。假設此時無人船的位置為P(X,Y),目標終點的位置為G(XG,YG),第i個障礙物的位置為Oi(Xi,Yi),那么無人船所處的任意一點的勢場環境就可以當成是由目標終點和障礙物所形成的合勢場Ut(P),表示為:

Ut(P)=Ua(P)+Ur(P)

(1)

式中:Ua(P)表示引力勢場;Ur(P)表示斥力勢場。

無人船在P位置所受到的合力為:

F(P)=Fa(P)+Fr(P)

(2)

式中:Fa(P)和Fr(P)分別表示無人船所受到的引力與斥力作用,無人船在合力F(P)的作用下前進。

1.1 引力勢場

當無人船處于目標終點作用的引力勢場中時,所受到的引力方向是朝向目標終點的。根據引力勢能的原理可知,目標終點對無人船的引力勢能Ua(P)會隨著兩者間的距離減小而減弱,受到的引力作用Fa(P)也隨之變小,反之則變大。在抵達目標終點時,引力也變為零。因此可由勢能與距離的關系定義無人船所受到的引力勢能為:

(3)

式中:ka為引力增益系數;dG為目標終點和無人船之間的相對距離。

Fa(P)=-▽Ua(P)=kadG

(4)

式中:dG為目標終點和無人船之間的相對距離,其值可根據兩者相對位置關系獲得:

(5)

1.2 斥力勢場

在二維行駛區域內,為避免和無人船發生碰撞,障礙物會對無人船產生斥力影響,距離決定了該斥力的影響程度。當無人船進入到斥力勢場影響區域內時,隨著與障礙物之間的距離越來越短,所受到的斥力將會逐漸增加,反之減小。而當無人船離開斥力勢場影響區域后,受到的斥力作用為零。由此可定義無人船所受到的第i個障礙物的斥力勢能為:

(6)

式中:kr為斥力增益系數;doi表示第i個障礙物與無人船之間的相對距離;Di表示第i個障礙物的斥力勢場影響半徑,其通常由無人船最小安全制動距離Dmin來決定。

根據勢能Ep和力F的關系,第i個障礙物的斥力表示為:

Fri(P)=-▽Uri(P)=

(7)

則無人船在斥力勢場中受到的合力表示成:

(8)

1.3 局部極小值問題

采用人工勢場法形成的路徑,能夠高效、準確、實時地規避障礙物,且路徑平滑安全,應用優勢十分明顯。但同樣地,它也有自身的缺陷之處,即局部極小值問題[9]。

由于無人船行駛位置發生變化時,其勢場大小和行駛方向也會隨之變化,因此,如圖2所示,在某些位置上時,無人船受到的引力Fa(P)和斥力Fr(P)相等或近似的時候,F(P)≈0,無人船到達零勢能點。在零勢能點上,無人船將會停止不動或在該位置附近來回徘徊,形成局部振蕩或者目標不可達現象,從而導致算法停滯,無法繼續向目標終點前進。

圖2 局部極小值點受力情況

2 基于改進人工勢場法的動態路徑規劃算法

動態路徑規劃的目的是引導無人船快速有效地避開障礙物,從而尋求一條經濟適用的航行路線。在實際航行過程中,需要從碰撞危險程度和船舶動力學約束的角度加以分析,從而對傳統的人工勢場法加以改進,使其快速形成一條安全、平滑、機動耗能少的動態路徑。

2.1 障礙物的危險度判定

在無人船實際航行過程中,遇到的所有障礙物是否需要規避,可根據碰撞危險度指標來對環境感知設備傳送的障礙物信息進行評估。根據衡量指標的不同,碰撞危險度可分為空間碰撞危險度(ISCR)和時間碰撞危險度(ITCR)[10]。其中,ISCP描述了無人船和障礙物在保持當前運動狀態的情況下空間上發生碰撞的可能性;ITCP描述了在發生空間碰撞的前提下,從當前時刻到形成空間碰撞危險的緊迫程度。因此,可定義碰撞復雜度表達式為:

(9)

(10)

ICR=ISCR·ITCR

(11)

式中:(x,y)t表示在t時刻障礙物的位置坐標;Dt表示無人船與障礙物之間的安全航行閥值,通常由無人船在平面投影上的最長邊來確定;Tmin由最小安全制動距離Dmin與無人船當前航速V所確定;TCPA表示最小會遇時間,由無人船和障礙物之間的相對距離與相對速度計算所得;TG表示無人船按當前航速行駛到目標終點所需的時間。

因此,為了區分障礙物對無人船的影響程度,可在原有的斥力函數中引入碰撞危險度因子:

(12)

2.2 改進人工勢場法模型

在傳統的人工勢場法中,由于勢場系數的影響,在局部位置上達到受力平衡,會導致局部極小值現象的產生。如果僅僅是通過調整引力增益系數或斥力增益系數來調節,也只是將局部極小值點偏移得離目標終點近一些或遠一些,并不能從根本上打破這種受力平衡。因此,本文提出虛擬引力Fv(P)作為一個新的外力打破這種受力平衡,促使無人船偏航,從而引導無人船盡快駛離障礙物區域。

如圖3所示,虛擬引力Fv(P)的受力方向與斥力Fr(P)方向保持垂直,且偏向于引力Fa(P)的一側,即滿足以下條件:

圖3 虛擬引力的引入

(13)

式中:f表示虛擬引力增益系數,為使無人船能盡快轉向,f應當盡可能使Fv(P)>Fr(P),因此可表示為:

(14)

這樣可以保證,在障礙物斥力影響區域范圍外,即Fv(P)=Fr(P)=0時,僅引力Fa(P)發生作用,促使無人船快速向目標終點靠近;而在障礙物斥力影響區域范圍內,由于存在Fv(P)與Fr(P)垂直,推動無人船遠離障礙目標。但當無人船避讓完障礙物后,此時的Fv(P)和Fr(P)再次變為0,僅引力發生作用,此時無人船會直接向目標終點行進。無人船此時的行進路線為一條全新的航線,而不是原計劃航線(在不考慮障礙物影響情況下原先規劃的航線)。因此,需要在原有的引力函數中增加原計劃航線對無人船的吸引和約束,使得無人船偏離原計劃航線后仍然能不斷地向其靠攏。

如圖4所示,當無人船與原計劃航線間的垂直距離超過偏航距離閥值Dp時,無人船開始受到偏航引力Fp的約束,促使無人船向原計劃航線靠攏;而未超過偏航距離閥值Dp時,偏航引力Fp未產生作用,無人船依舊在Fa(P)、Fv(P)和Fr(P)的聯合作用力下,遠離障礙物。由此形成改進后的引力函數為:

圖4 改進后的受力示意圖

(15)

式中:kp表示偏航引力增益系數;DL表示無人船到原計劃航線的垂直距離。

因此無人船所受到的聯合作用力為:

F(P)=Fa(P)+Fr(P)+Fv(P)+Fp(P)

(16)

2.3 動態路徑規劃算法

為了使規劃出的動態避障路徑更符合無人船的操縱特性,需要考慮船舶動力學的約束,即無人船在進行轉向時是一條光滑的弧線或曲線,而不是尖銳的拐角,因此需要通過無人船最大偏轉角度差Δθmax對動態路徑規劃算法進行約束和限制[11-12]。根據弧度公式Δl=Rmin·Δθmax和距離公式Δs=V·Δt可知,當Rmin=Dt=50 m,V=10 m/s時,無人船在1個轉舵周期(設定為0.2 s)內的最大偏轉角度差Δθmax為0.04°,即連續2個偏轉角度之間的變化率不應該超過0.04°。

由此結合傳統的比例、積分、微分(PID)+MMG船舶運動控制模型對無人船進行控制,可設計形成的基于改進人工勢場法的無人船動態路徑規劃算法如下:

(1) 根據環境感知設備(即探測雷達、激光跟蹤監視設備或超聲探測設備等)實時傳送的環境信息,判斷有無障礙物。若存在障礙物,則執行步驟(2);否則執行步驟(6)。

(2) 由式(9)、(10)與(11),計算出無人船周邊最小安全制動區域Dmin范圍內的所有障礙物的碰撞危險度ICR。

(3) 根據無人船當前的航向θ、航速V以及與障礙物之間的相對距離L、無人船安全航行閥值Dt,判斷是否需要緊急停船。如需要緊急停船,則關閉無人船發動機;否則執行步驟(5)。

(4) 通過式(8)與(12),得到所有障礙物對無人船的總斥力之和Fr(P)。

(5) 通過式(13)與(14),獲得虛擬引力Fv(P)。

(6) 根據式(15),計算無人船受到的目標引力Fa(P)。

(7) 根據無人船當前的位置信息,對船只的偏航程度進行判斷,并求得偏航引力Fp(P)。

(8) 通過式(16),確定無人船在勢場中所受合力方向。

(9) 根據無人船當前的航速V,判斷是否需要重新啟航。如需要重新啟航,則開啟無人船發動機,并執行步驟(10);否則直接執行步驟(10)。

(10) 由當前航速與限定航速間的差值,進行速度管理,調整航速V。

(11) 根據無人船當前的航向θ和舵角φ、合力F(P)的方向、最大偏轉角度差Δθmax與PID船舶控制模型,給出無人船的操舵舵角。

(12) 根據舵角和MMG船舶運動模型,保持無人船繼續航行。

算法流程圖如圖5所示。

圖5 無人船動態路徑規劃算法

3 仿真實驗結果與分析

為了驗證改進后的動態路徑規劃算法的有效性,本文采用MFC對算法進行了仿真,通過模擬多個目標的復雜環境,求解出一條經濟有效的可行路徑。

如圖6(a)所示,設定無人船的起點為(0,0),目標終點為(0,2 000 m),航行速度為10 m/s,生成理論航跡如圖中虛線所示。通過障礙圓模擬在航行過程中遭遇的障礙目標,分別設置圓心為A(-50 m,1 000 m)、B(50 m,500 m)和C(0,900 m),障礙圓的半徑為40 m,其中C為動目標,運動速度為5 m/s,航向為180°。安全航行閥值Dt=50 m,安全制動區域范圍Dmin=300 m,偏航距離閥值Dp=50 m,最大偏轉角度差Δθmax=0.04°,實際航行軌跡如圖6實線所示。

圖6 算法仿真效果圖

在航行過程中,障礙目標在無人船最小安全制動區域范圍Dmin之外時,船只保持原航速和航向航行。當遇到障礙目標C,對其進行碰撞危險度判斷,并執行避讓動作(向右偏航),如圖6(b)和6(c)所示。動目標C避讓完成后,此時在無人船視距范圍內僅有障礙目標A和B存在,判斷B可能存在碰撞危險,因此繼續對目標B執行避讓動作(向右偏航,此時偏航代價最小),如圖6(d)和6(e)所示。所有障礙目標均避讓完成后,無人船在偏航引力和目標引力的作用下,向原計劃航線逐漸靠攏,直至回歸原航線,如圖6(f)所示。

4 結束語

本文對人工勢場法的原理與不足進行了分析,并通過引入虛擬引力、偏航引力和碰撞危險度等方法,對引力函數和斥力函數進行了改進,可以解決局部極小值問題,使動態路徑規劃算法能夠在復雜障礙環境中求得最優解。MFC仿真結果表明,改進后的動態路徑規劃算法能夠有效規避在航行過程中遇到的障礙目標,到達目標終點。本文采用理想化航行區域,而在實際工程應用中應當考慮狹窄水域這一特殊限制條件,因此可將狹窄水域的兩岸抽象成靜態障礙物,在實際的動態路徑規劃算法中進行考慮,增強其自主化和安全性。