基于改進人工勢場法的船舶路徑規劃與跟蹤控制

寧君， 馬昊冉， 李鐵山

(1.大連海事大學 航海學院，遼寧 大連 116026； 2.電子科技大學 自動化工程學院，四川 成都 611730)

近年來，隨著物聯網、大數據、云計算等技術的快速發展，智能航海受到廣泛關注。面對復雜的海洋通航環境，在滿足海上航行安全，保證經濟性的前提下，如何使船舶在航行海域短時間內快速地規劃出一條由起始點到終點并可以安全避開所有障礙物的路徑最短航線，并使船舶能夠達到良好的路徑跟蹤效果，成為學者研究的熱點問題[1]。

在關于船舶路徑規劃的研究中，關于人工勢場法，劉琨等[2]提出用指數函數代替二次函數構造勢場函數并在斥力勢場函數中增加無人船與目標點的相對位置的一個因子從而解決目標不可達問題；陳鵬[3]在斥力函數中引入了碰撞危險度因子，同時在引力函數中加入了原航線對本船的引力，實現了對靜態障礙物進行避碰仿真實驗；張鳴鳴[4]提出一種“人工干預船舶航向”的方法來解決局部極小值問題。Ge等[5]構造了新的人工勢場法勢場函數以解決靜態情況下船舶避障出現目標不可達的問題。Song等[6]將速度障礙算法與人工勢場法相結合，引入了一種非緊急情況下的2級動態避障算法；通過結合粒子群優化算法提高計算效率以及計算精度，以此來解決欠驅動無人水面船舶 (under-actuated unmanned surface vehicles, USV)動態環境下緊急情況的避碰問題。Yu等[7]利用網格法建立未知區域的地表水環境模型，再將改進斥力函數的人工勢場法與方向隨機逃脫方法相結合，再結合A*算法解決了無人巡航船上濾波器出現的獲取不可達目標以及易陷入局部極小的問題。

在關于船舶路徑跟蹤控制的研究中，制導算法一直都是船舶運動控制的重要研究模塊，視線制導方法(line of sight，LOS)是一種應用廣泛的船舶制導算法，其主要應用在船舶直線路徑跟蹤中[8]。Healey等[9]首先利用LOS制導律對船舶進行控制，Fossen等[10,14-15]將自適應理論與LOS制導律結合，將漂角看作常量引入到LOS制導律中，有效地補償了由于海洋環境引起的漂角，增強了控制器的魯棒性。朱騁等[11]提出了一種基于Mamdani模型的自適應模糊LOS控制策略，該策略通過模糊控制修正LOS控制參數，改進了傳統LOS算法各項參數固定，不能實時調整的特點。Qiu等[12]采用了軌跡線性化控制技術及有限時間干擾觀測器，實現了期望的路徑跟蹤。沈智鵬等[13]針對建模不確定項以及內外擾動的欠驅動船舶路徑跟蹤控制問題，提出一種基于Lyapunov穩定性的神經元自適應迭代滑?？刂品椒?，解決了復雜海洋環境干擾下船舶的精確路徑跟蹤問題。

在關于船舶路徑規劃與控制的研究中，Huynh等[16]提出了使用最優遞推法對并聯機器人進行路徑規劃，使用動態濾波法對機器人進行路徑跟蹤，保證被控對象運動距離最短，避免出現奇異點。Precup等[17]使用灰狼優化算法(grey wolf optimizer，GWO)對被控對象進行路徑規劃，使用比例積分模糊控制器進行路徑跟蹤，實驗證明算法的有效性和性能顯著提高。Ji等[18]將三角函數和障礙物指數函數疊加構建了三維虛擬危險勢場，此方法適用于多種駕駛場合并具有良好的動態跟蹤性和機動性。在船舶領域，Shen等[19]結合樣條路徑模板對船舶進行路徑規劃，利用非線性模型預測控制技術進行船舶路徑跟蹤，提高了算法的抗干擾性和運行效率。張力行[20]將A*算法與COLREGs相結合對欠驅動船舶進行路徑規劃，然后將LOS與神經網絡控制器結合解決了欠驅動船舶路徑跟蹤的問題，這種方法在多種海洋環境的仿真實驗中行之有效。

