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

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

0 引 言

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

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

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

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

1 基于PRM的無人艇路徑規(guī)劃

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

圖1 規(guī)劃空間示例Fig.1 Illustration of planning space

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

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

圖4 路徑搜索結(jié)果Fig.4 Planning path output

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

在構(gòu)建概率地圖過程中，邊集通過局部規(guī)劃器完成。局部規(guī)劃器是一個高速的、作用單一的規(guī)劃單元，它將符合某種衡量準則的兩個采樣點連接起來。衡量準則通?；趦煞矫娴囊螅阂皇莾牲c之間距離與設定閾值的比較；二是兩點之間線段不能與障礙物發(fā)生碰撞。在可行路徑的查詢搜索階段,運用優(yōu)化方法對地圖邊集進行搜索，從而得到一組由邊構(gòu)成的從出發(fā)點到目的點并滿足優(yōu)化目標的路徑。如果起始點和目標點不存在相連路徑,則重新回到路徑圖構(gòu)建階段，進行節(jié)點增強以提高路線圖在復雜區(qū)域的連通性，并重新規(guī)劃直至輸出可行路徑。

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

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

圖6 傳統(tǒng)PRM方法的可行路徑Fig.6 Feasible paths by classic PRM

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

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

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

圖8 考慮航線方向約束的規(guī)劃路徑（Δψ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°）

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

2 路徑跟蹤控制

2.1 數(shù)學模型

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

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

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

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

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

對式（5）兩邊求導可得

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

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

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

2.2 模型預測控制

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

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

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

式中：

3 仿真分析

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

圖11 規(guī)劃空間Fig.11 Planning area

表1 主要運動參數(shù)Tab.1 Motion parameters

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

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

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

圖12 路徑規(guī)劃結(jié)果Fig.12 Path planning results

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

圖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

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

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

4 結(jié) 論

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

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

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

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