文龍貽彬，胡常青,2,3，趙榮利，徐宇新

(1.北京航天控制儀器研究所，北京 100039；2.中國地質(zhì)大學(xué)（北京）工程技術(shù)學(xué)院，北京 100083；3.青島海洋科學(xué)與技術(shù)試點國家實驗室，山東 青島 266237)

0 引 言

無人水面艇（以下簡稱無人艇）目前在國內(nèi)外軍事和民用領(lǐng)域的應(yīng)用日漸增多，而無人艇能夠自主航行的關(guān)鍵則取決于路徑規(guī)劃技術(shù)[1]。

與其他路徑規(guī)劃方法相比，人工勢場法在路徑規(guī)劃和避障領(lǐng)域有著極其廣泛的應(yīng)用，這是因為其理論較為簡單、易于實現(xiàn)從而極具吸引力[2]。除了建立吸引/排斥力模型外，許多其他勢場函數(shù)方法也正在被研究，這些方法能夠非常有效快速地規(guī)劃出路徑，但都存在局部最小值的問題[3]。

這個問題會導(dǎo)致以下幾種情況發(fā)生：

1）存在陷阱區(qū)域；

2）在相近的障礙物群中不能識別路徑；

3）在障礙物前震蕩；

4）在狹窄通道中擺動；

5）障礙物附近目標(biāo)不可達(dá)。

為了克服這些問題，目前國內(nèi)外研究者針對人工勢場法有如下幾種改進(jìn)方式：結(jié)合基于搜索規(guī)劃器的路徑規(guī)劃方法。如采用波前規(guī)劃器來克服局部最小值問題[4-5]。改進(jìn)勢場函數(shù)，定義一個只有一個局部最小值的勢場函數(shù)，從而解決上述問題。如斥力勢場函數(shù)改進(jìn)模型[6]。加入應(yīng)激行為，來擺脫極值點，重新向終點前進(jìn)。如加入一個隨機(jī)擾動[7]。加入中間點[8]。利用幾何方法重新規(guī)劃路徑，擺脫局部極小點[9]。結(jié)合智能算法的路徑規(guī)劃方法，如基于混沌優(yōu)化改進(jìn)的人工勢場法[10]。通過蟻群算法來不斷優(yōu)化人工勢場法獲得的路徑[11]。結(jié)合遺傳算法來改進(jìn)人工勢場法來獲得最優(yōu)路徑[12]。

上述方法可以解決局部最小值點導(dǎo)致的問題，但也存在著規(guī)劃過于復(fù)雜等問題。