船舶路徑規劃結果最后必須通過船舶跟蹤控制來實現。當前有關此方面研究較少，船舶路徑規劃及跟蹤控制中還存在著算法完善性[21-22]、路徑不夠平滑[23]和路徑跟蹤信號為虛擬信號[24-25]等問題?；谝陨涎芯糠治?，本文提出了一種基于改進人工勢場法的船舶路徑規劃與跟蹤控制方法，主要工作包括：

1)采用模擬退火算法優化傳統人工勢場法的斥力函數，既有效解決了傳統人工勢場法目標不可達和容易陷入局部極小值的缺陷，也解決了現有改進人工勢場法只能優化傳統人工勢場法目標不可達或局部極小值問題。

2)在此基礎上，對改進人工勢場法規劃出的路徑進行曲線擬合，同時考慮船舶運動學及動力學特點，將擬合后的平滑路線作為參考輸入信號進行路徑跟蹤，基于工業界廣為使用的內外環控制思想，設計運動學跟蹤引導率，實現了對參考信號的有效跟蹤，同時解決了船舶的欠驅動問題。

3)使用神經網絡對動力學子系統的不確定項進行逼近，實現了對內外擾動及未建模動態的補償，有效跟蹤了運動學控制子系統給出的期望速度，同時解決了傳統人工勢場法在進行路徑規劃時不能統籌考慮外界通航環境干擾的缺陷，實現了欠驅動船舶的高精度路徑規劃及跟蹤控制。

1 改進人工勢場法的船舶路徑規劃

1.1 人工勢場法

船舶領域中，人工勢場法(artificial potential field, APF)是將本船的航行環境抽象為勢力場，將本船視為移動物，所有的障礙物(暗礁、島嶼、浮標、其他船舶等)視為具有最高勢能的地方，目標點視為具有最低勢能的地方，這樣本船在障礙物產生的斥力以及目標點產生的引力共同作用下進行航行，逐漸地朝著勢場中最低勢能的地方也就是目標點航行。

設本船所在位置為(x,y)，X為本船位置向量，Xd為目標點位置向量，則本船在勢場中受到目標點的引力為Fatt(X)，所受到的障礙物的斥力為Frep(X)，引力與斥力的合力為Ftotal(X)，其方向為本船應該改變的目標航向。隨著本船的位置不斷變化，合力Ftotal(X)的方向也隨之變化，只要本船的航向隨著合力的方向而改變，本船就可以在航行中避開障礙物到達目標點。

根據人工勢場法原理可知，目標點對本船產生的引力Fatt(X)會隨著本船與目標點間的距離的增加而單調遞增，即本船距離目標點越遠，目標點產生的引力越大；反之當本船距離目標點越近，目標點產生的引力越小，而當本船到達目標點時，此時目標點產生的引力為零。所以，本船的引力勢場函數為：

(1)

式中：Ka為引力勢場常量；(X-Xd)2為本船到目標點的相對距離：

(X-Xd)n=|(x-xd)n|+|(y-yd)n|

(2)

本船所受引力可以表示為引力勢場函數的負梯度：

Fatt(X)=-grad(Uatt(X))=-Ka(X-Xd)=

-Ka[|(x-xd)n|+|(y-yd)n|]

(3)

式中引力Fatt(X)的方向為由本船指向目標點。

在勢場中，為了避免本船與障礙物發生碰撞，障礙物會對本船產生一定的斥力Frep(X)，且斥力的大小會隨著本船與障礙物間距離的變大而單調遞減，即本船距離障礙物越近，障礙物產生的斥力越大；而當本船距離障礙物越遠，障礙物所產生的斥力越??；而當這個距離無限大的時候即超出了障礙物的斥力影響范圍，本船將不會再受到障礙物產生的斥力影響，即可以認定此時障礙物產生的斥力為0。所以在斥力勢場部分需要分為2種情況計算。

