多約束條件下基于稀疏迭代勢場法的無人艇局部避碰方法

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

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

0 引 言

無人水面艇（以下簡稱無人艇）目前在國內外軍事和民用領域的應用日漸增多，而無人艇能夠自主航行的關鍵則取決于路徑規劃技術[1]。

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

這個問題會導致以下幾種情況發生：

1）存在陷阱區域；

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

3）在障礙物前震蕩；

4）在狹窄通道中擺動；

5）障礙物附近目標不可達。

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

上述方法可以解決局部最小值點導致的問題，但也存在著規劃過于復雜等問題。

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

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

1 傳統迭代勢場算法

1.1 柵格法

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

1.2 勢場函數

勢場函數如下式：

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

1.3 迭代二分搜索

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

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

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

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

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

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

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

2）需要迭代多次

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

3）需要調參

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

2 改進迭代勢場算法

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

2.1 改進勢場函數

改進勢場函數如下式：

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

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

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

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

2.2 稀疏點約束

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

稀疏點約束由聯通約束與最短路徑約束組成。

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

圖 3 重要聯通點Fig.3 Important connecting points

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

針對重要聯通點，再加入最短路徑約束。

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

采用Floby 算法對下式進行求解。

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

圖 4 稀疏點Fig.4 Sparse points

此項約束不但能夠將重要聯通點優化為稀疏點，而且可以規劃出最短路徑。路徑最短也就可以認為避開障礙物的時間最少（在符合無人艇運動學的情況下），從而滿足了局部路徑規劃的時間需求，能夠實現快速避碰。

2.3 安全距離約束

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

安全距離約束為：

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

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

式中：X 為障礙物集合；x 為障礙物集合中的柵格元素；E 為輸出結果；S 為膨脹結構元素，相關參數的選取如下式：

其中：Sshap為S 的形狀；os為障礙物形狀；ms為障礙物周邊可行區域大小；ss為環境場景；g 為關系函數；Ssize為S 的尺寸；dsafe為安全距離。

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

圖 5 圓形小尺寸膨脹結構元素和方形膨脹結構元素Fig.5 Round small size and square big size

圖5 左圖是基于Sshap=circle，Ssize=11 的膨脹結構元素的路徑規劃示意圖，其中環形區域為dsafe=5 的量化安全距離區域。

圖5 右圖是基于Sshap=square，Ssize=41 的膨脹結構元素的路徑規劃示意圖，其中外圈區域為dsafe=20 的量化安全距離區域。

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

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

2.4 無人艇運動學約束

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

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

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

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

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

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

2.5 海事規則約束

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

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

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

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

根據COLREGS 可以得到無人艇在遇到目標船只的避碰轉向模型：

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

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

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

改進迭代勢場算法流程如見圖10 所示。

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

3 仿真試驗

3.1 算法性能仿真

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

表 1 航行規則表Tab.1 Navigation rules table

圖 11 迭代勢場與改進迭代勢場法的路徑規劃圖Fig.11 Path planning with IPF and SIPF

圖11（a）和圖11（b）為傳統迭代勢場算法規劃出來的路徑，勢場函數參數a=1，b=1。

圖11（b）是在傳統迭代勢場算法的基礎上加入最短路徑約束所規劃出來的路徑。

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

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

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

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

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

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

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

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

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

3.2 無人艇靜態障礙物避碰仿真

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

圖 12 靜態障礙物避碰徑示意圖Fig.12 Static obstacle avoidance diagram

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

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

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

表 3 采用迭代勢場法與稀疏迭代勢場法進行靜態障礙物避障得到的相關數據Tab.3 Related data of static obstacle avoidance using IPF and SIPF

3.3 無人艇動態障礙物避碰仿真

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

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

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

4 實船驗證

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

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

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

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

5 結 語

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