李清亮，李彬，孫國皓，崔星，毛新濤

1 四川大學 電氣工程學院，四川 成都 610065

2 四川大學 空天科學與工程學院，四川 成都 610065

3 北京軒宇空間科技有限公司，北京 100194

0 引 言

近年來，無人駕駛受到日益廣泛的關注，在飛行器、智能車、艦船等領域得以快速發(fā)展。智能化是未來艦船的發(fā)展趨勢之一[1]。由于無人艇的制造成本低、制造周期短、環(huán)境適應能力強、人力成本低，其在海洋資源勘測、航道測量、環(huán)境監(jiān)測、水域清潔、軍事作戰(zhàn)等領域具備良好的應用前景。如何快速生成無人艇在復雜環(huán)境下的避障航線，是其自主化發(fā)展的關鍵技術之一[2]。

李文華等[3]總結了在工業(yè)4.0 背景下世界范圍內船舶自主化航行技術的主要發(fā)展路徑，分析了無人艇航跡規(guī)劃、智能避障等關鍵技術。余必秀等[4]通過增加代價函數從而改進了傳統(tǒng)的A*算法，可用于規(guī)劃無人航道測量船的運動路徑。Song 等[5]提出了平滑的A*算法來減少冗余的路徑點，從而可以提供更連續(xù)的路徑。Liu 等[6]提出了一種基于快速步進（fast marching，F(xiàn)M）方法的無人艇信息導航路徑規(guī)劃算法，具有計算速度快、計算復雜度低等特點，該算法覆蓋了2 個區(qū)域（航行區(qū)域和避碰區(qū)域），可以保證規(guī)劃軌跡不違反任何禁區(qū)。Cho 等[7]在改進現(xiàn)有視線（line-of-sight，LOS）制導算法和速度障礙（velocity obstacle，VO）制導算法的基礎上，提出了航點跟蹤與避碰算法：即在路徑點轉換過程中引入額外的控制方案以提高跟蹤控制的穩(wěn)定性，并對傳統(tǒng)的VO 算法進行改進，從而有效解決了遇到障礙物時的位置不確定性問題，同時在真實海洋環(huán)境下驗證了算法的有效性。歐陽子路等[8]將雙向搜索 樹（bidirectional rapidly-exploring random trees，Bi-RRT）算法與速度障礙法相結合，提出了基于改進Bi-RRT 的無人艇自動避碰算法，并采用并行延伸擴展的2 顆搜索樹提高了算法的實時性。受人工智能技術的影響，很多智能算法已在無人艇路徑規(guī)劃領域得以運用，如粒子群算法[9]、蟻群算法[10]、強化學習[11]、人工神經網絡[12]等。舒宗玉[9]提出了多目標優(yōu)化混合粒子群路徑規(guī)劃算法，實現(xiàn)了無人艇路徑長度、路徑平滑性和安全性的多目標優(yōu)化。邱晨等[10]基于改進的蟻群算法，規(guī)劃了無人救生艇的最短無碰撞安全路徑。沈海青[11]將A*并行決策動態(tài)避障算法與基于深度Q 學習的智能避碰算法相結合，為無人艇提供了多層避碰導航。魏新勇[12]利用卷積神經網絡技術識別附近水域的障礙物，并結合模糊數學理論構建了危險系數指標，提出了遠程航跡重規(guī)劃和近程反應式避障方法。

障礙物的存在為航跡規(guī)劃帶來了巨大的挑戰(zhàn)，其中障礙物處理的難易程度直接決定了航跡優(yōu)化問題的復雜程度，而現(xiàn)有文獻對障礙物的處理方法普遍比較復雜。為此，本文擬采用圓形包絡面和凸四邊形包絡面對障礙物進行包絡處理，并將避障問題轉化為在笛卡爾坐標系下的不等式約束問題；通過引入精確罰函數，以簡化處理多個障礙物帶來的多個強制約束問題。同時，鑒于現(xiàn)有研究成果在航跡規(guī)劃方面鮮有考慮無人艇的運動特性，本文擬基于無人艇的運動方程，采用控制參數化方法求解時間最優(yōu)的控制問題；通過結合控制參數化方法與精確罰函數方法，較好地處理無人艇的航跡規(guī)劃和自動避障問題，并采用仿真驗證本文方法的有效性。

