基于改進(jìn)人工勢(shì)場(chǎng)法的船舶路徑規(guī)劃與跟蹤控制

寧君， 馬昊冉， 李鐵山

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

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

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

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

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

船舶路徑規(guī)劃結(jié)果最后必須通過(guò)船舶跟蹤控制來(lái)實(shí)現(xiàn)。當(dāng)前有關(guān)此方面研究較少，船舶路徑規(guī)劃及跟蹤控制中還存在著算法完善性[21-22]、路徑不夠平滑[23]和路徑跟蹤信號(hào)為虛擬信號(hào)[24-25]等問(wèn)題。基于以上研究分析，本文提出了一種基于改進(jìn)人工勢(shì)場(chǎng)法的船舶路徑規(guī)劃與跟蹤控制方法，主要工作包括：

1)采用模擬退火算法優(yōu)化傳統(tǒng)人工勢(shì)場(chǎng)法的斥力函數(shù)，既有效解決了傳統(tǒng)人工勢(shì)場(chǎng)法目標(biāo)不可達(dá)和容易陷入局部極小值的缺陷，也解決了現(xiàn)有改進(jìn)人工勢(shì)場(chǎng)法只能優(yōu)化傳統(tǒng)人工勢(shì)場(chǎng)法目標(biāo)不可達(dá)或局部極小值問(wèn)題。

2)在此基礎(chǔ)上，對(duì)改進(jìn)人工勢(shì)場(chǎng)法規(guī)劃出的路徑進(jìn)行曲線擬合，同時(shí)考慮船舶運(yùn)動(dòng)學(xué)及動(dòng)力學(xué)特點(diǎn)，將擬合后的平滑路線作為參考輸入信號(hào)進(jìn)行路徑跟蹤，基于工業(yè)界廣為使用的內(nèi)外環(huán)控制思想，設(shè)計(jì)運(yùn)動(dòng)學(xué)跟蹤引導(dǎo)率，實(shí)現(xiàn)了對(duì)參考信號(hào)的有效跟蹤，同時(shí)解決了船舶的欠驅(qū)動(dòng)問(wèn)題。

3)使用神經(jīng)網(wǎng)絡(luò)對(duì)動(dòng)力學(xué)子系統(tǒng)的不確定項(xiàng)進(jìn)行逼近，實(shí)現(xiàn)了對(duì)內(nèi)外擾動(dòng)及未建模動(dòng)態(tài)的補(bǔ)償，有效跟蹤了運(yùn)動(dòng)學(xué)控制子系統(tǒng)給出的期望速度，同時(shí)解決了傳統(tǒng)人工勢(shì)場(chǎng)法在進(jìn)行路徑規(guī)劃時(shí)不能統(tǒng)籌考慮外界通航環(huán)境干擾的缺陷，實(shí)現(xiàn)了欠驅(qū)動(dòng)船舶的高精度路徑規(guī)劃及跟蹤控制。

1 改進(jìn)人工勢(shì)場(chǎng)法的船舶路徑規(guī)劃

1.1 人工勢(shì)場(chǎng)法

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

設(shè)本船所在位置為(x,y)，X為本船位置向量，Xd為目標(biāo)點(diǎn)位置向量，則本船在勢(shì)場(chǎng)中受到目標(biāo)點(diǎn)的引力為Fatt(X)，所受到的障礙物的斥力為Frep(X)，引力與斥力的合力為Ftotal(X)，其方向?yàn)楸敬瑧?yīng)該改變的目標(biāo)航向。隨著本船的位置不斷變化，合力Ftotal(X)的方向也隨之變化，只要本船的航向隨著合力的方向而改變，本船就可以在航行中避開(kāi)障礙物到達(dá)目標(biāo)點(diǎn)。

根據(jù)人工勢(shì)場(chǎng)法原理可知，目標(biāo)點(diǎn)對(duì)本船產(chǎn)生的引力Fatt(X)會(huì)隨著本船與目標(biāo)點(diǎn)間的距離的增加而單調(diào)遞增，即本船距離目標(biāo)點(diǎn)越遠(yuǎn)，目標(biāo)點(diǎn)產(chǎn)生的引力越大；反之當(dāng)本船距離目標(biāo)點(diǎn)越近，目標(biāo)點(diǎn)產(chǎn)生的引力越小，而當(dāng)本船到達(dá)目標(biāo)點(diǎn)時(shí)，此時(shí)目標(biāo)點(diǎn)產(chǎn)生的引力為零。所以，本船的引力勢(shì)場(chǎng)函數(shù)為：