2011 年，Hossein Adeli 等提出了一種迭代勢場算法[13，能夠很好地解決移動機(jī)器人的路徑規(guī)劃問題，并依然保持了算法理論的簡潔性和易于實現(xiàn)性。但上述方法存在需要調(diào)參、耗費較大的計算資源等缺點，不適用于局部路徑規(guī)劃。

本文提出一種基于柵格法的稀疏迭代勢場方法。通過稀疏點約束方法和構(gòu)建一種比傳統(tǒng)迭代勢場算法更加簡單的勢場函數(shù)，提高算法運行速度，并解決柵格法離散化所帶來的缺陷（限制了路徑方向變化只能為π/4 的倍數(shù)，會導(dǎo)致無人艇不必要的運動轉(zhuǎn)向），不但能規(guī)劃一條任意角度的最短路徑，還大幅減少了計算時間。此外針對無人艇局部路徑規(guī)劃需要滿足海事規(guī)則的特點，建立了海事規(guī)則和轉(zhuǎn)向避碰模型，最終讓無人艇實現(xiàn)實時、快速、安全地避碰，規(guī)劃出來的路徑符合無人艇的運動特性且能夠通過改變安全距離的方式，實現(xiàn)場景自適應(yīng)功能。

1 傳統(tǒng)迭代勢場算法

1.1 柵格法

利用柵格法對環(huán)境進(jìn)行建模，將工作空間劃分為一系列具有障礙物信息的正方形柵格網(wǎng)絡(luò)單元，如圖1 所示。圖中黑色區(qū)域為可行區(qū)域，白色區(qū)域為不可行區(qū)域。柵格中的每一個單元格就代表一個路徑點。但柵格法的離散化建模方法，限制了無人艇的路徑方向變化只能為π/4 的倍數(shù)。當(dāng)為稠密路徑點時，S0無法直達(dá)P1，需要S0→S1→P1或S0→S8→P1。

1.2 勢場函數(shù)

勢場函數(shù)如下式：

圖 1 柵格法環(huán)境建模和柵格法離散化缺陷Fig.1 Environment modeling with grid method and Discretization defect of grid method

1.3 迭代二分搜索

最優(yōu)路徑是由一系列最優(yōu)點組成，只要得到每一個最優(yōu)路徑點，就可以得到最優(yōu)路徑。

根據(jù)式（1），對可行區(qū)域中每一點計算勢能值，并存入一個集合中。

然后進(jìn)行最優(yōu)點尋找，并不斷進(jìn)行迭代二分搜索，直至生成一條最優(yōu)路徑，如圖2 所示。

圖 2 迭代二分搜索示意圖Fig.2 Iterative binary search diagram

迭代勢場算法可以直接得到一條全局最優(yōu)路徑（此處的最優(yōu)路徑是指考慮到障礙物排斥勢場的最優(yōu)路徑，而非最短路徑），但其存在3 個缺陷。

1）需要計算d(c,obs)

為了得到勢場函數(shù)，需要計算 d(c ,obs)項的大小，即使利用Brushfire 算法來計算，也需要對整張地圖進(jìn)行一次遍歷，當(dāng)?shù)貓D較大時，計算 d(c ,obs)將會消耗一定時間。

2）需要迭代多次

為了得到考慮障礙物的排斥勢場的最優(yōu)平滑路徑，需要迭代足夠多的次數(shù)，但當(dāng)?shù)螖?shù)過多時，就會變成稠密點路徑，將會出現(xiàn)柵格法離散化問題，如圖1 和圖2（d）所示，此外運行時間也會大大增多。

3）需要調(diào)參

該算法可以通過調(diào)整 a ， b的值，改變安全距離的大小。但其缺點是當(dāng)場景或環(huán)境地圖不同時，需要基于經(jīng)驗進(jìn)行調(diào)參才能改變安全距離的大小，且無法得到準(zhǔn)確的安全距離值，無法滿足無人艇局部避碰場景自適應(yīng)的要求。

2 改進(jìn)迭代勢場算法

為了解決上述缺陷，本文提出了一種改進(jìn)迭代勢場算法。

2.1 改進(jìn)勢場函數(shù)

改進(jìn)勢場函數(shù)如下式：

其中：各項含義等同于式（1），但是舍棄了Uobs(c)項，即勢場函數(shù)只考慮起點與終點的吸引力。

這樣的好處是減少了計算 Uobs(c)項所耗費的時間，從而更好地滿足局部路徑規(guī)劃的實時性需求。

另一方面在改進(jìn)勢場函數(shù)的基礎(chǔ)上，通過建立安全模型可以得到一條量化了安全距離的最優(yōu)路徑，這樣就解決了傳統(tǒng)迭代勢場法需要調(diào)參的問題，從而讓無人艇具備自適應(yīng)場景的能力。同時也解決了由于舍棄了 Uobs(c)項，導(dǎo)致路徑離障礙物過近的問題。

基于改進(jìn)勢場函數(shù)進(jìn)行迭代二分搜索得到的點應(yīng)為最優(yōu)點Poptimal，因為這個點只在可行區(qū)域中進(jìn)行尋優(yōu)且確保了路徑的存在。此外，由于改進(jìn)勢場函數(shù)不考慮障礙物的排斥力，當(dāng)障礙物擋住路徑時，為了保證迭代路徑點相互聯(lián)通，此時最優(yōu)點必將出現(xiàn)在障礙物邊緣。定義這些出現(xiàn)在障礙物邊緣的最優(yōu)點Poptimal為關(guān)鍵點Pkey，易知最短路徑一定可以由Poptimal中的某些點聯(lián)通而成。

