限制區域水面無人艇路徑規劃與跟蹤控制研究

劉正鋒，張隆輝，魏納新，匡曉峰

（中國船舶科學研究中心，江蘇 無錫 214082）

0 引 言

水面無人艇（Unmanned Surface Vehicle，USV）作為一種無人化與智能化作戰平臺，在軍用和民用領域都有著廣泛的應用前景。從目前的發展趨勢來看，無人艇技術研究的重點在于確保無人艇能夠在包含靜態和動態障礙物的復雜海域環境下安全、有效地自主航行。路徑規劃及路徑跟蹤控制是無人艇自主航行的前提，是無人艇智能化程度的重要體現，同時也是迫切需要解決的關鍵技術問題。

路徑規劃屬于無人艇任務規劃的底層問題，包含兩種類型：一種是環境信息已知的全局路徑規劃；另一種是基于傳感器信息的局部路徑規劃。常用的規劃方法有啟發式搜索算法[1-2]、人工勢場法及智能規劃算法等。Dijkstra算法是一種早期的路徑搜索算法，其速度較慢，研究人員又發展了A*算法及其改進算法，提高了計算速度。然而這類方法所得的路徑距離障礙物太近，會影響無人艇自主航行的安全。人工勢場法[3-4]的原理是將目標點視為吸引子，而將障礙物看為斥力點，無人艇沿著引力與斥力的合力方向前進，該方法明顯的不足是存在局部“陷阱”，在障礙物前會產生振蕩，因此人工勢場的設計是算法能否應用成功的關鍵。智能規劃算法[5-7]隨著人工智能技術的發展取得了長足的進步，比較典型的有粒子群算法、遺傳算法及一些混合改進算法等，規劃路徑通常是一條較為光滑的曲線。當地圖信息復雜時，規劃路徑會出現較多的航向改變點，導致無人艇路徑跟蹤時需進行較為頻繁的操舵控制，間接地影響無人艇自主航行的安全性能。概率地圖法(PRM)[8-12]是一種基于采樣的圖形算法，通過在規劃環境中隨機設置路標點，從起始點出發，在保證不與障礙物相交的前提下將當前點與所有臨近點用直線段連接，構成初始路徑圖；然后運用優化算法對初始路徑圖進行查詢搜索，從而獲得從出發點到目標點滿足優化目標的優化路徑。該方法可以很好地解決高維空間中路徑規劃問題，其算法的復雜度主要來源于搜索路徑圖的難度，環境范圍以及空間維度影響不大，并且計算量較小，具有較好的實時性，在機器人任務規劃以及無人機路徑規劃中得到了廣泛的應用?；诟怕实貓D法良好的工程應用特性，本文將該方法應用到水面無人艇的路徑規劃中。

跟蹤控制的主要目的是保證無人艇按照路徑規劃設計的期望路徑航行。水面無人艇在復雜的海洋環境中航行時，由于船舶自身的欠驅動特性，對操縱性能、控制特性以及可靠性提出了更高的要求。從20世紀90年代起，國內外學者對水面無人艇的欠驅動控制問題[13-15]進行了深入研究。研究的主要方法包括Backstepping設計法、Lyapunov直接法、模糊控制、滑?？刂?、預測控制等，其中預測控制以其在約束控制問題處理上的優勢而廣受關注[16-18]。Li等[19-20]在加強橫搖約束時，考慮舵機飽和約束和運動速率約束，利用MPC實現了對四自由度水面船舶的航跡跟蹤問題。Oh等[21-22]在欠驅動船舶跟蹤軌跡過程中先利用視距導引法對航路點跟蹤問題建模，同時設計航路點跟蹤的導航和控制系統，通過求解線性約束優化問題來提高路徑跟蹤控制器的性能；Zheng等[23]基于非線性模型和連續線性化后的水面船舶分別設計了MPC控制器，研究了水面船舶的約束軌跡跟蹤問題，驗證了MPC對船舶軌跡跟蹤的有效性，特別是處理約束的能力；Zhang等[24]考慮了橫搖約束限制，設計了預測控制器，研究了受外界載荷干擾下的欠驅動水面船舶路徑跟蹤問題。上述工作大多是針對預設路徑開展的跟蹤控制器的設計，沒有考慮限制區域航行時航跡規劃的實現及影響，但是這些研究工作為水面無人艇路徑規劃后跟蹤控制器的設計提供了很好的參考。