(1)

式中：Ka為引力勢(shì)場(chǎng)常量；(X-Xd)2為本船到目標(biāo)點(diǎn)的相對(duì)距離：

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

(2)

本船所受引力可以表示為引力勢(shì)場(chǎng)函數(shù)的負(fù)梯度：

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

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

(3)

式中引力Fatt(X)的方向?yàn)橛杀敬赶蚰繕?biāo)點(diǎn)。

在勢(shì)場(chǎng)中，為了避免本船與障礙物發(fā)生碰撞，障礙物會(huì)對(duì)本船產(chǎn)生一定的斥力Frep(X)，且斥力的大小會(huì)隨著本船與障礙物間距離的變大而單調(diào)遞減，即本船距離障礙物越近，障礙物產(chǎn)生的斥力越大；而當(dāng)本船距離障礙物越遠(yuǎn)，障礙物所產(chǎn)生的斥力越?。欢?dāng)這個(gè)距離無(wú)限大的時(shí)候即超出了障礙物的斥力影響范圍，本船將不會(huì)再受到障礙物產(chǎn)生的斥力影響，即可以認(rèn)定此時(shí)障礙物產(chǎn)生的斥力為0。所以在斥力勢(shì)場(chǎng)部分需要分為2種情況計(jì)算。

假定以障礙物為中心的斥力勢(shì)場(chǎng)影響范圍為ρ0，本船與障礙物的距離為ρ，則當(dāng)ρ≤ρ0時(shí)，本船與障礙物的距離與所受斥力成線性關(guān)系；當(dāng)ρ>ρ0時(shí)，由于本船與障礙物距離ρ超出障礙物斥力場(chǎng)影響范圍，所以不再受斥力場(chǎng)的影響，本船所受斥力將變?yōu)?。所以，本船的斥力勢(shì)場(chǎng)函數(shù)為：

(4)

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

本船所受斥力可以表示為斥力勢(shì)場(chǎng)函數(shù)的負(fù)梯度：

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

(5)

本船受到的合力為目標(biāo)點(diǎn)產(chǎn)生的引力以及所有障礙物產(chǎn)生的斥力的矢量和：

(6)

式中n為本船所在位置受斥力影響的障礙物個(gè)數(shù)。

本船運(yùn)動(dòng)方向?yàn)樗艿暮狭Ψ较?，在合力的控制下，本船在航行時(shí)會(huì)避開(kāi)障礙物并向目標(biāo)點(diǎn)不斷靠近。當(dāng)本船不斷向目標(biāo)點(diǎn)航行時(shí)，本船所受到目標(biāo)點(diǎn)產(chǎn)生的引力將會(huì)成線性減小，當(dāng)引力減小為0時(shí)，意味著本船到達(dá)目標(biāo)點(diǎn)[26]。

1.2 模擬退火算法

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

模擬退火算法是以隨機(jī)過(guò)程模仿固體物理冷卻過(guò)程的一種算法，在這個(gè)過(guò)程中，固體內(nèi)部的粒子即對(duì)應(yīng)著模擬退火算法中的解，內(nèi)能即是解的目標(biāo)值，通過(guò)控制溫度參數(shù)調(diào)整其他參數(shù)，使目標(biāo)函數(shù)得到全局最小值也就是最優(yōu)解。流程如下：

1)給定一個(gè)初始溫度T0，隨機(jī)給定初始狀態(tài)X以及一個(gè)合適的退火速度θ，令T=T0，確定每個(gè)θ時(shí)的迭代次數(shù)，即Metropolis鏈長(zhǎng)L；

2)令Xi=X+ΔX(ΔX為一個(gè)很小的均勻分布的隨機(jī)擾動(dòng))。并計(jì)算新?tīng)顟B(tài)下勢(shì)場(chǎng)強(qiáng)度與初始狀態(tài)勢(shì)場(chǎng)強(qiáng)度之差，即ΔU=U(Xi)-U(X)；

