限制區(qū)域水面無人艇路徑規(guī)劃與跟蹤控制研究

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

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

0 引 言

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

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

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

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

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

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

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

圖2 隨機(jī)采樣點(diǎn)分布Fig.2 Sampling points in planning space

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

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

為驗(yàn)證限制區(qū)域內(nèi)PRM方法在無人艇路徑規(guī)劃中的效果，假設(shè)規(guī)劃空間范圍為500×500，在此范圍內(nèi)隨機(jī)生成50個(gè)障礙物，考慮邊界影響膨脹后的圓形半徑取為10，無人艇起始點(diǎn)位置為[10，10]，目標(biāo)點(diǎn)位置為[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ā)點(diǎn)到達(dá)目標(biāo)點(diǎn)，并且都能有效避開障礙物。但是，部分單次規(guī)劃出的可行路徑航線方向會(huì)出現(xiàn)急劇的變化，最大約為116°，這顯然不利于無人艇的路徑跟蹤控制，在工程實(shí)際中不適用，需要剔除。為避免這一問題，我們可以進(jìn)行多次規(guī)劃，選取適應(yīng)度更優(yōu)的規(guī)劃路徑。圖7給出了圖6所示的10條可行路徑中以路徑最短為評(píng)價(jià)指標(biāo)的尋優(yōu)結(jié)果。顯然，最短路徑更為平滑，航線方向不會(huì)出現(xiàn)大幅度變化，最大約為45°，有利于無人艇的路徑跟蹤控制。

圖7 當(dāng)前最短路徑Fig.7 Current shortest path

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

圖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°）

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

2 路徑跟蹤控制

2.1 數(shù)學(xué)模型

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

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

參考圖10所示的坐標(biāo)系，當(dāng)前航線段PkPk-1的方向ψk可以表示為

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

無人艇的位置在當(dāng)前航線段上可以表示為

對(duì)式（5）兩邊求導(dǎo)可得

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

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

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

2.2 模型預(yù)測控制

路徑跟蹤數(shù)學(xué)模型式（10）的標(biāo)準(zhǔn)形式如下：

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

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

式中：

3 仿真分析

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

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

表1 主要運(yùn)動(dòng)參數(shù)Tab.1 Motion parameters

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

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

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

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

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

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

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

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

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

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

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

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

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

4 結(jié) 論

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

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

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

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