1 問題模型

1.1 無人艇運動學建模

為簡化處理，本文將無人艇視為僅規(guī)劃其運動航跡的質點，其運動學坐標系如圖1 所示。圖中， θ為無人艇的航向角，V為無人艇合速度矢量的模長，Vx和Vy分別為V沿x軸和y軸的速度分量。設定無人艇在t時刻的位置坐標為 (x(t),y(t))，狀態(tài)量為X(t)=[x(t)y(t) θ(t)V(t)]T， 控制量為u(t)=[γ(t)a(t)]T，則無人艇的動態(tài)方程（變量上方的符號“·”表示關于時間的一階導數）為

式中：V(t)為無人艇在t時刻的合速度矢量的模長；θ(t)為無人艇在t時刻的航向角； γ(t)為無人艇在t時 刻 θ 的 角 速 度；a(t) 為 無 人 艇 在t時 刻V的 加速度。

為便于描述，式（1）可以簡記為函數f：

圖1 無人艇運動學坐標系Fig. 1 Kinematic coordinate system of USV

1.2 靜態(tài)障礙物的規(guī)避區(qū)建模

在實際航行環(huán)境中，障礙物的形狀大多不規(guī)則，如果對障礙進行精確建模，勢必會顯著增加建模的復雜程度和工作量。為簡化處理，本文將采用圓形包絡面和凸四邊形包絡面來對障礙物進行包絡覆蓋。對于長寬比較小的不規(guī)則障礙物，可以采用圓形包絡面處理，如圖2（a）和圖2（b）所示。對于長寬比過大的細長型障礙物，如果仍然采用圓形包絡面處理，會造成較大的可通航區(qū)域浪費（圖2（c）），不利于航跡規(guī)劃。為此，對于細長型障礙物，宜采用凸四邊形包絡面進行處理，如圖2（d）所示。由障礙物包絡面和標準禁航區(qū)構成的不可航行區(qū)域，在下文統(tǒng)稱為禁航區(qū)（禁止無人艇航行的區(qū)域）。

圖2 障礙物區(qū)域的包絡圖Fig. 2 Envelope diagram of obstacle area

1.2.1 圓形包絡面建模

對于長寬比較小的不規(guī)則障礙物，可以采用圓形包絡面以簡化建模的復雜度。將障礙物的最長端連線作為包絡圓的直徑，而直徑中點作為包絡圓的圓心，如圖3 所示，圓心記作 (xi,yi)，半徑記作ri，其中i為障礙物序號。為保證無人艇的安全性，需引入安全閾值 ρ，其值為船身長度的3 倍。將增加了安全閾值的包絡圓作為覆蓋障礙物的禁航區(qū)域，即圖3 中的虛線圓，其半徑Ri=ri+ρ。為使無人艇規(guī)避障礙物，則其位置應滿足：

1.2.2 凸四邊形包絡面建模

圖3 圓形包絡面建模Fig. 3 Circular envelope modeling

對于長寬比較大的不規(guī)則障礙物，如果繼續(xù)采用圓形包絡面進行建模處理，會浪費大量的可航行區(qū)域，增加無人艇的航行距離，甚至出現(xiàn)無可航行區(qū)域情況。為此，宜采用凸四邊形包絡面進行建模處理。如圖4 所示，凸四邊形包絡面由4 條相交的直線lj,1，lj,2，lj,3，lj,4（j為障礙物序號）圍合而成。圖4 中的實線凸四邊形為障礙物的最小面積包絡面，虛線凸四邊形為考慮了安全閾值 ρ之后的包絡面。本文將采用斜截式直線方程對lj,1，lj,2，lj,3，lj,4進行建模，設定kj,1，kj,2，kj,3，kj,4為4 條邊的斜率，bj,1，bj,2，bj,3，bj,4為4 條虛線邊對應直線在y軸上的截距。

圖4 凸四邊形包絡面建模Fig. 4 Convex quadrilateral envelope modeling

為使無人艇規(guī)避凸四邊形包絡面，其位置應滿足下列4 個不等式之一：