4)重復(fù)循環(huán)2)、3)直到系統(tǒng)達(dá)到平衡狀態(tài)(判斷逃離局部極小值后則終止)。

T(t)=AT(t-1)

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

選擇銷(xiāo)子的下表面為研究面，在摩擦實(shí)驗(yàn)前后，使用SEM S3700進(jìn)行電鏡掃描，觀察銷(xiāo)子下表面的微觀結(jié)構(gòu)的變化。使用EDX對(duì)銷(xiāo)子下表面的化學(xué)組成進(jìn)行測(cè)量，對(duì)比摩擦實(shí)驗(yàn)前后化學(xué)組成的變化。探究高溫摩擦實(shí)驗(yàn)對(duì)樣品鋼渣表面和化學(xué)組成的影響。

1.3 改進(jìn)人工勢(shì)場(chǎng)法

人工勢(shì)場(chǎng)法其不足之處在于當(dāng)目標(biāo)點(diǎn)靠近障礙物的時(shí)候,船舶將無(wú)法到達(dá)目標(biāo)點(diǎn),并存在局部穩(wěn)定點(diǎn),容易產(chǎn)生死鎖現(xiàn)象,因而可能使船舶到達(dá)目標(biāo)點(diǎn)之前就停留在局部最優(yōu)點(diǎn)上[27]。

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

(7)

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

船舶在航行過(guò)程中，在某一位置，船舶所受到的引力與斥力大小相等方向相反，處于受力平衡狀態(tài)，達(dá)到穩(wěn)定。從而使得該位置成為勢(shì)場(chǎng)的全局最小點(diǎn)，導(dǎo)致船舶不能到達(dá)目標(biāo)點(diǎn)。在復(fù)雜的航行環(huán)境中，可能存在多個(gè)局部極小值點(diǎn)。

針對(duì)勢(shì)場(chǎng)中可能存在多個(gè)局部極小值點(diǎn)的問(wèn)題，人工勢(shì)場(chǎng)法中加入模擬退火算法，主要是利用模擬退火算法的突跳性對(duì)模型進(jìn)行局部最優(yōu)化。在程序中，給定一個(gè)初始溫度以及一個(gè)初始狀態(tài)，并給定合適的退火速度。隨著本船的行進(jìn)，初始溫度也慢慢降低，在溫度降低的過(guò)程中，模擬退火算法會(huì)對(duì)每一個(gè)狀態(tài)下的勢(shì)場(chǎng)強(qiáng)度進(jìn)行計(jì)算，同時(shí)，會(huì)在每一個(gè)狀態(tài)下給定一個(gè)隨機(jī)擾動(dòng)，并計(jì)算新?tīng)顟B(tài)下勢(shì)場(chǎng)強(qiáng)度與初始狀態(tài)勢(shì)場(chǎng)強(qiáng)度之差，并對(duì)這個(gè)差值進(jìn)行分析。按照Metropolis準(zhǔn)則，若差值小于等于0，則程序會(huì)接受這個(gè)新?tīng)顟B(tài)；若差值大于0，則以一定的概率接受新?tīng)顟B(tài)，反之會(huì)保留原初始狀態(tài)。如此往復(fù)，就可以避免本船在行進(jìn)中產(chǎn)生局部極小值的情況，保證了程序的有效性以及可行性。

改進(jìn)的人工勢(shì)場(chǎng)法首先要改進(jìn)原有的傳統(tǒng)人工勢(shì)場(chǎng)法斥力函數(shù)，在斥力函數(shù)中增加有關(guān)船舶與目標(biāo)點(diǎn)之間的相對(duì)距離參數(shù)，將改進(jìn)后的人工勢(shì)場(chǎng)法與模擬退火算法相結(jié)合，得到最新的改進(jìn)算法，改進(jìn)后的算法流程圖1所示。

圖1 改進(jìn)人工勢(shì)場(chǎng)法流程

2 船舶路徑跟蹤控制

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