假定以障礙物為中心的斥力勢場影響范圍為ρ0，本船與障礙物的距離為ρ，則當ρ≤ρ0時，本船與障礙物的距離與所受斥力成線性關系；當ρ>ρ0時，由于本船與障礙物距離ρ超出障礙物斥力場影響范圍，所以不再受斥力場的影響，本船所受斥力將變為0。所以，本船的斥力勢場函數為：

(4)

式中：kr為斥力勢場常數；ρ(X,Xg)為本船到障礙物的歐幾里得距離。

本船所受斥力可以表示為斥力勢場函數的負梯度：

Frep(X)=-grad[Urep(X)]=

(5)

本船受到的合力為目標點產生的引力以及所有障礙物產生的斥力的矢量和：

(6)

式中n為本船所在位置受斥力影響的障礙物個數。

本船運動方向為所受的合力方向，在合力的控制下，本船在航行時會避開障礙物并向目標點不斷靠近。當本船不斷向目標點航行時，本船所受到目標點產生的引力將會成線性減小，當引力減小為0時，意味著本船到達目標點[26]。

1.2 模擬退火算法

模擬退火算法(simulated annealing algorithm, SA)來源于固體退火原理，主要受物理學中固體物質在融化狀態下冷卻至結晶狀態的退火過程啟發而提出的一種隨機搜索算法。其退火過程主要包含加熱升溫、恒溫過程和冷卻過程。

模擬退火算法是以隨機過程模仿固體物理冷卻過程的一種算法，在這個過程中，固體內部的粒子即對應著模擬退火算法中的解，內能即是解的目標值，通過控制溫度參數調整其他參數，使目標函數得到全局最小值也就是最優解。流程如下：

1)給定一個初始溫度T0，隨機給定初始狀態X以及一個合適的退火速度θ，令T=T0，確定每個θ時的迭代次數，即Metropolis鏈長L；

2)令Xi=X+ΔX(ΔX為一個很小的均勻分布的隨機擾動)。并計算新狀態下勢場強度與初始狀態勢場強度之差，即ΔU=U(Xi)-U(X)；

4)重復循環2)、3)直到系統達到平衡狀態(判斷逃離局部極小值后則終止)。

T(t)=AT(t-1)

式中：A為小于1但接近于1的數，具體參考0.85≤A≤1；t為時間變量。

選擇銷子的下表面為研究面，在摩擦實驗前后，使用SEM S3700進行電鏡掃描，觀察銷子下表面的微觀結構的變化。使用EDX對銷子下表面的化學組成進行測量，對比摩擦實驗前后化學組成的變化。探究高溫摩擦實驗對樣品鋼渣表面和化學組成的影響。

1.3 改進人工勢場法

人工勢場法其不足之處在于當目標點靠近障礙物的時候,船舶將無法到達目標點,并存在局部穩定點,容易產生死鎖現象,因而可能使船舶到達目標點之前就停留在局部最優點上[27]。

在眾多有關人工勢場法的研究中，目標點與障礙物距離相對較遠，當船舶靠近目標點時，障礙物對船舶產生的斥力變得很小甚至可以忽略不計，船舶將只受到來自目標點的引力進而到達目標點。但在復雜的航行環境下，基本都會存在障礙物在目標點附近，這種情況下，船舶靠近目標點的同時也在向障礙物靠近，按照傳統人工勢場法中定義的引力函數和斥力函數，此時障礙物產生的斥力將遠遠大于船舶受到的引力，進而船舶將不會到達目標點。為解決目標不可達的問題，需要重新定義一個斥力函數，這個函數需要滿足當船舶向目標點航行并靠近時，船舶受到的斥力盡可能地小至趨于零，那么目標點所在的位置將是整個勢場的全局最小點，從而解決目標不可達的問題。新的勢場函數中考慮了船舶與目標點之間的距離，在原有的斥力函數中添加一個系數項θ，改進以后的斥力函數為：

(7)