水面無人艇的路徑規劃和路徑跟蹤是密切聯系的，路徑規劃需要考慮無人艇自身運動控制的特性，同時規劃路徑需要通過路徑跟蹤來實現。因此，本文針對限制區域水面無人艇的路徑規劃與控制技術進行研究，首先利用概率地圖法在限制區域進行路徑規劃，然后通過模型預測控制完成無人艇對規劃路徑的跟蹤控制。

1 基于PRM的無人艇路徑規劃

概率地圖法通過構造自由空間中的連通圖，將連續空間的規劃問題轉換為拓撲空間的規劃問題，問題求解的復雜度主要依賴于路徑搜索復雜度，而與整個規劃環境的復雜度以及規劃空間的維度無關?；镜腜RM算法分為兩個階段：概率地圖的構建以及路徑的查詢搜索。首先，PRM算法在自由空間內（圖1）基于某種概率分布進行空間采樣生成路徑點V（圖2），并將這些采樣點與它們的鄰居節點進行互聯構建地圖中的邊集E（圖3），從而構成代表自由空間的概率地圖R；然后從起始點開始在概率地圖中進行搜索，找出一條連接起始點和目標點的可行路徑（圖4）。PRM算法的實現流程如圖5所示。

圖1 規劃空間示例Fig.1 Illustration of planning space

圖2 隨機采樣點分布Fig.2 Sampling points in planning space

圖3 鄰近點連接得到的路徑段Fig.3 Feasible edges of neighboring points

圖4 路徑搜索結果Fig.4 Planning path output

圖5 概率地圖算法流程圖Fig.5 Flow chart of probabilistic roadmap method

在構建概率地圖過程中，邊集通過局部規劃器完成。局部規劃器是一個高速的、作用單一的規劃單元，它將符合某種衡量準則的兩個采樣點連接起來。衡量準則通常基于兩方面的要求：一是兩點之間距離與設定閾值的比較；二是兩點之間線段不能與障礙物發生碰撞。在可行路徑的查詢搜索階段,運用優化方法對地圖邊集進行搜索，從而得到一組由邊構成的從出發點到目的點并滿足優化目標的路徑。如果起始點和目標點不存在相連路徑,則重新回到路徑圖構建階段，進行節點增強以提高路線圖在復雜區域的連通性，并重新規劃直至輸出可行路徑。

概率地圖方法實現簡單，且具有良好的工程實用性，因此本文將該方法應用到水面無人艇路徑規劃中。水面無人艇在航行時會受到水域的限制威脅，如暗礁、浮標、移動的船只等，路徑規劃的主要目的就是在地圖空間內尋找初始點到目標點并避開障礙物的可行途徑。為簡化問題，用圓形或由相交圓構成的區域將障礙物覆蓋，這也為利用概率地圖法在處理包含復雜外形障礙物區域路徑規劃問題時提供了合理的便捷條件。因此，本文在進行路徑規劃研究時直接用圓形障礙物代替。另外，在水面無人艇路徑規劃過程中，為保證規劃路徑的可行性與有效性，需要對障礙物進行膨脹處理，膨脹半徑可以采用無人艇的回轉半徑代替，也可以根據航行環境要求進行設置。結合無人艇的操縱性能要求，為保證路徑跟蹤控制過程中水面無人艇艏向不會發生大幅度的改變，需要提高規劃路徑的工程實用性，本文給出兩種處理方法：（1）PRM方法路徑規劃的結果與采樣點的分布相關，考慮到該方法效率高，可以在規劃空間內進行路徑點多次采樣及路徑規劃，并在規劃路徑中比較尋優，選擇適應度更優的規劃路徑作為目標路徑；（2）在路徑查詢搜索時需要加以約束，可以設置路徑線段方向變化的閾值范圍，搜索下一路徑線段時，僅保留與當前路徑方向偏差在閾值范圍內的線段，以確保規劃路徑的方向角不發生劇烈的變化。

為驗證限制區域內PRM方法在無人艇路徑規劃中的效果，假設規劃空間范圍為500×500，在此范圍內隨機生成50個障礙物，考慮邊界影響膨脹后的圓形半徑取為10，無人艇起始點位置為[10，10]，目標點位置為[490，490]，路徑規劃結果如圖6～9所示。

圖6 傳統PRM方法的可行路徑Fig.6 Feasible paths by classic PRM