在研究船舶的路徑規(guī)劃及跟蹤控制時(shí)，考慮船舶在6個(gè)自由度方向的運(yùn)動(dòng)會(huì)導(dǎo)致參數(shù)的增多，不僅會(huì)對(duì)仿真效果產(chǎn)生影響，而且還會(huì)增加運(yùn)算量[28]。因此，本文只考慮船舶橫蕩、縱蕩和艏搖。

(8)

(9)

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

(10)

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

(11)

(12)

式中k1>0。

u2=yd-k2s2

(13)

式中k2>0。

(14)

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

(15)

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

(16)

當(dāng)θ與θd有誤差時(shí)，會(huì)造成控制律無(wú)法精確實(shí)現(xiàn)，只有通過(guò)更快的收斂算法，使θ快速跟蹤θd，才能解決這個(gè)問(wèn)題。通過(guò)設(shè)計(jì)姿態(tài)控制律θ，實(shí)現(xiàn)θ跟蹤θd。取θe=θ-θd，滑模函數(shù)為s3=θe，得到姿態(tài)控制律為：

ω=θd-k3s3-η3sgns3

(17)

式中k3、η3為函數(shù)系數(shù)，且k3>0，η3>0。

本文的運(yùn)動(dòng)學(xué)控制子系統(tǒng)基于內(nèi)外環(huán)控制思想，位置子系統(tǒng)為外環(huán)系統(tǒng)，姿態(tài)子系統(tǒng)為內(nèi)環(huán)系統(tǒng)，外環(huán)產(chǎn)生指令信號(hào)θd后，傳遞給內(nèi)環(huán)系統(tǒng)，內(nèi)環(huán)系統(tǒng)則通過(guò)滑?？刂坡蓪?shí)現(xiàn)對(duì)中間指令信號(hào)的追蹤。為了實(shí)現(xiàn)穩(wěn)定的內(nèi)環(huán)滑模控制，本節(jié)采用的是工程上一般采用的方法，即內(nèi)環(huán)收斂速度大于外環(huán)收斂速度的方法,通過(guò)θ快速跟蹤θd，保證閉環(huán)系統(tǒng)的穩(wěn)定性[24]。在本算法中通過(guò)調(diào)整內(nèi)外環(huán)控制增益系數(shù)，保證內(nèi)環(huán)收斂速度遠(yuǎn)大于外環(huán)收斂速度。具有內(nèi)外雙環(huán)的閉環(huán)系統(tǒng)如圖2所示。

圖2 具有雙環(huán)的閉環(huán)系統(tǒng)結(jié)構(gòu)

(18)

式中：R為微分器參數(shù)；U(t)為待微分的輸入信號(hào)；x1為信號(hào)追蹤；x2為信號(hào)一階導(dǎo)數(shù)的估計(jì)；微分器初始值x1(0)=0，x2(0)=0。由于該微分器具有積分鏈?zhǔn)浇Y(jié)構(gòu)，本文對(duì)含有噪聲的信號(hào)求導(dǎo)時(shí)，噪聲只含在微分器的最后一層，通過(guò)積分作用信號(hào)一階導(dǎo)數(shù)中的噪聲能夠被更充分地抑制。

2.2 基于神經(jīng)網(wǎng)絡(luò)船舶動(dòng)力學(xué)控制

本節(jié)將2.1節(jié)的設(shè)計(jì)拓展到動(dòng)力學(xué)控制，動(dòng)力學(xué)控制器的目標(biāo)是跟蹤制導(dǎo)律給定的參考速度。船舶動(dòng)力模型表示為[25]：

(19)

由于u=Ucosθ，則可以得到U和r的動(dòng)態(tài)方程為：

(20)

動(dòng)力學(xué)控制器的控制目的是讓船舶跟蹤期望前向速度與期望角速度:

(21)

式中δ1與δ2為較小的正數(shù)。

采用神經(jīng)網(wǎng)絡(luò)估計(jì)動(dòng)力學(xué)模型不確定性與外部海洋環(huán)境的擾動(dòng)，有界控制律給出控制力與力矩。式(20)可轉(zhuǎn)化為一階非線性控制系統(tǒng):

(22)

muUsinθr)

(23)

(24)

(25)

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

(26)

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

使用RBF神經(jīng)網(wǎng)絡(luò)逼近f(x)，網(wǎng)絡(luò)算法為：