式中：Urep(X)為障礙物的斥力場；kr則表示斥力勢場常數，是正數；ρ為船舶在空間位置中與障礙物間距離；ρ0為以障礙物為中心的斥力勢場影響范圍；X為船舶位置向量；Xd為目標點位置向量；ρ(X,Xg)表示船舶到障礙物的歐幾里得距離；n為一個大于零的任意實數。其中，船舶與目標點間的相對距離|X-Xd|n=|(x-xd)n|+|(y-yd)|n，在原有的斥力函數中引入了船舶與目標點之間的相對距離，保障了目標點在整個勢場中處于全局最小的位置。

船舶在航行過程中，在某一位置，船舶所受到的引力與斥力大小相等方向相反，處于受力平衡狀態，達到穩定。從而使得該位置成為勢場的全局最小點，導致船舶不能到達目標點。在復雜的航行環境中，可能存在多個局部極小值點。

針對勢場中可能存在多個局部極小值點的問題，人工勢場法中加入模擬退火算法，主要是利用模擬退火算法的突跳性對模型進行局部最優化。在程序中，給定一個初始溫度以及一個初始狀態，并給定合適的退火速度。隨著本船的行進，初始溫度也慢慢降低，在溫度降低的過程中，模擬退火算法會對每一個狀態下的勢場強度進行計算，同時，會在每一個狀態下給定一個隨機擾動，并計算新狀態下勢場強度與初始狀態勢場強度之差，并對這個差值進行分析。按照Metropolis準則，若差值小于等于0，則程序會接受這個新狀態；若差值大于0，則以一定的概率接受新狀態，反之會保留原初始狀態。如此往復，就可以避免本船在行進中產生局部極小值的情況，保證了程序的有效性以及可行性。

改進的人工勢場法首先要改進原有的傳統人工勢場法斥力函數，在斥力函數中增加有關船舶與目標點之間的相對距離參數，將改進后的人工勢場法與模擬退火算法相結合，得到最新的改進算法，改進后的算法流程圖1所示。

圖1 改進人工勢場法流程

2 船舶路徑跟蹤控制

2.1 船舶的滑模路徑跟蹤控制

在研究船舶的路徑規劃及跟蹤控制時，考慮船舶在6個自由度方向的運動會導致參數的增多，不僅會對仿真效果產生影響，而且還會增加運算量[28]。因此，本文只考慮船舶橫蕩、縱蕩和艏搖。

(8)

(9)

為了實現x跟蹤xd，y跟蹤yd，首先要設計控制律U。設置理想的跟蹤軌跡為(xd,yd)，則跟蹤誤差方程為：

(10)

式中：xe為船舶所在位置橫坐標與跟蹤軌跡橫坐標的誤差，則xe=x-xd；ye為船舶所在位置縱坐標與跟蹤軌跡縱坐標的誤差，則ye=y-yd。為了方便計算，可以?。?/p>

(11)

(12)

式中k1>0。

u2=yd-k2s2

(13)

式中k2>0。

(14)

設理想軌跡的船舶航向角為θd。如果θ=θd，則理想的軌跡控制律式(12)和式(13)可以實現，但是在實際模型式(9)中，初始階段的θ和θd不可能完全一致，這樣就會導致跟蹤系統式(10)的不穩定。因此，需要將式(14)求得的船舶航向角當成理想值：

(15)

綜上所述，得到實際的位置控制律為：

(16)

當θ與θd有誤差時，會造成控制律無法精確實現，只有通過更快的收斂算法，使θ快速跟蹤θd，才能解決這個問題。通過設計姿態控制律θ，實現θ跟蹤θd。取θe=θ-θd，滑模函數為s3=θe，得到姿態控制律為：

ω=θd-k3s3-η3sgns3

(17)

式中k3、η3為函數系數，且k3>0，η3>0。

本文的運動學控制子系統基于內外環控制思想，位置子系統為外環系統，姿態子系統為內環系統，外環產生指令信號θd后，傳遞給內環系統，內環系統則通過滑?？刂坡蓪崿F對中間指令信號的追蹤。為了實現穩定的內環滑?？刂?，本節采用的是工程上一般采用的方法，即內環收斂速度大于外環收斂速度的方法,通過θ快速跟蹤θd，保證閉環系統的穩定性[24]。在本算法中通過調整內外環控制增益系數，保證內環收斂速度遠大于外環收斂速度。具有內外雙環的閉環系統如圖2所示。