2.2 稀疏點約束

為了防止出現(xiàn)柵格法離散化問題和提高計算時間，在算法中加入稀疏點約束。

稀疏點約束由聯(lián)通約束與最短路徑約束組成。

首先加入聯(lián)通約束。采用Straight of line 算法作為迭代終止條件，即只需最優(yōu)點間兩兩聯(lián)通即可，如圖3 所示。這樣得到的最優(yōu)點數(shù)量大幅減小，從稠密點減少為重要聯(lián)通點。

圖 3 重要聯(lián)通點Fig.3 Important connecting points

聯(lián)通約束的目的是得到少量的點序列來完成路徑規(guī)劃任務(wù)，其搜索量和時空開銷會減小,收斂速度變快。不但減少了計算時間，而且得到任意角度的路徑。

針對重要聯(lián)通點，再加入最短路徑約束。

式中： d(Pi,Pj)為相鄰重要聯(lián)通點的歐式距離；n 為重要聯(lián)通聯(lián)通點個數(shù)。

采用Floby 算法對下式進(jìn)行求解。

將求解出來的點定義為稀疏點Psparse，從而得到一條由Psparse連接而成的最短路徑，如圖4 所示。

圖 4 稀疏點Fig.4 Sparse points

此項約束不但能夠?qū)⒅匾?lián)通點優(yōu)化為稀疏點，而且可以規(guī)劃出最短路徑。路徑最短也就可以認(rèn)為避開障礙物的時間最少（在符合無人艇運動學(xué)的情況下），從而滿足了局部路徑規(guī)劃的時間需求，能夠?qū)崿F(xiàn)快速避碰。

2.3 安全距離約束

基于稀疏點約束，得到一條由Psparse連接而成的最短路徑。由于Psparse都在障礙物邊緣，容易導(dǎo)致危險。因此加入安全距離約束，確保路徑中的每一個路徑點離障礙物有一定的距離，在快速避碰的同時確保安全。

安全距離約束為：

式中： dsafe為安全距離； d(c ,obs)為路徑點到最近障礙物點的歐式距離。

采用障礙物膨化的方法，實現(xiàn)上述約束，使Psparse出現(xiàn)在膨化后的障礙物邊緣。障礙物膨脹公式如下：

式中：X 為障礙物集合；x 為障礙物集合中的柵格元素；E 為輸出結(jié)果；S 為膨脹結(jié)構(gòu)元素，相關(guān)參數(shù)的選取如下式：

其中：Sshap為S 的形狀；os為障礙物形狀；ms為障礙物周邊可行區(qū)域大?。籹s為環(huán)境場景；g 為關(guān)系函數(shù)；Ssize為S 的尺寸；dsafe為安全距離。

選擇不同大小與形狀的膨脹結(jié)構(gòu)元素，會改變路徑關(guān)鍵點的位置，從而對路徑長度與路徑拐角的大小造成影響，如圖5 所示。圖像大小為300*300 像素，圖像中心處有一個半徑15 像素的圓形障礙物。

圖 5 圓形小尺寸膨脹結(jié)構(gòu)元素和方形膨脹結(jié)構(gòu)元素Fig.5 Round small size and square big size

圖5 左圖是基于Sshap=circle，Ssize=11 的膨脹結(jié)構(gòu)元素的路徑規(guī)劃示意圖，其中環(huán)形區(qū)域為dsafe=5 的量化安全距離區(qū)域。

圖5 右圖是基于Sshap=square，Ssize=41 的膨脹結(jié)構(gòu)元素的路徑規(guī)劃示意圖，其中外圈區(qū)域為dsafe=20 的量化安全距離區(qū)域。

通過圖5 的對比，可以看出，Ssize決定了dsafe的大?。籗shap影響著路徑拐角的大小。

在加入了安全距離約束后，每一個路徑點都會滿足 d(c ,obs)≥dsafe，且 d(Psparse,obs)=dsafe，因此對安全距離進(jìn)行了量化，從而能夠規(guī)劃出一條滿足安全距離的最短路徑。