(27)

f=W*Th(x)+ε

(28)

(29)

(30)

定義Lyapunov函數(shù)為:

(31)

(32)

設(shè)計(jì)控制律為:

(33)

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

(34)

取η>|ε|max，自適應(yīng)律為:

(35)

3 實(shí)驗(yàn)仿真與分析

為了驗(yàn)證模擬退火算法改進(jìn)的人工勢(shì)場(chǎng)法進(jìn)行船舶路徑規(guī)劃的有效性及船舶對(duì)期望航線的路徑跟蹤和控制器的有效性，本文采用Matlab/Simulink進(jìn)行路徑規(guī)劃及跟蹤控制仿真。

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

圖3為簡(jiǎn)單障礙物環(huán)境下，傳統(tǒng)人工勢(shì)場(chǎng)法與采用模擬退火算法改進(jìn)的人工勢(shì)場(chǎng)法路徑規(guī)劃的實(shí)驗(yàn)結(jié)果；圖4為較復(fù)雜障礙物環(huán)境下，傳統(tǒng)人工勢(shì)場(chǎng)法與采用模擬退火算法改進(jìn)的人工勢(shì)場(chǎng)法路徑規(guī)劃的實(shí)驗(yàn)結(jié)果。

圖3 簡(jiǎn)單障礙物下傳統(tǒng)與改進(jìn)人工勢(shì)場(chǎng)法路徑規(guī)劃

圖4 復(fù)雜障礙物下傳統(tǒng)與改進(jìn)人工勢(shì)場(chǎng)法路徑規(guī)劃

通過(guò)圖3～4結(jié)果可以看出，在簡(jiǎn)單障礙物條件下，傳統(tǒng)人工勢(shì)場(chǎng)法與改進(jìn)人工勢(shì)場(chǎng)法都可規(guī)劃出一條由起點(diǎn)到終點(diǎn)的有效路徑，但改進(jìn)的人工勢(shì)場(chǎng)法路徑更平滑，且具有可行性；在較為復(fù)雜的障礙物條件下，傳統(tǒng)人工勢(shì)場(chǎng)法的路徑規(guī)劃出現(xiàn)了目標(biāo)不可達(dá)的缺陷，而采用改進(jìn)斥力函數(shù)并采用模擬退火算法改進(jìn)的人工勢(shì)場(chǎng)法克服了傳統(tǒng)人工勢(shì)場(chǎng)法的缺陷，對(duì)于較復(fù)雜的路徑規(guī)劃更加有效。

其次，對(duì)規(guī)劃出的最優(yōu)路徑進(jìn)行平滑處理，并作為輸入信號(hào)進(jìn)行路徑跟蹤。為了便于分析，以下實(shí)驗(yàn)采用圖3(b)的簡(jiǎn)單障礙物下改進(jìn)人工勢(shì)場(chǎng)法進(jìn)行實(shí)驗(yàn)。使用Matlab中ployfit函數(shù)對(duì)最優(yōu)路徑進(jìn)行曲線擬合，擬合函數(shù)為：

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

(36)

式(36)為一個(gè)多項(xiàng)式，在擬合過(guò)程中，需要對(duì)不同的多項(xiàng)式次數(shù)進(jìn)行擬合。擬合次數(shù)少，路徑擬合效果比較差，擬合次數(shù)過(guò)多，會(huì)出現(xiàn)龍格現(xiàn)象，只有當(dāng)誤差最小時(shí)，才會(huì)出現(xiàn)較為理想的擬合效果。本文擬合函數(shù)為10次多項(xiàng)式，擬合效果如圖5所示，圖中藍(lán)色圓圈為實(shí)際路徑，紅色曲線為擬合曲線。

圖5 改進(jìn)人工勢(shì)場(chǎng)法路徑曲線擬合

然后，對(duì)擬合出來(lái)的曲線進(jìn)行路徑跟蹤。控制對(duì)象為式(8)，位置指令[xd,yd]為xd=t，yd為擬合多項(xiàng)式函數(shù)。參數(shù)設(shè)置為k1=k2=0.3，k3=3，η3=0.50，位置初始值為[0,0,0]，采用控制律式(18)和式(20)，針對(duì)姿態(tài)控制律式(20)的切換項(xiàng)，采用飽和函數(shù)代替切換函數(shù),邊界層厚度取0.1，微分器參數(shù)取xd=[Ud,rd]，仿真結(jié)果如圖6～8所示。