圖2 具有雙環的閉環系統結構

(18)

式中：R為微分器參數；U(t)為待微分的輸入信號；x1為信號追蹤；x2為信號一階導數的估計；微分器初始值x1(0)=0，x2(0)=0。由于該微分器具有積分鏈式結構，本文對含有噪聲的信號求導時，噪聲只含在微分器的最后一層，通過積分作用信號一階導數中的噪聲能夠被更充分地抑制。

2.2 基于神經網絡船舶動力學控制

本節將2.1節的設計拓展到動力學控制，動力學控制器的目標是跟蹤制導律給定的參考速度。船舶動力模型表示為[25]：

(19)

由于u=Ucosθ，則可以得到U和r的動態方程為：

(20)

動力學控制器的控制目的是讓船舶跟蹤期望前向速度與期望角速度:

(21)

式中δ1與δ2為較小的正數。

采用神經網絡估計動力學模型不確定性與外部海洋環境的擾動，有界控制律給出控制力與力矩。式(20)可轉化為一階非線性控制系統:

(22)

muUsinθr)

(23)

(24)

(25)

式中c>0。則可以得到：

(26)

式中F(x)=f(x)+d(t)。

使用RBF神經網絡逼近f(x)，網絡算法為：

(27)

f=W*Th(x)+ε

(28)

(29)

(30)

定義Lyapunov函數為:

(31)

(32)

設計控制律為:

(33)

式中ηsgn(s)為魯棒項，則：

(34)

取η>|ε|max，自適應律為:

(35)

3 實驗仿真與分析

為了驗證模擬退火算法改進的人工勢場法進行船舶路徑規劃的有效性及船舶對期望航線的路徑跟蹤和控制器的有效性，本文采用Matlab/Simulink進行路徑規劃及跟蹤控制仿真。

首先，構造具有障礙物的環境圖，并對傳統人工勢場法和改進人工勢場法進行分析對比，證明改進人工勢場法對船舶路徑規劃的有效性。建立環境地圖長和寬均為[-2,12]的正方形，在環境地圖中隨機建立n個黑色圓形障礙物，障礙物的圓心用一個n×2的向量表示，障礙物的半徑為0.5。建立歐氏坐標系，設置船舶起始點的位置為(0，0)，目標點的位置為(10，10)。在對比實驗中，傳統人工勢場法與改進人工勢場法采用相同的參數。設置人工勢場法的步長為0.1，引力增益系數為5，斥力增益系數為15，障礙影響距離為2。模擬退火算法中的參數設置為初始溫度，終止溫度T0=0，循環次數為100次。

圖3為簡單障礙物環境下，傳統人工勢場法與采用模擬退火算法改進的人工勢場法路徑規劃的實驗結果；圖4為較復雜障礙物環境下，傳統人工勢場法與采用模擬退火算法改進的人工勢場法路徑規劃的實驗結果。

圖3 簡單障礙物下傳統與改進人工勢場法路徑規劃

圖4 復雜障礙物下傳統與改進人工勢場法路徑規劃

通過圖3～4結果可以看出，在簡單障礙物條件下，傳統人工勢場法與改進人工勢場法都可規劃出一條由起點到終點的有效路徑，但改進的人工勢場法路徑更平滑，且具有可行性；在較為復雜的障礙物條件下，傳統人工勢場法的路徑規劃出現了目標不可達的缺陷，而采用改進斥力函數并采用模擬退火算法改進的人工勢場法克服了傳統人工勢場法的缺陷，對于較復雜的路徑規劃更加有效。

其次，對規劃出的最優路徑進行平滑處理，并作為輸入信號進行路徑跟蹤。為了便于分析，以下實驗采用圖3(b)的簡單障礙物下改進人工勢場法進行實驗。使用Matlab中ployfit函數對最優路徑進行曲線擬合，擬合函數為：