2.4 無人艇運動學(xué)約束

現(xiàn)有的路徑規(guī)劃算法，往往把無人艇看作一個理想點，沒有考慮到無人艇的運動性能。實際航行中，當(dāng)無人艇以穩(wěn)定速度轉(zhuǎn)彎時，為了防止無人艇翻船，存在一個最小轉(zhuǎn)彎半徑Rm。當(dāng)規(guī)劃出的路徑拐角較小時，無人艇無法跟蹤此路徑。

因此傳統(tǒng)方法會對每一個路徑拐點進(jìn)行判斷。當(dāng)拐角為銳角時，進(jìn)行拐角優(yōu)化處理。如圖6 所示。利用半徑為Rm的圓弧對路徑點A 前后的路徑進(jìn)行擬合,從而得到切點B1、B2，原來的路徑為 B1B2A可以擬合為，再對圓弧做切線，將得到 C1C2作為拐角優(yōu)化后的路徑來代替。

圖 6 拐角約束Fig.6 Corner constraint diagram

由于本文對勢場函數(shù)進(jìn)行了改進(jìn)，并加入稀疏點約束，路徑關(guān)鍵節(jié)點將出現(xiàn)在障礙物邊緣，如圖7（a）所示。由于又加入了安全距離約束，最后會得到一條量化安全距離的最短路徑，因此這條路徑不會存在為銳角的拐角，如圖7（b）所示。圖7（b）中的障礙物由圖7（a）中障礙物膨化得到，膨脹結(jié)構(gòu)元素的形狀為正方形，尺寸為3。

圖 7 自動滿足運動學(xué)約束示意圖Fig.7 Automatically satisfying kinematic constraint

從圖7 可以看出，加入了安全距離約束后，路徑拐角將不會出現(xiàn)銳角。

2.5 海事規(guī)則約束

國際海上避碰規(guī)則公約（International Collision Regulations，COLREGS）是由國際海事組織制定，為了防止、避免海事船舶相撞而制定的海上交通規(guī)則[14]。為了無人艇的航行安全，應(yīng)使無人艇遵守國際海上避碰規(guī)則公約。

根據(jù)COLREGS、船舶避碰的經(jīng)驗數(shù)據(jù)以及實際船只在復(fù)雜海洋環(huán)境中航行范圍，為無人艇定義了追趕（Overtaking，OT）、相遇（Head-on，HO）、右交叉（Crossing from right，CFR）、左交叉（Crossing from left，CFL）4 種沖突局面，并可得到?jīng)_突局面的關(guān)系函數(shù)和航行規(guī)則表。

式中：P 為目標(biāo)船只所在范圍，如圖8 所示。 Δθ為無人艇與目標(biāo)船只航向角角度差，并將無人艇的航向定義為0°，。

圖 8 無人艇與目標(biāo)船只相對位置圖Fig.8 Relative position map of USV and target vessel

根據(jù)COLREGS 可以得到無人艇在遇到目標(biāo)船只的避碰轉(zhuǎn)向模型：

由于改進(jìn)迭代勢場法在加入了安全距離約束后，可以通過量化安全距離，沿膨化后障礙物邊緣通行。因此可以通過環(huán)境感知系統(tǒng)檢測目標(biāo)船只的位置，再利用HMM 的方法預(yù)測目標(biāo)船只的位置，根據(jù)表1 和式（9），判斷將向哪個方向轉(zhuǎn)向。然后對障礙物進(jìn)行選擇性膨脹，從而滿足海上避障規(guī)則公約的要求，如圖9 所示。

圖 9 海事規(guī)則約束下的無人艇避障方向Fig.9 Obstacle avoidance direction under the COLREGS

圖9 （a）表示在不加入海事規(guī)則約束下的無人艇動態(tài)避碰路線。由于此時無人艇會進(jìn)入HO 局面，按照式（9）和表1 可知，無人艇應(yīng)向右繞行。為了確保符合COLREGS，在障礙物中沿繞行方向的反方向進(jìn)行選擇性膨脹，如圖11（b）所示。由于Psparse是能夠構(gòu)成最短路徑的最優(yōu)點，針對圖11（b）中的非對稱障礙物，將會規(guī)劃出向右繞行的路徑。