圖6給出了利用傳統PRM規劃出的10條可行路徑?？梢钥闯觯琍RM產生的規劃路徑是由一系列直線段組成，每一條路徑都能保證無人艇從出發點到達目標點，并且都能有效避開障礙物。但是，部分單次規劃出的可行路徑航線方向會出現急劇的變化，最大約為116°，這顯然不利于無人艇的路徑跟蹤控制，在工程實際中不適用，需要剔除。為避免這一問題，我們可以進行多次規劃，選取適應度更優的規劃路徑。圖7給出了圖6所示的10條可行路徑中以路徑最短為評價指標的尋優結果。顯然，最短路徑更為平滑，航線方向不會出現大幅度變化，最大約為45°，有利于無人艇的路徑跟蹤控制。

圖7 當前最短路徑Fig.7 Current shortest path

圖8給出了考慮航線方向約束（航線方向變化量不超過45°）的10條規劃路徑。可以看出，各可行路徑航線段的方向變化均在允許范圍內平滑地過渡到下一航線段，都適合作為無人艇的跟蹤路徑。以路徑長度作為適應度指標來評價，最短路徑如圖9所示，該路徑更為平滑，且整段路徑僅出現一個航向改變點，航線方向變化約為10°，可以選為無人艇跟蹤控制的目標路徑。

圖8 考慮航線方向約束的規劃路徑（Δψk≤45°）Fig.8 Feasible paths considering route direction constraint（Δψk≤45°）

圖9 考慮約束的最短路徑（Δψk≤45°）Fig.9 Shortest path considering route direction constraint（Δψk≤45°）

從上面的示例可以看出，概率地圖法可以成功地應用于限制區域內水面無人艇的路徑規劃，規劃路徑由一系列直線段組成，便于實現水面無人艇路徑跟蹤控制。此外，通過多次規劃尋優或者加入航線方向約束限制，規劃所得路徑的適應度更優、更平滑，更有利于無人艇的跟蹤控制，可以作為無人艇跟蹤控制的最終目標路徑。

2 路徑跟蹤控制

2.1 數學模型

無人艇路徑規劃完成后，需要設計跟蹤控制器來控制無人艇的運動從而實現規劃路徑的跟蹤。實際海況中水面船舶的運動非常復雜，就路徑跟蹤控制問題而言，僅需考慮縱向、橫向以及艏搖運動，可將問題簡化為三自由度水平面運動控制問題。對于欠驅動水面無人艇而言，路徑跟蹤主要通過舵的控制使無人艇從起始位置沿著設計路徑航行，在此過程中可以忽略時間和航速的控制問題，著重考慮操舵對無人艇艏向的控制。通常情況下，艇體對操舵的響應是一種緩慢的運動，可以用野本方程來描述?？紤]艏向控制時非線性的影響，野本方程形式如下：

式中，K和T分別為無人艇的回轉性指數和操縱性指數，α為非線性模型系數，ψ為無人艇艏向角。

參考圖10所示的坐標系，當前航線段PkPk-1的方向ψk可以表示為

圖10 路徑跟蹤示意圖Fig.10 Sketch of path following

無人艇的位置在當前航線段上可以表示為

對式（5）兩邊求導可得

無人艇航行速度U在慣性系和艇體坐標系中滿足如下關系：

跟蹤過程中不考慮縱向運動的控制，可以忽略操舵對無人艇航速的影響。考慮簡化，假設u=U，v=0，代入式（6）可得

結合操舵運動響應方程式（2），最終可得無人艇路徑跟蹤的數學模型為

2.2 模型預測控制

路徑跟蹤數學模型式（10）的標準形式如下：

參考模型預測控制建模方法，定義二次型性能函數J為

式中，N為預測范圍，Q和R分別為權重矩陣。約束條件包括：

式中：

3 仿真分析

為綜合驗證限制區域內水面無人艇路徑規劃及跟蹤控制的效果，進行了數值仿真試驗。限制區域范圍為1 000 m×1 000 m，在此范圍內生成50個隨機障礙物，考慮邊界影響膨脹設定障礙區域半徑范圍[15 m，40 m]來模擬不同尺度的障礙物。限制區域環境如圖11所示，無人艇起始點位置為[10 m，10 m]，目標點位置為[990 m，990 m]。路徑跟蹤控制過程中的主要仿真參數如表1所示。

圖11 規劃空間Fig.11 Planning area

表1 主要運動參數Tab.1 Motion parameters

設計的模型預測控制器中，預測范圍取為N=50，權重矩陣選取Q=diag{[1,1,1]}，R=1，無人艇舵角的控制采用優化求解的第一個指令分量執行。在路徑跟蹤控制時，不失一般性考慮，可令初始艏向指向目標點。另外，為確保航線段切換順利，設定一個閾值范圍（本文取為3L），當無人艇進入路徑轉向點的閾值范圍內時，跟蹤路徑切換為下一航線段直至無人艇到達目標位置。