當某邊的斜率不存在時，所在邊的不等式約束形式即變?yōu)閷(t)的約束。為簡化約束形式，式（4）可以轉化為

式中： min(·)為取最小值函數；z=1，2，3，4，為凸四邊形包絡面的直線序號。

1.3 優(yōu)化問題建模

航跡規(guī)劃的目標是：已知無人艇的初始狀態(tài)，在考慮禁航區(qū)域、最小/最大航行速度和終點位置約束的前提下，實現(xiàn)運動總時間的極小化，該問題模型（P1）為：

式中：C0 為控制量約束；C1 為無人艇運動學方程約束；C2 為無人艇的初始狀態(tài)；C3，C4 為終端位置約束；C5 為無人艇的航行速度約束；C6，C7 分別為圓形禁航區(qū)和凸四邊形禁航區(qū)；T為無人船的最大運動時間；γmax為最大轉向角速度；amax為最大航行加速度；X(0)為0 時刻的無人艇狀態(tài)；X0為無人艇的初始狀態(tài)；xT，yT為無人艇的終端位置坐標；Vmax是無人艇的最大航行速率；Ni，Nj分別為圓形包絡禁航區(qū)和凸四邊形包絡禁航區(qū)的障礙物最大數量。時間域 [0,T]是一個變化域，而約束C5～C7 中均包含每個時刻的狀態(tài)約束，這實際上將進一步產生無數個約束，所以給問題處理帶來了巨大的挑戰(zhàn)。

2 基于精確罰函數的最優(yōu)控制算法

2.1 控制參數化

本文將采用時間尺度變換和控制參數化方法來處理上述優(yōu)化問題。如圖5 所示，時間尺度變換將可變時間域t∈[0,T]變 換為固定域s∈[0, 1]，其中s為轉換后的時間刻度。由文獻[13-14]可知

圖5 時間尺度變換Fig. 5 Time scale transformation

為便于優(yōu)化處理，本文將連續(xù)控制量在s∈[0, 1]內等分為K個區(qū)間，從而產生K+1 個節(jié)點{0,s1,s2,···,sK}，其中sK=1。分段參數化的結果如圖6 所示，其函數表達式為[15]

圖6 控制參數化Fig. 6 Control parameterization

因此，式（6）可以改寫為問題模型（P2）：

式中：S0～S7 為約束C0～C7 的改寫形式；x(1)，y(1)為s=1 時刻的位置狀態(tài)。

2.2 基于精確罰函數的約束處理

（P2）中含有終端等式約束S3 和S4 以及連續(xù)狀態(tài)不等式約束S5～S7，這就為問題求解帶來了極大的挑戰(zhàn)。為此，本文采用精確罰函數[13,16]來處理約束S3～S7，以便將（P2）轉化為僅含控制約束和初始狀態(tài)約束的優(yōu)化問題，其優(yōu)化目標函數J為：

以上式中： ?為懲罰參數；X(1)為無人艇的終端狀態(tài)； α ＞1, β ＞2, γ ＞2, δ,Wi,Wj均 為 正 實 數 參 數；??α為懲罰權重； δ?β為懲罰因子； ?γW為松弛因子，其中W為松弛因子的權重； κ為縮放因子。

對凸四邊形包絡面而言，當無人艇違反凸四邊形包絡面的約束時，必將同時違反凸四邊形包絡面4 條邊的約束，經過4 次累乘后，其懲罰值將遠大于違反圓形包絡面的懲罰值。因此，本文采用了縮放因子 κ來對凸四邊形包絡面約束進行縮放處理。

在優(yōu)化初期，由于存在大量違反約束的情況，為了放寬對約束的懲罰程度，可以增加 ?以增加懲罰因子，從而減小懲罰權重并增加松弛因子；隨著優(yōu)化的進行，違反約束的情況將逐漸減少，可以減小 ?來減小懲罰因子，從而增加懲罰權重并減小松弛因子，最終進一步減少違反約束的情況。當 ?趨近0 時，懲罰權重將趨于無窮大，松弛因子趨于0，懲罰因子趨于0，此時，即可認為優(yōu)化結束后的目標函數J完全滿足了約束條件。