圖6 實(shí)際海況船舶路徑跟蹤航線

從圖6～8中可以看出，船舶在跟蹤路徑時(shí)，位置和航向角與規(guī)劃航線沒(méi)有較大偏差，船舶在具有雙環(huán)結(jié)構(gòu)的閉環(huán)控制系統(tǒng)控制下，可以完美地跟蹤路徑，完成從起始點(diǎn)到目標(biāo)點(diǎn)的跟蹤任務(wù)。并且θd在區(qū)間(-π/2,π/2)內(nèi)，滿(mǎn)足式(17)的要求，具有良好的跟蹤效果。

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

圖8 輸入信號(hào)變化曲線

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

圖9 動(dòng)力學(xué)控制力矩

圖10 動(dòng)力學(xué)系統(tǒng)控制力

仿真結(jié)果分析可得，通過(guò)合理設(shè)計(jì)運(yùn)動(dòng)學(xué)內(nèi)外雙環(huán)的控制增益，在保證船舶航向收斂速度快于合速度收斂速度的前提下，運(yùn)動(dòng)學(xué)子系統(tǒng)能夠快速跟蹤擬合后的期望路徑。動(dòng)力學(xué)子系統(tǒng)能夠快速跟蹤運(yùn)動(dòng)學(xué)子系統(tǒng)給出的引導(dǎo)率，實(shí)現(xiàn)基于改進(jìn)人工勢(shì)場(chǎng)算法的船舶路徑規(guī)劃及跟蹤控制。

4 結(jié)論

1)本文基于模擬退火算法對(duì)傳統(tǒng)的人工勢(shì)場(chǎng)法進(jìn)行優(yōu)化，解決了傳統(tǒng)人工勢(shì)場(chǎng)法存在的目標(biāo)不可達(dá)以及易陷入局部最小值的問(wèn)題。

2)將改進(jìn)人工勢(shì)場(chǎng)法的規(guī)劃航線作為參考跟蹤路徑，通過(guò)多項(xiàng)式擬合，使其作為船舶運(yùn)動(dòng)學(xué)控制子系統(tǒng)的參考輸入信號(hào)。

3)基于工業(yè)界廣泛使用的內(nèi)外雙環(huán)控制方法，分別設(shè)計(jì)了船舶運(yùn)動(dòng)學(xué)控制律，即合速度引導(dǎo)率，轉(zhuǎn)艏角速度引導(dǎo)率。

4)考慮船舶動(dòng)力學(xué)子系統(tǒng)的特性，研究了船舶動(dòng)力學(xué)控制問(wèn)題。通過(guò)引入神經(jīng)網(wǎng)絡(luò)對(duì)船舶動(dòng)力學(xué)子系統(tǒng)的未知不確定項(xiàng)進(jìn)行逼近，設(shè)計(jì)了一種基于徑向基神經(jīng)網(wǎng)絡(luò)算法的滑?？刂破?，實(shí)現(xiàn)了對(duì)內(nèi)外擾動(dòng)及未建模動(dòng)態(tài)的補(bǔ)償，同時(shí)解決了傳統(tǒng)人工勢(shì)場(chǎng)法在進(jìn)行路徑規(guī)劃時(shí)不能統(tǒng)籌考慮外界通航環(huán)境干擾的缺陷。

5)通過(guò)仿真實(shí)驗(yàn)，驗(yàn)證了改進(jìn)人工勢(shì)場(chǎng)法路徑規(guī)劃的有效性和船舶對(duì)期望航線的路徑跟蹤的可行性，實(shí)現(xiàn)了對(duì)運(yùn)動(dòng)學(xué)控制子系統(tǒng)引導(dǎo)率的跟蹤控制，進(jìn)而實(shí)現(xiàn)了船舶的路徑規(guī)劃與跟蹤，為船舶路徑規(guī)劃與跟蹤控制的結(jié)合提供了較好的方法。

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