y=a1xn+a2xn-1+a3xn-2+…+anx+an-1

(36)

式(36)為一個多項式，在擬合過程中，需要對不同的多項式次數進行擬合。擬合次數少，路徑擬合效果比較差，擬合次數過多，會出現龍格現象，只有當誤差最小時，才會出現較為理想的擬合效果。本文擬合函數為10次多項式，擬合效果如圖5所示，圖中藍色圓圈為實際路徑，紅色曲線為擬合曲線。

圖5 改進人工勢場法路徑曲線擬合

然后，對擬合出來的曲線進行路徑跟蹤?？刂茖ο鬄槭?8)，位置指令[xd,yd]為xd=t，yd為擬合多項式函數。參數設置為k1=k2=0.3，k3=3，η3=0.50，位置初始值為[0,0,0]，采用控制律式(18)和式(20)，針對姿態控制律式(20)的切換項，采用飽和函數代替切換函數,邊界層厚度取0.1，微分器參數取xd=[Ud,rd]，仿真結果如圖6～8所示。

圖6 實際海況船舶路徑跟蹤航線

從圖6～8中可以看出，船舶在跟蹤路徑時，位置和航向角與規劃航線沒有較大偏差，船舶在具有雙環結構的閉環控制系統控制下，可以完美地跟蹤路徑，完成從起始點到目標點的跟蹤任務。并且θd在區間(-π/2,π/2)內，滿足式(17)的要求，具有良好的跟蹤效果。

圖7 位置和角度的跟蹤曲線

圖8 輸入信號變化曲線

最后，因為實驗中沒有考慮船舶動力學控制問題，因此設計基于徑向基神經網絡的船舶動力學控制器，跟蹤運動學制導律給出的期望速度，解決在真實的海洋環境航行時，船舶容易受到海洋環境風浪流的干擾，系統存在不確定性，對船舶航行造成影響的問題。設計控制指令為xd=[Ud,rd]，控制律采用式(33)，自適應律采用式(35)，取γ=50，η=0.5。根據網絡輸入x1和x2的實際范圍來設計高斯基函數的參數，參數c和b的取值分別為[-2,-1,0,1,2]和3。網絡權值中各個元素的初始值取0.1，仿真結果如圖9和圖10所示。

圖9 動力學控制力矩

圖10 動力學系統控制力

仿真結果分析可得，通過合理設計運動學內外雙環的控制增益，在保證船舶航向收斂速度快于合速度收斂速度的前提下，運動學子系統能夠快速跟蹤擬合后的期望路徑。動力學子系統能夠快速跟蹤運動學子系統給出的引導率，實現基于改進人工勢場算法的船舶路徑規劃及跟蹤控制。

4 結論

1)本文基于模擬退火算法對傳統的人工勢場法進行優化，解決了傳統人工勢場法存在的目標不可達以及易陷入局部最小值的問題。

2)將改進人工勢場法的規劃航線作為參考跟蹤路徑，通過多項式擬合，使其作為船舶運動學控制子系統的參考輸入信號。

3)基于工業界廣泛使用的內外雙環控制方法，分別設計了船舶運動學控制律，即合速度引導率，轉艏角速度引導率。

4)考慮船舶動力學子系統的特性，研究了船舶動力學控制問題。通過引入神經網絡對船舶動力學子系統的未知不確定項進行逼近，設計了一種基于徑向基神經網絡算法的滑?？刂破?，實現了對內外擾動及未建模動態的補償，同時解決了傳統人工勢場法在進行路徑規劃時不能統籌考慮外界通航環境干擾的缺陷。

5)通過仿真實驗，驗證了改進人工勢場法路徑規劃的有效性和船舶對期望航線的路徑跟蹤的可行性，實現了對運動學控制子系統引導率的跟蹤控制，進而實現了船舶的路徑規劃與跟蹤，為船舶路徑規劃與跟蹤控制的結合提供了較好的方法。

本文只考慮了單船的路徑規劃與跟蹤控制，對于多船編隊路徑規劃與跟蹤控制方面還有待研究。