改進(jìn)迭代勢場算法流程如見圖10 所示。

圖 10 改進(jìn)迭代勢場法流程圖Fig.10 Improved iterative potential field method flow chart

3 仿真試驗

3.1 算法性能仿真

由于無人艇航行實際場景中的障礙物分布往往較為簡單與單一，為了能夠清楚顯示本文算法的優(yōu)點，采用如圖11 所示的復(fù)雜迷宮環(huán)境來驗證稀疏迭代算法的性能和有效性，并與傳統(tǒng)迭代勢場算法進(jìn)行對比分析。其中圖像大小為300*300 像素。

表 1 航行規(guī)則表Tab.1 Navigation rules table

圖 11 迭代勢場與改進(jìn)迭代勢場法的路徑規(guī)劃圖Fig.11 Path planning with IPF and SIPF

圖11（a）和圖11（b）為傳統(tǒng)迭代勢場算法規(guī)劃出來的路徑，勢場函數(shù)參數(shù)a=1，b=1。

圖11（b）是在傳統(tǒng)迭代勢場算法的基礎(chǔ)上加入最短路徑約束所規(guī)劃出來的路徑。

圖11（c）和圖11（d）為稀疏勢場算法規(guī)劃出來的路徑，其中的勢場函數(shù)參數(shù)。

圖11（c）為稀疏迭代勢場算法（沒有加入安全距離約束）規(guī)劃出來的路徑。圖11（d）為稀疏迭代勢場算法（安全距離為4 個像素，膨脹結(jié)構(gòu)元素形狀為正方形）規(guī)劃出來的路徑。

規(guī)劃出來的路徑相關(guān)參數(shù)見表2。其中time 為算法運行時間，lenth 為路徑長度，dist 為平均障礙物距離，即

表 2 迭代勢場法與稀疏迭代勢場法的迷宮地圖試驗數(shù)據(jù)Tab.2 Related data of IPF and SIPF on maze map

式中：n 為路徑點個數(shù)， d(ci,obs)為每一個路徑點到離自己最近阻礙點的距離。Uobs(c)

從圖11（a）和表2 可以看出，由于考慮 ，迭代勢場算法規(guī)劃出的路徑整體較為平滑，但無法量化安全距離，且計算時間較長，不滿足局部避碰的實時性要求。

從圖11（b）中可以看出，即使加入了最短路徑約束，迭代勢場算法規(guī)劃出來的路徑依然不是最短路徑，這是由于 Uobs(c)項影響了路徑最優(yōu)點的生成位置。因此認(rèn)為迭代勢場算法不具備快速避碰功能。

從圖11（c）與圖11（d）以及表2 可以看出，稀疏迭代勢場算法在加入了安全距離約束后可以規(guī)劃出一條量化了安全距離的最短路徑，符合快速避碰的需求。

綜上，我們可以看出稀疏迭代勢場算法的平均運行效率比傳統(tǒng)迭代勢場算法提高了14.4 倍，此外能夠量化安全距離，得到最短路徑，滿足海事規(guī)則和運動學(xué)約束，能夠讓無人艇實現(xiàn)實時、快速、動態(tài)地安全避碰。

3.2 無人艇靜態(tài)障礙物避碰仿真

設(shè)初始運動參數(shù)如下：航速v = 5 m/s，航向θ=90°，無人艇航向坐標(biāo)系見圖8，終點的方位為90°，距離無人艇280 m。靜態(tài)障礙物是一個半徑15 m的圓形障礙物，方位為90°，距離無人艇125 m。其中地圖大小為300*300 像素。

圖 12 靜態(tài)障礙物避碰徑示意圖Fig.12 Static obstacle avoidance diagram

圖12 （a）為迭代勢場算法規(guī)劃出來的路徑，勢場函數(shù)參數(shù) a= 1， b= 1。

