基于精確罰函數(shù)的無(wú)人艇航跡規(guī)劃和自動(dòng)避障算法

李清亮，李彬，孫國(guó)皓，崔星，毛新濤

1 四川大學(xué) 電氣工程學(xué)院，四川 成都 610065

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

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

0 引 言

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

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

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

1 問題模型

1.1 無(wú)人艇運(yùn)動(dòng)學(xué)建模

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

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

為便于描述，式（1）可以簡(jiǎn)記為函數(shù)f：

圖1 無(wú)人艇運(yùn)動(dòng)學(xué)坐標(biāo)系Fig. 1 Kinematic coordinate system of USV

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

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

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

1.2.1 圓形包絡(luò)面建模

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

1.2.2 凸四邊形包絡(luò)面建模

圖3 圓形包絡(luò)面建模Fig. 3 Circular envelope modeling

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

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

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

當(dāng)某邊的斜率不存在時(shí)，所在邊的不等式約束形式即變?yōu)閷?duì)x(t)的約束。為簡(jiǎn)化約束形式，式（4）可以轉(zhuǎn)化為

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

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

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

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

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

2.1 控制參數(shù)化

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

圖5 時(shí)間尺度變換Fig. 5 Time scale transformation

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

圖6 控制參數(shù)化Fig. 6 Control parameterization

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

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

2.2 基于精確罰函數(shù)的約束處理

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

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

對(duì)凸四邊形包絡(luò)面而言，當(dāng)無(wú)人艇違反凸四邊形包絡(luò)面的約束時(shí)，必將同時(shí)違反凸四邊形包絡(luò)面4 條邊的約束，經(jīng)過4 次累乘后，其懲罰值將遠(yuǎn)大于違反圓形包絡(luò)面的懲罰值。因此，本文采用了縮放因子 κ來(lái)對(duì)凸四邊形包絡(luò)面約束進(jìn)行縮放處理。

在優(yōu)化初期，由于存在大量違反約束的情況，為了放寬對(duì)約束的懲罰程度，可以增加 ?以增加懲罰因子，從而減小懲罰權(quán)重并增加松弛因子；隨著優(yōu)化的進(jìn)行，違反約束的情況將逐漸減少，可以減小 ?來(lái)減小懲罰因子，從而增加懲罰權(quán)重并減小松弛因子，最終進(jìn)一步減少違反約束的情況。當(dāng) ?趨近0 時(shí)，懲罰權(quán)重將趨于無(wú)窮大，松弛因子趨于0，懲罰因子趨于0，此時(shí)，即可認(rèn)為優(yōu)化結(jié)束后的目標(biāo)函數(shù)J完全滿足了約束條件。

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

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

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

3 數(shù)值仿真

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

表1 仿真數(shù)據(jù)Table 1 Simulated data

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

圖7 無(wú)人艇的航跡圖Fig. 7 Track map of USV

圖8 無(wú)人艇位置隨時(shí)間的變化圖Fig. 8 Diagram of USV position changing with time

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

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

圖10 輸入控制信號(hào)Fig. 10 Input control signal

4 結(jié) 語(yǔ)

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