需注意的是， δ在理論上為正無窮才能使優(yōu)化過程中的 ?趨近于0。在實際計算中， ?小于一定的閾值即可認為其足夠小，因此可將 δ設置為一個較大的常數值。對于松弛因子權重W，可以根據不同懲罰項的松弛程度來調整具體數值；關于其他參數的設置，僅需滿足理論要求的約束即可[16]。

式（12）是一個標準的最優(yōu)參數選擇問題，為了將其轉化為常規(guī)的非線性優(yōu)化問題，需獲得目標函數J的梯度方程[15]，即

基于式（17），即可將式（12）轉化為一個無狀態(tài)約束的最優(yōu)參數選擇問題，且其適用于任何有效的分級優(yōu)化器進行求解。

3 數值仿真

為了驗證本文算法的可行性，設定無人艇的初始狀態(tài)X(0)=[0, 0, 0.75, 5]T， 終點位置為(xT,yT)=(500, 500)， 最大速率Vmax=8 m/s；罰函數的相關參數為： α=1.5, β=3, γ=3, δ=108； κ=10?4；Wi=0.3，其中i=1，2，3，4，5，6；Wj=0.3，其中j=1，2，其他參數如表1 所示。

表1 仿真數據Table 1 Simulated data

為了驗證包絡面建模對禁航區(qū)域的普適性，本文在仿真中設置了不同半徑的圓形包絡面和凸四邊形包絡面，其Matlab 仿真結果如圖7 所示（藍色填充區(qū)域為實際禁航區(qū)，虛線包絡區(qū)為考慮安全閾值后的禁航區(qū)）。根據仿真結果，無人艇從初始位置出發(fā)，可以完全規(guī)避禁航區(qū)域，其理想航跡的總時長為100.6 s。從圖7 可以看出，為了極小化航行時間，航跡與1 個禁航區(qū)存在相切的情況，在實際航行過程中，某些不確定性因素的影響可能使無人艇存在碰撞危險。不過，本文設置了安全閾值，可以確保航行過程中無人艇與實際禁航區(qū)的安全距離。圖8 所示為無人艇位置隨時間的變化曲線。

圖7 無人艇的航跡圖Fig. 7 Track map of USV

圖8 無人艇位置隨時間的變化圖Fig. 8 Diagram of USV position changing with time

圖9、圖10 所示為無人艇的運動速度和控制量隨時間的變化曲線，其仿真結果滿足相關約束條件。從圖9 可以看出，為了實現(xiàn)極小化的航行時間，無人艇在80 s 之前從初始狀態(tài)加速到了最大航行速度，并維持在最大速度附近；在80 s 之后，由于無人艇存在大角度的航向調整，為了極小化時間，需在轉彎半徑和轉彎速度之間進行權衡，雖然較小的轉彎半徑可以縮短轉彎航程，但需同時減小航行速度。從仿真結果可以看出，為了極小化航行時間，該無人艇的航向角速度正向滿舵且航行速度下降，從而匹配了最佳的轉彎半徑?？梢?，該仿真結果驗證了本文算法的有效性。

圖9 無人艇速度隨時間的變化曲線Fig. 9 Diagram of USV speed changing with time

圖10 輸入控制信號Fig. 10 Input control signal

4 結 語

本文采用圓形和凸四邊形包絡面處理不規(guī)則障礙物，將規(guī)避不規(guī)則障礙物的問題轉化為笛卡爾坐標系下的不等式約束條件，從而將無人艇避障航跡規(guī)劃問題構建為含有連續(xù)狀態(tài)不等式約束和終端約束的時間最優(yōu)控制問題。通過控制參數化和時間尺度變換，將時間最優(yōu)控制問題進一步轉化為了最優(yōu)參數選擇問題；同時，利用精確罰函數方法，將連續(xù)狀態(tài)不等式約束和終端約束構建為約束懲罰函數并添加到目標函數中，最終轉化為一個無狀態(tài)約束的最優(yōu)參數選擇問題，其適用于任何有效的分級優(yōu)化技術進行求解。因此，本文所提出的算法可以有效處理無人艇航跡規(guī)劃中的避障問題。