圖12（b）為稀疏迭代勢場算法（沒有加入安全距離約束）規(guī)劃出來的路徑，規(guī)劃出來的路徑相關(guān)參數(shù)見表3，表中參數(shù)含義與表2 相同。

從表3 可以看出，稀疏迭代勢場算法的路徑規(guī)劃時間比迭代勢場算法小了一個數(shù)量級。從圖12（b），圖5 可以看出，稀疏迭代勢場算法規(guī)劃出來的路徑是最短路徑，且不存在有銳角的拐角，滿足無人艇的運動學(xué)約束。當(dāng)dsafe增大時，lenth，dist 也會隨之增大。表明稀疏迭代勢場算法可以很好地規(guī)劃出一條量化了安全距離的最短路徑。此外平均規(guī)劃效率比傳統(tǒng)迭代勢場法提高了16.6 倍。

表 3 采用迭代勢場法與稀疏迭代勢場法進(jìn)行靜態(tài)障礙物避障得到的相關(guān)數(shù)據(jù)Tab.3 Related data of static obstacle avoidance using IPF and SIPF

3.3 無人艇動態(tài)障礙物避碰仿真

設(shè)本艇初始運動參數(shù)如下：航速v=5 m/s，航向θ=90°，無人艇航向坐標(biāo)系見圖8，終點的方位為90°，距離無人艇280 m。動態(tài)障礙物是一個半徑15 m的圓形障礙物，其中地圖大小為300*300 像素。

動態(tài)障礙物以3 m/s 的速度向東航行，且 Δθ=0°，基于HMM 方法，預(yù)測動態(tài)障礙物距離無人艇125 m，在無人艇90°方向。根據(jù)表1 和式（9），可知無人艇與目標(biāo)船只處于OT 局面，無人艇應(yīng)向左繞行。為此采取圖13 所示的選擇性膨脹方法，確保無人艇避碰符合海事規(guī)則約束。

圖 13 OT 局面下無人艇動態(tài)航跡Fig.13 USV dynamic track under OT situation

4 實船驗證

針對算法開展實船試驗。基于海圖和航海雷達(dá)獲知全局地圖，并使用稀疏迭代勢場法進(jìn)行全局路徑試驗?；诤胶＠走_(dá)、激光雷達(dá)、聲吶、攝像機(jī)等傳感器獲知局部環(huán)境信息，并基于稀疏迭代勢場法進(jìn)行局部路徑規(guī)劃，局部路徑規(guī)劃的感知距離為600 m。在距離障礙物100 m 時開始進(jìn)行避障，量化安全距離為25 m。

圖 14 全局路徑規(guī)劃徑示意圖Fig.14 Static obstacle avoidance diagram

全局路徑規(guī)劃圖如圖14 所示。其中無人艇以12 kn速度航行，航向為45°，目標(biāo)船只以5 kn 速度航行，航向為90°，無人艇航向坐標(biāo)系見圖8。

由于目標(biāo)船只近似為矩形且周邊只有一個障礙物，采用正方形結(jié)構(gòu)算子進(jìn)行膨化以滿足安全距離約束?？梢钥闯?，該方法能夠規(guī)劃出一條安全、符合C O L R E G S、無人艇運動學(xué)約束的最短路徑。在600*600 像素的地圖上能夠以2 Hz 的頻率進(jìn)行規(guī)劃，滿足無人艇局部避碰的實時性要求。

5 結(jié) 語

本文針對無人水面艇的局部避碰進(jìn)行了研究，所提出的稀疏迭代勢場算法不但避免了傳統(tǒng)人工勢場法所產(chǎn)生的局部極小值問題，還解決了柵格法規(guī)劃的路徑無法滿足無人艇運動約束的問題，而且可以實現(xiàn)實時、快速、安全地避碰，并且規(guī)劃出來的路徑符合無人艇的運動特性和海事規(guī)則。最終能夠在多約束條件下，規(guī)劃得到一條量化了安全距離的最短路徑，讓無人艇具備自適應(yīng)場景功能。仿真分析和實船試驗均驗證了本文所提方法的有效性。且表明稀疏迭代勢場法的運行效率比迭代勢場算法提高了14 倍。