首先在模擬的限制區域環境中進行路徑規劃尋求無人艇從起始位置到達目標位置的目標路徑。圖12給出了不考慮航線方向約束和考慮航線反向約束兩種情形，通過PRM多次規劃尋優比較后所得的最終目標路徑，分別記為Path1和Path2。

圖12的結果表明，兩條路徑均可以保證從起始位置避開障礙物到達目標位置。不考慮航線方向約束的目標路徑（Path1）長度約1 447 m，整體路徑由兩條直線段組成，存在一次航線方向改變，角度大約為35°；考慮航線方向約束的目標路徑（Path2）長度約1 400 m，整體路徑由三條直線段組成，存在兩次航線方向改變，角度變化量最大值約為20°，即相對而言，目標路徑Path2路徑長度更短而且顯得更為平滑，更有利于進行跟蹤控制。

圖12 路徑規劃結果Fig.12 Path planning results

下面分別對以上兩條目標路徑，利用模型預測控制器對水面無人艇進行跟蹤控制仿真。圖13～15為無人艇對目標路徑1進行跟蹤控制的結果，圖16～18為無人艇對目標路徑2進行跟蹤控制的結果。

圖13 路徑1跟蹤控制Fig.13 Path1 following control

圖14 路徑1跟蹤誤差時歷曲線Fig.14 Path1 cross tracking error

圖15 路徑1跟蹤舵角指令Fig.15 Rudder control in Path1 following

圖16 路徑2跟蹤控制Fig.16 Path2 following control

圖17 路徑2跟蹤誤差時歷曲線Fig.17 Path2 cross tracking error

圖18 路徑2跟蹤舵角指令Fig.18 Rudder control in Path2 following

仿真結果表明，在模型預測控制器作用下,水面無人艇通過對舵的控制，可以快速平穩地逼近并收斂于跟蹤路徑，最終實現對目標路徑Path1和目標路徑Path2的跟蹤。文中所示的路徑跟蹤過程中，Path1大約耗時960 s，Path2約耗時930 s。

圖14和圖17分別給出了路徑Path1和路徑Path2跟蹤控制過程中的路徑偏差曲線。路徑跟蹤過程中，由于無人艇到達轉向路徑點需要進行航線段的切換，實時路徑誤差由當前無人艇位置與下一航線段的偏差決定，所以在顯示圖中會出現跳躍信號，并非控制過程不穩定。無人艇對兩條路徑跟蹤控制過程中的操舵指令分別如圖15和圖18所示。不難看出，舵角指令的觸發點對應于跟蹤路徑的切換轉向點。從目標路徑Path1和Path2的跟蹤效果局部對比可以看出，路徑2比路徑1更平滑，整個跟蹤控制過程中位置誤差相對較小，并且能更快速地收斂于目標路徑，這也說明了目標路徑2更適合作為路徑跟蹤控制的目標路徑。

4 結 論

本文介紹了對限制區域水面無人艇路徑規劃與跟蹤控制的研究。該研究利用概率地圖法在限制區域內進行路徑規劃，并設計模型預測控制器對目標路徑進行跟蹤控制，將無人艇的路徑規劃和跟蹤控制有機地結合起來。算例仿真分析結果表明：

（1）概率地圖法可以成功地應用于限制區域內水面無人艇的路徑規劃，規劃路徑由一系列直線段組成，有利于對水面無人艇進行路徑跟蹤控制器的設計；同時，通過多次規劃、增加航線方向角度約束限制等，可以提高PRM規劃路徑的工程實用性；

（2）考慮路徑跟蹤控制的特殊性，采用線性化操舵響應模型所設計的模型預測控制算法，可以快速平穩地實現欠驅動水面無人艇對目標路徑的跟蹤控制。

本文工作為限制區域內水面無人艇的路徑規劃和跟蹤控制技術研究提供了一條行之有效的實施途徑。需要指出的是，PRM規劃出的路徑依賴于采樣點在規劃空間的分布。因此，PRM規劃通常得不到最短路徑，但是PRM可以在短時間內規劃出多條可供選擇的可行途徑以及次優路徑。在實際工程中，無人艇路徑規劃與跟蹤控制還需考慮限制水域內動態障礙物的影響、外界環境條件的影響以及突發事件的影響，這要求路徑規劃具有更為快速高效的響應能力。另外，在路徑跟蹤控制時，舵機控制除了飽和舵角約束還需考慮轉舵速率的約束，這些內容有待于在以后的工作中進一步研究。