一種規(guī)避動靜態(tài)障礙物的無人船路徑跟蹤視距制導(dǎo)方法及系統(tǒng)
1.本發(fā)明涉及無人船運(yùn)動控制技術(shù)領(lǐng)域，具體而言，尤其涉及一種規(guī)避動靜態(tài)障礙物的無人船路徑跟蹤視距制導(dǎo)方法及系統(tǒng)。
背景技術(shù)：
2.21世紀(jì)是海洋的世紀(jì)，世界各國圍繞海洋權(quán)益的爭奪日益升溫，開發(fā)和利用海洋資源成為當(dāng)今熱潮。無人船作為常見的自主航行器，由于其自身的小型化、智能化和具備多用途海洋運(yùn)載平臺被廣泛用來執(zhí)行具有危險(xiǎn)性和重復(fù)性特點(diǎn)的工作。實(shí)際的海洋環(huán)境十分復(fù)雜，存在如礁石、沉船以及正在航行的船只等多種靜態(tài)和動態(tài)障礙物，給無人船的安全航行帶來巨大挑戰(zhàn)。為降低無人船對岸端操作人員的依賴性，無人船應(yīng)具備自主決策和智能避障的能力，無人船需根據(jù)外部傳感器感知環(huán)境信息，實(shí)時自主地完成避障過程。
3.目前，在無人船在運(yùn)動控制領(lǐng)域中，國內(nèi)外學(xué)者已經(jīng)對避障問題進(jìn)行了相關(guān)研究，并提出了許多避障算法，如速度障礙法、人工勢場法、基于強(qiáng)化學(xué)習(xí)避碰避障算法和基于向量場的避障避碰算法等。其中速度障礙法通過分析自身位置和障礙物之間的幾何關(guān)系，計(jì)算避障所需的速度和航向，但當(dāng)速度受限的情況下時，不能保證避障后的路徑最優(yōu)，降低任務(wù)效率。基于人工勢場法的避障系統(tǒng)若已知障礙物分布，可先獲取人工勢場，即可計(jì)算出最優(yōu)路徑，當(dāng)未知障礙物的分布時，可根據(jù)自身裝載傳感器檢測周圍環(huán)境信息實(shí)時構(gòu)造周圍勢場，但獲取局部最優(yōu)路徑時可能陷入局部極小值，需進(jìn)行改進(jìn)。而強(qiáng)化學(xué)習(xí)是將動態(tài)規(guī)劃與監(jiān)督學(xué)習(xí)相結(jié)合的一種新型學(xué)習(xí)方法，但當(dāng)運(yùn)行場景發(fā)生變化時，往往需要重新對模型進(jìn)行訓(xùn)練，代價(jià)相對較高。
4.基于向量場避碰避障算法是一種通過排斥向量場來定義幾乎全局反饋的避障算法，根據(jù)反饋控制律，迫使系統(tǒng)沿矢量場流動收斂至目標(biāo)路徑。該算法不需要定義碰撞避免目標(biāo)的類李雅普諾夫函數(shù)或勢能函數(shù)，并且不會出現(xiàn)遠(yuǎn)離目標(biāo)點(diǎn)的情況。總體而言，現(xiàn)有的無人船運(yùn)動控制依然存在如下問題：
5.一、現(xiàn)有的基于路徑跟蹤視距制導(dǎo)的方法中，一般不考慮避碰問題，只考慮無人船跟蹤指定路徑，但如果在實(shí)際航行中遇到的未知障礙物時，不能保證無人船的安全航行。
6.二、現(xiàn)有的基于向量場規(guī)避障礙物的方法中，多數(shù)采用混合場來進(jìn)行規(guī)避障礙物，即吸引場制導(dǎo)與排斥場避障相結(jié)合，但設(shè)計(jì)過程較為復(fù)雜，需根據(jù)不同運(yùn)動場景設(shè)計(jì)不同向量場。
7.三、現(xiàn)有的基于需要參數(shù)調(diào)試的規(guī)避障礙物的方法中，如強(qiáng)化學(xué)習(xí)算法，其避碰參數(shù)需要依賴獎勵函數(shù)通過不斷的數(shù)據(jù)訓(xùn)練獲得，更換環(huán)境后需要重新進(jìn)行訓(xùn)練，不能保證路徑最優(yōu)而表現(xiàn)出局限性。
技術(shù)實(shí)現(xiàn)要素：
8.根據(jù)上述提出的技術(shù)問題，提供一種規(guī)避動靜態(tài)障礙物的無人船路徑跟蹤視距制
導(dǎo)方法及系統(tǒng)。本發(fā)明將視距制導(dǎo)與基于向量場避碰避障方法中排斥場理論相結(jié)合，提出一種規(guī)避動靜態(tài)障礙物的無人船路徑跟蹤視距制導(dǎo)方法，通過視距制導(dǎo)產(chǎn)生的制導(dǎo)向量和排斥場產(chǎn)生的排斥向量相結(jié)合，得到用于控制欠驅(qū)動無人船的安全制導(dǎo)角，保證了欠驅(qū)動無人船在路徑跟蹤過程中規(guī)避動靜態(tài)障礙物以保證航行的安全性。
9.本發(fā)明采用的技術(shù)手段如下：
10.一種規(guī)避動靜態(tài)障礙物的無人船路徑跟蹤視距制導(dǎo)方法，包括：
11.構(gòu)建欠驅(qū)動無人船運(yùn)動學(xué)模型；
12.根據(jù)構(gòu)建的欠驅(qū)動無人船運(yùn)動學(xué)模型，計(jì)算得到視距制導(dǎo)角；
13.根據(jù)得到的視距制導(dǎo)角，計(jì)算得到向量場引導(dǎo)角；
14.根據(jù)得到的視距制導(dǎo)角和向量場引導(dǎo)角，計(jì)算得到合成向量最終制導(dǎo)角；
15.根據(jù)得到的合成向量最終制導(dǎo)角和無人船實(shí)際航向信息，設(shè)計(jì)角速度制導(dǎo)律，實(shí)現(xiàn)欠驅(qū)動無人船運(yùn)動學(xué)閉環(huán)安全控制。
16.進(jìn)一步地，所述構(gòu)建欠驅(qū)動無人船運(yùn)動學(xué)模型，包括：
17.將無人船的運(yùn)動學(xué)模型描述為：
[0018][0019]
其中，x，y，ψ分別表示無人船在地球坐標(biāo)系下的x軸、y軸上的位置以及艏搖角信息，u，v，r分別表示無人船在船體坐標(biāo)系下的縱蕩速度、橫漂速度以及艏搖角速度；
[0020]
在欠驅(qū)動無人船運(yùn)動規(guī)劃中，假設(shè)橫漂速度v為0，將無人船的運(yùn)動學(xué)模型進(jìn)一步表示為：
[0021][0022]
進(jìn)一步地，所述根據(jù)構(gòu)建的欠驅(qū)動無人船運(yùn)動學(xué)模型，計(jì)算得到視距制導(dǎo)角，包括：
[0023]
根據(jù)當(dāng)前無人船的位置信息和航向信息x，y，ψ，以及需要跟蹤的路徑點(diǎn)信息，將兩個相鄰路徑點(diǎn)表示為：
[0024]
p
(i-1)
＝[x
(i-1)
，y
(i-1)
]
t
，pi＝[xi，yi]
t
[0025]
其中，下角標(biāo)i表示路徑點(diǎn)位次，pi表示第i個路徑點(diǎn)的位置信息，xi，yi分別表示目標(biāo)點(diǎn)在地球坐標(biāo)系下的x軸、y軸上的位置；
[0026]
定義路徑切向角ψd，將ψd表示為：
[0027]
ψd＝atan2(y
(i-1)-yi，x
(i-1)-xi)
[0028]
將無人船在航行過程中的位置跟蹤誤差表示為：
[0029][0030]
其中，s和e分別表示為路徑坐標(biāo)系下的橫向跟蹤誤差和縱向跟蹤誤差，r(ψd)為旋轉(zhuǎn)矩陣，表示為：
[0031][0032]
設(shè)計(jì)視距制導(dǎo)的引導(dǎo)角：
[0033][0034]
其中，δ為一個正的參數(shù)；
[0035]
將設(shè)計(jì)的視距制導(dǎo)的引導(dǎo)角表示為一個向量v
los
，如下所示：
[0036]vlos
＝[cosψ
los
，sinψ
los
]
t
。
[0037]
進(jìn)一步地，所述根據(jù)得到的視距制導(dǎo)角，計(jì)算得到向量場引導(dǎo)角，包括：
[0038]
基于欠驅(qū)動無人艇當(dāng)前的位置和航向信息x，y，ψ，和障礙物位置信息po以及路徑點(diǎn)信息，根據(jù)向量場定義，引入向量場f(pe)：
[0039][0040]
其中，κ表示一個參數(shù)，α＝[α
x
，αy]
t
為確定向量場指向的向量，pe＝p-po，p＝[x，y]
t
為當(dāng)前無人船位置信息，po＝[xo，yo]
t
為障礙物位置信息；
[0041]
定義障礙物周圍排斥場，根據(jù)引入的向量場f(pe)的公式，得到向量場分量的解析形式：
[0042][0043]
其中，[α
x
，αy]
t
＝[cos α,sin α]
t
，α＝atan2(y
o-yi，x
o-xi)，κ設(shè)計(jì)為：
[0044][0045]
其中，φ＝atan2(y
i-yo，x
i-xo)+π；
[0046]
計(jì)算向量場引導(dǎo)矢量角ψ
gvf
，如下：
[0047][0048]
其中，||
·
||表示向量的范數(shù)；
[0049]
將排斥場引導(dǎo)角表示為向量fo，如下：
[0050]fo
＝[cosψ
gvf
，sinψ
gvf
]
t
。
[0051]
進(jìn)一步地，所述根據(jù)得到的視距制導(dǎo)角和向量場引導(dǎo)角，計(jì)算得到合成向量最終制導(dǎo)角，包括：
[0052]
根據(jù)視距制導(dǎo)角v
los
和排斥場引導(dǎo)角fo，為了實(shí)現(xiàn)安全路徑跟蹤任務(wù)，設(shè)計(jì)控制律，如下：
[0053]fg
＝(1-λ)fo+λv
los
＝[f
gx
，f
gy
]
t
[0054]
其中，λ是一個平滑過渡函數(shù)，它在欠驅(qū)動無人船遠(yuǎn)離障礙物時值取為1，當(dāng)欠驅(qū)動無人船接近障礙物時減小，直到在障礙物的邊界時取值為0；λ設(shè)計(jì)如下：
[0055][0056]
其中，ro為障礙物半徑，r
max
，r
min
為用戶選擇的碰撞避免參數(shù)；
[0057]
為滿足連續(xù)性，設(shè)計(jì)系數(shù)如下：
[0058][0059][0060]
設(shè)計(jì)合成向量最終制導(dǎo)角ψg，如下：
[0061]
ψg＝atan2(f
gy
，f
gx
)。
[0062]
進(jìn)一步地，所述根據(jù)得到的合成向量最終制導(dǎo)角和無人船實(shí)際航向信息，設(shè)計(jì)角速度制導(dǎo)律，實(shí)現(xiàn)欠驅(qū)動無人船運(yùn)動學(xué)閉環(huán)安全控制，包括：
[0063]
基于無人船實(shí)際航向信息ψ和向量最終制導(dǎo)角ψg，考慮無人船行駛過程中假設(shè)縱蕩速度u自由變化，設(shè)計(jì)角速度控制器為：
[0064]
rg＝-k tanh(ψ-ψg)
[0065]
其中，k為正的參數(shù)，tanh(
·
)為飽和函數(shù)。
[0066]
本發(fā)明還提供了一種基于上述規(guī)避動靜態(tài)障礙物的無人船路徑跟蹤視距制導(dǎo)方法的無人船路徑跟蹤視距制導(dǎo)系統(tǒng)，包括：
[0067]
欠驅(qū)動無人船運(yùn)動學(xué)模型構(gòu)建模塊，用于構(gòu)建欠驅(qū)動無人船運(yùn)動學(xué)模型；
[0068]
視距制導(dǎo)模塊，用于根據(jù)構(gòu)建的欠驅(qū)動無人船運(yùn)動學(xué)模型，計(jì)算得到視距制導(dǎo)角；
[0069]
排斥向量場模塊，用于根據(jù)得到的視距制導(dǎo)角，計(jì)算得到向量場引導(dǎo)角；
[0070]
向量合成模塊，用于根據(jù)得到的視距制導(dǎo)角和向量場引導(dǎo)角，計(jì)算得到合成向量最終制導(dǎo)角；
[0071]
運(yùn)動學(xué)制導(dǎo)模塊，用于根據(jù)得到的合成向量最終制導(dǎo)角和無人船實(shí)際航向信息，設(shè)計(jì)角速度制導(dǎo)律，實(shí)現(xiàn)欠驅(qū)動無人船運(yùn)動學(xué)閉環(huán)安全控制。
[0072]
進(jìn)一步地，所述視距制導(dǎo)模塊、排斥向量場模塊、向量合成模塊以及運(yùn)動學(xué)制導(dǎo)模塊的連接關(guān)系如下：
[0073]
所述視距制導(dǎo)模塊的輸入端與欠驅(qū)動無人船輸出的位置信息x，y、航向信息ψ以及外部輸入即當(dāng)前需要跟蹤的路徑點(diǎn)p
(i-1)
，pi相連，所述視距制導(dǎo)模塊的輸出端與向量合成模塊的輸入端相連；
[0074]
所述排斥向量場模塊的輸入端與欠驅(qū)動無人船輸出的位置信息x，y、航向信息ψ以及障礙物位置信息po相連，所述排斥向量場模塊的輸出端與向量合成模塊的輸入端相連；
[0075]
所述向量合成模塊的輸入端與所述視距制導(dǎo)模塊的輸出端信號視距引導(dǎo)角向量vlos
和所述排斥向量場模塊的輸出端信號排斥場引導(dǎo)向量fo相連，所述向量合成模塊的輸出端與所述運(yùn)動學(xué)制導(dǎo)模塊的輸入端相連；
[0076]
所述運(yùn)動學(xué)制導(dǎo)模塊的輸入端與所述合成向量模塊的輸出信號最終制導(dǎo)角ψg和無人船實(shí)際航向信息ψ相連，所述運(yùn)動學(xué)制導(dǎo)模塊的輸出端信號制導(dǎo)角速度rg與欠驅(qū)動無人船相連。
[0077]
較現(xiàn)有技術(shù)相比，本發(fā)明具有以下優(yōu)點(diǎn)：
[0078]
1、本發(fā)明提供的規(guī)避動靜態(tài)障礙物的無人船路徑跟蹤視距制導(dǎo)方法，與現(xiàn)有的基于路徑跟蹤視距制導(dǎo)的方法相比，本發(fā)明在路徑跟蹤的基礎(chǔ)上，考慮了欠驅(qū)動無人船航行過程中出現(xiàn)靜態(tài)障礙物和動態(tài)障礙物情況，保證了欠驅(qū)動無人船航行過程中的安全性。
[0079]
2、本發(fā)明提供的規(guī)避動靜態(tài)障礙物的無人船路徑跟蹤視距制導(dǎo)方法，與現(xiàn)有基于向量場規(guī)避障礙物的方法相比，本發(fā)明采用視距制導(dǎo)和向量場引導(dǎo)相結(jié)合的方式，根據(jù)欠驅(qū)動無人艇與障礙物之間的距離獲取權(quán)重分配，有效地解決了規(guī)避算法設(shè)計(jì)的復(fù)雜問題。
[0080]
3、本發(fā)明提供的規(guī)避動靜態(tài)障礙物的無人船路徑跟蹤視距制導(dǎo)方法，與現(xiàn)有需要參數(shù)調(diào)試的規(guī)避障礙物的方法相比，本發(fā)明僅需提供障礙物大小和避碰半徑等外部信息，不需要進(jìn)行復(fù)雜的參數(shù)調(diào)節(jié)過程，避免了算法應(yīng)用過程中調(diào)整參數(shù)困難的問題。
[0081]
基于上述理由本發(fā)明可在無人船運(yùn)動控制等領(lǐng)域廣泛推廣。
附圖說明
[0082]
為了更清楚地說明本發(fā)明實(shí)施例或現(xiàn)有技術(shù)中的技術(shù)方案，下面將對實(shí)施例或現(xiàn)有技術(shù)描述中所需要使用的附圖做以簡單地介紹，顯而易見地，下面描述中的附圖是本發(fā)明的一些實(shí)施例，對于本領(lǐng)域普通技術(shù)人員來講，在不付出創(chuàng)造性勞動性的前提下，還可以根據(jù)這些附圖獲得其他的附圖。
[0083]
圖1為本發(fā)明規(guī)避動靜態(tài)障礙物的無人船路徑跟蹤視距制導(dǎo)系統(tǒng)結(jié)構(gòu)示意圖。
[0084]
圖2為本發(fā)明實(shí)施例提供的障礙物周圍排斥場示意圖。
[0085]
圖3為本發(fā)明實(shí)施例提供的欠驅(qū)動無人船軌跡圖。
[0086]
圖4為本發(fā)明實(shí)施例提供的路徑跟蹤過程中欠驅(qū)動無人船與障礙物之間的距離變化曲線。
[0087]
圖5為本發(fā)明實(shí)施例提供的路徑跟蹤過程中欠驅(qū)動無人船的橫向跟蹤誤差曲線。
[0088]
圖6為本發(fā)明實(shí)施例提供的路徑跟蹤過程中欠驅(qū)動無人船的縱向跟蹤誤差曲線。
具體實(shí)施方式
[0089]
需要說明的是，在不沖突的情況下，本發(fā)明中的實(shí)施例及實(shí)施例中的特征可以相互組合。下面將參考附圖并結(jié)合實(shí)施例來詳細(xì)說明本發(fā)明。
[0090]
為使本發(fā)明實(shí)施例的目的、技術(shù)方案和優(yōu)點(diǎn)更加清楚，下面將結(jié)合本發(fā)明實(shí)施例中的附圖，對本發(fā)明實(shí)施例中的技術(shù)方案進(jìn)行清楚、完整地描述，顯然，所描述的實(shí)施例僅僅是本發(fā)明一部分實(shí)施例，而不是全部的實(shí)施例。以下對至少一個示例性實(shí)施例的描述實(shí)際上僅僅是說明性的，決不作為對本發(fā)明及其應(yīng)用或使用的任何限制。基于本發(fā)明中的實(shí)施例，本領(lǐng)域普通技術(shù)人員在沒有做出創(chuàng)造性勞動前提下所獲得的所有其他實(shí)施例，都屬于本發(fā)明保護(hù)的范圍。
[0091]
需要注意的是，這里所使用的術(shù)語僅是為了描述具體實(shí)施方式，而非意圖限制根據(jù)本發(fā)明的示例性實(shí)施方式。如在這里所使用的，除非上下文另外明確指出，否則單數(shù)形式也意圖包括復(fù)數(shù)形式，此外，還應(yīng)當(dāng)理解的是，當(dāng)在本說明書中使用術(shù)語“包含”和/或“包括”時，其指明存在特征、步驟、操作、器件、組件和/或它們的組合。
[0092]
除非另外具體說明，否則在這些實(shí)施例中闡述的部件和步驟的相對布置、數(shù)字表達(dá)式和數(shù)值不限制本發(fā)明的范圍。同時，應(yīng)當(dāng)清楚，為了便于描述，附圖中所示出的各個部分的尺寸并不是按照實(shí)際的比例關(guān)系繪制的。對于相關(guān)領(lǐng)域普通技術(shù)人員己知的技術(shù)、方法和設(shè)備可能不作詳細(xì)討論，但在適當(dāng)情況下，所述技術(shù)、方法和設(shè)備應(yīng)當(dāng)被視為授權(quán)說明書的一部分。在這里示出和討論的所有示例中，任向具體值應(yīng)被解釋為僅僅是示例性的，而不是作為限制。因此，示例性實(shí)施例的其它示例可以具有不同的值。應(yīng)注意到：相似的標(biāo)號和字母在下面的附圖中表示類似項(xiàng)，因此，一旦某一項(xiàng)在一個附圖中被定義，則在隨后的附圖中不需要對其進(jìn)行進(jìn)一步討論。
[0093]
在本發(fā)明的描述中，需要理解的是，方位詞如“前、后、上、下、左、右”、“橫向、豎向、垂直、水平”和“頂、底”等所指示的方位或位置關(guān)系通常是基于附圖所示的方位或位置關(guān)系，僅是為了便于描述本發(fā)明和簡化描述，在未作相反說明的情況下，這些方位詞并不指示和暗示所指的裝置或元件必須具有特定的方位或者以特定的方位構(gòu)造和操作，因此不能理解為對本發(fā)明保護(hù)范圍的限制：方位詞“內(nèi)、外”是指相對于各部件本身的輪廓的內(nèi)外。
[0094]
為了便于描述，在這里可以使用空間相對術(shù)語，如“在
……
之上”、“在
……
上方”、“在
……
上表面”、“上面的”等，用來描述如在圖中所示的一個器件或特征與其他器件或特征的空間位置關(guān)系。應(yīng)當(dāng)理解的是，空間相對術(shù)語旨在包含除了器件在圖中所描述的方位之外的在使用或操作中的不同方位。例如，如果附圖中的器件被倒置，則描述為“在其他器件或構(gòu)造上方”或“在其他器件或構(gòu)造之上”的器件之后將被定位為“在其他器件或構(gòu)造下方”或“在其位器件或構(gòu)造之下”。因而，示例性術(shù)語“在
……
上方”可以包括“在
……
上方”和“在
……
下方”兩種方位。該器件也可以其他不同方式定位(旋轉(zhuǎn)90度或處于其他方位)，并且對這里所使用的空間相對描述作出相應(yīng)解釋。
[0095]
此外，需要說明的是，使用“第一”、“第二”等詞語來限定零部件，僅僅是為了便于對相應(yīng)零部件進(jìn)行區(qū)別，如沒有另行聲明，上述詞語并沒有特殊含義，因此不能理解為對本發(fā)明保護(hù)范圍的限制。
[0096]
本發(fā)明提供了一種規(guī)避動靜態(tài)障礙物的無人船路徑跟蹤視距制導(dǎo)方法，包括：
[0097]
s1、構(gòu)建欠驅(qū)動無人船運(yùn)動學(xué)模型；
[0098]
s2、根據(jù)構(gòu)建的欠驅(qū)動無人船運(yùn)動學(xué)模型，計(jì)算得到視距制導(dǎo)角；
[0099]
s3、根據(jù)得到的視距制導(dǎo)角，計(jì)算得到向量場引導(dǎo)角；
[0100]
s4、根據(jù)得到的視距制導(dǎo)角和向量場引導(dǎo)角，計(jì)算得到合成向量最終制導(dǎo)角；
[0101]
s5、根據(jù)得到的合成向量最終制導(dǎo)角和無人船實(shí)際航向信息，設(shè)計(jì)角速度制導(dǎo)律，實(shí)現(xiàn)欠驅(qū)動無人船運(yùn)動學(xué)閉環(huán)安全控制。
[0102]
具體實(shí)施時，作為本發(fā)明優(yōu)選的實(shí)施方式，所述步驟s1具體包括：
[0103]
s11、將無人船的運(yùn)動學(xué)模型描述為：
[0104][0105]
其中，x，y，ψ分別表示無人船在地球坐標(biāo)系下的x軸、y軸上的位置以及艏搖角信息，u，v，r分別表示無人船在船體坐標(biāo)系下的縱蕩速度、橫漂速度以及艏搖角速度；
[0106]
s12、在欠驅(qū)動無人船運(yùn)動規(guī)劃中，假設(shè)橫漂速度v為0，將無人船的運(yùn)動學(xué)模型進(jìn)一步表示為：
[0107][0108]
具體實(shí)施時，作為本發(fā)明優(yōu)選的實(shí)施方式，所述步驟s2具體包括：
[0109]
s21、根據(jù)當(dāng)前無人船的位置信息和航向信息x，y，ψ，以及需要跟蹤的路徑點(diǎn)信息，將兩個相鄰路徑點(diǎn)表示為：
[0110]
p
(i-1)
＝[x
(i-1)
，y
(i-1)
]
t
，pi＝[xi，yi]
t
[0111]
其中，下角標(biāo)i表示路徑點(diǎn)位次，pi表示第i個路徑點(diǎn)的位置信息，xi，yi分別表示目標(biāo)點(diǎn)在地球坐標(biāo)系下的x軸、y軸上的位置；
[0112]
s22、定義路徑切向角ψd，將ψd表示為：
[0113]
ψd＝atan2(y
(i-1)-yi，x
(i-1)-xi)
[0114]
s23、將無人船在航行過程中的位置跟蹤誤差表示為：
[0115][0116]
其中，s和e分別表示為路徑坐標(biāo)系下的橫向跟蹤誤差和縱向跟蹤誤差，r(ψd)為旋轉(zhuǎn)矩陣，表示為：
[0117][0118]
s24、設(shè)計(jì)視距制導(dǎo)的引導(dǎo)角：
[0119][0120]
其中，δ為一個正的參數(shù)，在本實(shí)施例中，前視距離參數(shù)設(shè)計(jì)為δ＝7。
[0121]
s25、將設(shè)計(jì)的視距制導(dǎo)的引導(dǎo)角表示為一個向量v
los
，如下所示：
[0122]vlos
＝[cosψ
los
，sinψ
los
]
t
。
[0123]
具體實(shí)施時，作為本發(fā)明優(yōu)選的實(shí)施方式，所述步驟s3具體包括：
[0124]
s31、基于欠驅(qū)動無人艇當(dāng)前的位置和航向信息x，y，ψ，和障礙物位置信息po以及路徑點(diǎn)信息，根據(jù)向量場定義，引入向量場f(pe)：
[0125][0126]
其中，κ表示一個參數(shù)，α＝[α
x
，αy]
t
為確定向量場指向的向量，pe＝p-po，p＝[x，y]
t
為當(dāng)前無人船位置信息，po＝[xo，yo]
t
為障礙物位置信息；
[0127]
s32、定義障礙物周圍排斥場，根據(jù)引入的向量場f(pe)的公式，得到向量場分量的解析形式：
[0128][0129]
其中，[α
x
，αy]
t
＝[cosα，sinα]
t
，α＝atan2(y
o-yi，x
o-xi)，κ設(shè)計(jì)為：
[0130][0131]
其中，φ＝atan2(y
i-yo，x
i-xo)+π；
[0132]
s33、計(jì)算向量場引導(dǎo)矢量角ψ
gvf
，如下：
[0133][0134]
其中，||
·
||表示向量的范數(shù)；
[0135]
s34、將排斥場引導(dǎo)角表示為向量fo，如下：
[0136]fo
＝[cosψ
gvf
，sinψ
gvf
]
t
。
[0137]
具體實(shí)施時，作為本發(fā)明優(yōu)選的實(shí)施方式，所述步驟s4具體包括：
[0138]
s41、根據(jù)視距制導(dǎo)角v
los
和排斥場引導(dǎo)角fo，為了實(shí)現(xiàn)安全路徑跟蹤任務(wù)，設(shè)計(jì)控制律，如下：
[0139]fg
＝(1-λ)fo+λv
los
＝[f
gx
，f
gy
]
t
[0140]
其中，λ是一個平滑過渡函數(shù)，它在欠驅(qū)動無人船遠(yuǎn)離障礙物時值取為1，當(dāng)欠驅(qū)動無人船接近障礙物時減小，直到在障礙物的邊界時取值為0；λ設(shè)計(jì)如下：
[0141][0142]
其中，ro為障礙物半徑，r
max
，r
min
為用戶選擇的碰撞避免參數(shù)；在本實(shí)施例中，為實(shí)現(xiàn)不同避碰效果所引入?yún)?shù)：圖中ro為用戶設(shè)定障礙物半徑，一般大于真實(shí)具體障礙物，為驗(yàn)證算法的有效性，設(shè)計(jì)靜態(tài)障礙物ro＝10，動態(tài)障礙物ro＝5，r
max
，r
min
為用戶選擇的碰撞避免參數(shù)。
[0143]
s42、為滿足連續(xù)性，設(shè)計(jì)系數(shù)如下：
[0144][0145]
[0146]
s43、設(shè)計(jì)合成向量最終制導(dǎo)角ψg，如下：
[0147]
ψg＝atan2(f
gy
，f
gx
)。
[0148]
具體實(shí)施時，作為本發(fā)明優(yōu)選的實(shí)施方式，所述步驟s5具體包括：
[0149]
基于無人船實(shí)際航向信息ψ和向量最終制導(dǎo)角ψg，考慮無人船行駛過程中假設(shè)縱蕩速度u自由變化，設(shè)計(jì)角速度控制器為：
[0150]
rg＝-ktanh(ψ-ψg)
[0151]
其中，k為正的參數(shù)，tanh(
·
)為飽和函數(shù)。在本實(shí)施例中，為實(shí)現(xiàn)角速度控制所引入的控制參數(shù)：k＝0.5。
[0152]
本發(fā)明實(shí)施例還提供了一種基于上述規(guī)避動靜態(tài)障礙物的無人船路徑跟蹤視距制導(dǎo)方法的無人船路徑跟蹤視距制導(dǎo)系統(tǒng)，包括：
[0153]
欠驅(qū)動無人船運(yùn)動學(xué)模型構(gòu)建模塊，用于構(gòu)建欠驅(qū)動無人船運(yùn)動學(xué)模型；
[0154]
視距制導(dǎo)模塊，用于根據(jù)構(gòu)建的欠驅(qū)動無人船運(yùn)動學(xué)模型，計(jì)算得到視距制導(dǎo)角；
[0155]
排斥向量場模塊，用于根據(jù)得到的視距制導(dǎo)角，計(jì)算得到向量場引導(dǎo)角；
[0156]
向量合成模塊，用于根據(jù)得到的視距制導(dǎo)角和向量場引導(dǎo)角，計(jì)算得到合成向量最終制導(dǎo)角；
[0157]
運(yùn)動學(xué)制導(dǎo)模塊，用于根據(jù)得到的合成向量最終制導(dǎo)角和無人船實(shí)際航向信息，設(shè)計(jì)角速度制導(dǎo)律，實(shí)現(xiàn)欠驅(qū)動無人船運(yùn)動學(xué)閉環(huán)安全控制。
[0158]
具體實(shí)施時，作為本發(fā)明優(yōu)選的實(shí)施方式，所述視距制導(dǎo)模塊、排斥向量場模塊、向量合成模塊以及運(yùn)動學(xué)制導(dǎo)模塊的連接關(guān)系如下：
[0159]
所述視距制導(dǎo)模塊的輸入端與欠驅(qū)動無人船輸出的位置信息x，y、航向信息ψ以及外部輸入即當(dāng)前需要跟蹤的路徑點(diǎn)p
(i-1)
，pi相連，所述視距制導(dǎo)模塊的輸出端與向量合成模塊的輸入端相連；
[0160]
所述排斥向量場模塊的輸入端與欠驅(qū)動無人船輸出的位置信息x，y、航向信息ψ以及障礙物位置信息po相連，所述排斥向量場模塊的輸出端與向量合成模塊的輸入端相連；
[0161]
所述向量合成模塊的輸入端與所述視距制導(dǎo)模塊的輸出端信號視距引導(dǎo)角向量v
los
和所述排斥向量場模塊的輸出端信號排斥場引導(dǎo)向量fo相連，所述向量合成模塊的輸出端與所述運(yùn)動學(xué)制導(dǎo)模塊的輸入端相連；
[0162]
所述運(yùn)動學(xué)制導(dǎo)模塊的輸入端與所述合成向量模塊的輸出信號最終制導(dǎo)角ψg和無人船實(shí)際航向信息ψ相連，所述運(yùn)動學(xué)制導(dǎo)模塊的輸出端信號制導(dǎo)角速度rg與欠驅(qū)動無人船相連。
[0163]
如圖1所示，根據(jù)欠驅(qū)動無人船運(yùn)動學(xué)模型，在運(yùn)動學(xué)層次，計(jì)算得到視距制導(dǎo)角，進(jìn)一步將視距制導(dǎo)角表示為一個向量。根據(jù)向量場分量的解析形式定義障礙物周圍排斥場，獲得向量場引導(dǎo)角，并將獲得的向量場引導(dǎo)角表示為向量形式。為了實(shí)現(xiàn)安全路徑跟蹤任務(wù)，根據(jù)權(quán)重λ取值得到角度制導(dǎo)律。考慮欠驅(qū)動無人船行駛過程中縱蕩速度u自由變化，運(yùn)動學(xué)控制只需設(shè)計(jì)角速度制導(dǎo)律，實(shí)現(xiàn)欠驅(qū)動無人船運(yùn)動學(xué)閉環(huán)安全控制。
[0164]
實(shí)施例
[0165]
為了驗(yàn)證本發(fā)明方法的有效性，進(jìn)行了仿真實(shí)驗(yàn)，根據(jù)仿真結(jié)果可知，圖3為欠驅(qū)動無人船路徑跟蹤過程中運(yùn)動軌跡圖，圖中虛線表示無人船參考軌跡即需要跟蹤的路徑信息，實(shí)線表示無人船實(shí)際軌跡，圖中大的圓形障礙物為靜態(tài)障礙物，圖中小的障礙物為動態(tài)
障礙物并最終移動到圖右側(cè)虛線圓位置，從圖中可以看出欠驅(qū)動無人船跟蹤上給定的參考路徑，并繞開了環(huán)境中的靜態(tài)障礙物和動態(tài)障礙物。圖4為路徑跟蹤過程中欠驅(qū)動無人船與靜態(tài)障礙物和動態(tài)障礙物之間的距離變化曲線，從圖中可看出欠驅(qū)動無人船航行時與三個靜態(tài)障礙物和一個動態(tài)障礙物之間的距離始終大于設(shè)定的最小安全距離。圖5-6為欠驅(qū)動無人船跟蹤參考路徑時的橫向跟蹤誤差和縱向跟蹤誤向圖像，在規(guī)避障礙物時發(fā)生動態(tài)變化，并均在可接受范圍內(nèi)，符合控制要求。
[0166]
最后應(yīng)說明的是：以上各實(shí)施例僅用以說明本發(fā)明的技術(shù)方案，而非對其限制；盡管參照前述各實(shí)施例對本發(fā)明進(jìn)行了詳細(xì)的說明，本領(lǐng)域的普通技術(shù)人員應(yīng)當(dāng)理解：其依然可以對前述各實(shí)施例所記載的技術(shù)方案進(jìn)行修改，或者對其中部分或者全部技術(shù)特征進(jìn)行等同替換；而這些修改或者替換，并不使相應(yīng)技術(shù)方案的本質(zhì)脫離本發(fā)明各實(shí)施例技術(shù)方案的范圍。
技術(shù)特征：
1.一種規(guī)避動靜態(tài)障礙物的無人船路徑跟蹤視距制導(dǎo)方法，其特征在于，包括：構(gòu)建欠驅(qū)動無人船運(yùn)動學(xué)模型；根據(jù)構(gòu)建的欠驅(qū)動無人船運(yùn)動學(xué)模型，計(jì)算得到視距制導(dǎo)角；根據(jù)得到的視距制導(dǎo)角，計(jì)算得到向量場引導(dǎo)角；根據(jù)得到的視距制導(dǎo)角和向量場引導(dǎo)角，計(jì)算得到合成向量最終制導(dǎo)角；根據(jù)得到的合成向量最終制導(dǎo)角和無人船實(shí)際航向信息，設(shè)計(jì)角速度制導(dǎo)律，實(shí)現(xiàn)欠驅(qū)動無人船運(yùn)動學(xué)閉環(huán)安全控制。2.根據(jù)權(quán)利要求1所述的規(guī)避動靜態(tài)障礙物的無人船路徑跟蹤視距制導(dǎo)方法，其特征在于，所述構(gòu)建欠驅(qū)動無人船運(yùn)動學(xué)模型，包括：將無人船的運(yùn)動學(xué)模型描述為：其中，x，y，ψ分別表示無人船在地球坐標(biāo)系下的x軸、y軸上的位置以及艏搖角信息，u，v，r分別表示無人船在船體坐標(biāo)系下的縱蕩速度、橫漂速度以及艏搖角速度；在欠驅(qū)動無人船運(yùn)動規(guī)劃中，假設(shè)橫漂速度v為0，將無人船的運(yùn)動學(xué)模型進(jìn)一步表示為：3.根據(jù)權(quán)利要求1所述的規(guī)避動靜態(tài)障礙物的無人船路徑跟蹤視距制導(dǎo)方法，其特征在于，所述根據(jù)構(gòu)建的欠驅(qū)動無人船運(yùn)動學(xué)模型，計(jì)算得到視距制導(dǎo)角，包括：根據(jù)當(dāng)前無人船的位置信息和航向信息x，y，ψ，以及需要跟蹤的路徑點(diǎn)信息，將兩個相鄰路徑點(diǎn)表示為：p
(i-1)
＝[x
(i-1)
，y
(i-1)
]
t
，p
i
＝[x
i
，y
i
]
t
其中，下角標(biāo)i表示路徑點(diǎn)位次，p
i
表示第i個路徑點(diǎn)的位置信息，x
i
，y
i
分別表示目標(biāo)點(diǎn)在地球坐標(biāo)系下的x軸、y軸上的位置；定義路徑切向角ψ
d
，將ψ
d
表示為：ψ
d
＝atan2(y
(i-1)-y
i
，x
(i-1)-x
i
)將無人船在航行過程中的位置跟蹤誤差表示為：其中，s和e分別表示為路徑坐標(biāo)系下的橫向跟蹤誤差和縱向跟蹤誤差，r(ψ
d
)為旋轉(zhuǎn)矩陣，表示為：設(shè)計(jì)視距制導(dǎo)的引導(dǎo)角：
其中，δ為一個正的參數(shù)；將設(shè)計(jì)的視距制導(dǎo)的引導(dǎo)角表示為一個向量v
los
，如下所示：v
los
＝[cosψ
los
，sinψ
los
]
t
。4.根據(jù)權(quán)利要求1所述的規(guī)避動靜態(tài)障礙物的無人船路徑跟蹤視距制導(dǎo)方法，其特征在于，所述根據(jù)得到的視距制導(dǎo)角，計(jì)算得到向量場引導(dǎo)角，包括：基于欠驅(qū)動無人艇當(dāng)前的位置和航向信息x，y，ψ，和障礙物位置信息p
o
以及路徑點(diǎn)信息，根據(jù)向量場定義，引入向量場f(p
e
)：其中，κ表示一個參數(shù)，α＝[α
x
，α
y
]
t
為確定向量場指向的向量，p
e
＝p-p
o
，p＝[x，y]
t
為當(dāng)前無人船位置信息，p
o
＝[x
o
，y
o
]
t
為障礙物位置信息；定義障礙物周圍排斥場，根據(jù)引入的向量場f(p
e
)的公式，得到向量場分量的解析形式：其中，[α
x
，α
y
]
t
＝[cosα，sinα]
t
，α＝atan2(y
o-y
i
，x
o-x
i
)，k設(shè)計(jì)為：其中，φ＝atan2(y
i-y
o
，x
i-x
o
)+π；計(jì)算向量場引導(dǎo)矢量角ψ
gvf
，如下：其中，||
·
||表示向量的范數(shù)；將排斥場引導(dǎo)角表示為向量f
o
，如下：f
o
＝[cosψ
gvf
，sinψ
gvf
]
t
。5.根據(jù)權(quán)利要求1所述的規(guī)避動靜態(tài)障礙物的無人船路徑跟蹤視距制導(dǎo)方法，其特征在于，所述根據(jù)得到的視距制導(dǎo)角和向量場引導(dǎo)角，計(jì)算得到合成向量最終制導(dǎo)角，包括：根據(jù)視距制導(dǎo)角v
los
和排斥場引導(dǎo)角f
o
，為了實(shí)現(xiàn)安全路徑跟蹤任務(wù)，設(shè)計(jì)控制律，如下：f
g
＝(1-λ)f
o
+λv
los
＝[f
gx
，f
gy
]
t
其中，λ是一個平滑過渡函數(shù)，它在欠驅(qū)動無人船遠(yuǎn)離障礙物時值取為1，當(dāng)欠驅(qū)動無人船接近障礙物時減小，直到在障礙物的邊界時取值為0；λ設(shè)計(jì)如下：
其中，r
o
為障礙物半徑，r
max
，r
min
為用戶選擇的碰撞避免參數(shù)；為滿足連續(xù)性，設(shè)計(jì)系數(shù)如下：為滿足連續(xù)性，設(shè)計(jì)系數(shù)如下：設(shè)計(jì)合成向量最終制導(dǎo)角ψ
g
，如下：ψ
g
＝atan2(f
gy
，f
gx
)。6.根據(jù)權(quán)利要求1所述的規(guī)避動靜態(tài)障礙物的無人船路徑跟蹤視距制導(dǎo)方法，其特征在于，所述根據(jù)得到的合成向量最終制導(dǎo)角和無人船實(shí)際航向信息，設(shè)計(jì)角速度制導(dǎo)律，實(shí)現(xiàn)欠驅(qū)動無人船運(yùn)動學(xué)閉環(huán)安全控制，包括：基于無人船實(shí)際航向信息ψ和向量最終制導(dǎo)角ψ
g
，考慮無人船行駛過程中假設(shè)縱蕩速度u自由變化，設(shè)計(jì)角速度控制器為：r
g
＝-k tanh(ψ-ψ
g
)其中，k為正的參數(shù)，tanh(
·
)為飽和函數(shù)。7.一種基于權(quán)利要求1-6中任意一項(xiàng)權(quán)利要求所述規(guī)避動靜態(tài)障礙物的無人船路徑跟蹤視距制導(dǎo)方法的無人船路徑跟蹤視距制導(dǎo)系統(tǒng)，其特征在于，包括：欠驅(qū)動無人船運(yùn)動學(xué)模型構(gòu)建模塊，用于構(gòu)建欠驅(qū)動無人船運(yùn)動學(xué)模型；視距制導(dǎo)模塊，用于根據(jù)構(gòu)建的欠驅(qū)動無人船運(yùn)動學(xué)模型，計(jì)算得到視距制導(dǎo)角；排斥向量場模塊，用于根據(jù)得到的視距制導(dǎo)角，計(jì)算得到向量場引導(dǎo)角；向量合成模塊，用于根據(jù)得到的視距制導(dǎo)角和向量場引導(dǎo)角，計(jì)算得到合成向量最終制導(dǎo)角；運(yùn)動學(xué)制導(dǎo)模塊，用于根據(jù)得到的合成向量最終制導(dǎo)角和無人船實(shí)際航向信息，設(shè)計(jì)角速度制導(dǎo)律，實(shí)現(xiàn)欠驅(qū)動無人船運(yùn)動學(xué)閉環(huán)安全控制。8.根據(jù)權(quán)利要求7所述的規(guī)避動靜態(tài)障礙物的無人船路徑跟蹤視距制導(dǎo)系統(tǒng)，其特征在于，所述視距制導(dǎo)模塊、排斥向量場模塊、向量合成模塊以及運(yùn)動學(xué)制導(dǎo)模塊的連接關(guān)系如下：所述視距制導(dǎo)模塊的輸入端與欠驅(qū)動無人船輸出的位置信息x，y、航向信息ψ以及外部輸入即當(dāng)前需要跟蹤的路徑點(diǎn)p
(i-1)
，p
i
相連，所述視距制導(dǎo)模塊的輸出端與向量合成模塊的輸入端相連；所述排斥向量場模塊的輸入端與欠驅(qū)動無人船輸出的位置信息x，y、航向信息ψ以及障礙物位置信息p
o
相連，所述排斥向量場模塊的輸出端與向量合成模塊的輸入端相連；所述向量合成模塊的輸入端與所述視距制導(dǎo)模塊的輸出端信號視距引導(dǎo)角向量v
los
和所述排斥向量場模塊的輸出端信號排斥場引導(dǎo)向量f
o
相連，所述向量合成模塊的輸出端與所述運(yùn)動學(xué)制導(dǎo)模塊的輸入端相連；
所述運(yùn)動學(xué)制導(dǎo)模塊的輸入端與所述合成向量模塊的輸出信號最終制導(dǎo)角ψ
g
和無人船實(shí)際航向信息ψ相連，所述運(yùn)動學(xué)制導(dǎo)模塊的輸出端信號制導(dǎo)角速度r
g
與欠驅(qū)動無人船相連。
技術(shù)總結(jié)
本發(fā)明提供一種規(guī)避動靜態(tài)障礙物的無人船路徑跟蹤視距制導(dǎo)方法及系統(tǒng)，屬于無人船運(yùn)動控制技術(shù)領(lǐng)域，本發(fā)明方法，包括：構(gòu)建欠驅(qū)動無人船運(yùn)動學(xué)模型；根據(jù)構(gòu)建的欠驅(qū)動無人船運(yùn)動學(xué)模型，計(jì)算得到視距制導(dǎo)角；根據(jù)得到的視距制導(dǎo)角，計(jì)算得到向量場引導(dǎo)角；根據(jù)得到的視距制導(dǎo)角和向量場引導(dǎo)角，計(jì)算得到合成向量最終制導(dǎo)角；根據(jù)得到的合成向量最終制導(dǎo)角和無人船實(shí)際航向信息，設(shè)計(jì)角速度制導(dǎo)律，實(shí)現(xiàn)欠驅(qū)動無人船運(yùn)動學(xué)閉環(huán)安全控制。本發(fā)明通過視距制導(dǎo)產(chǎn)生的制導(dǎo)向量和排斥場產(chǎn)生的排斥向量相結(jié)合，得到用于控制欠驅(qū)動無人船的安全制導(dǎo)角，保證了欠驅(qū)動無人船在路徑跟蹤過程中規(guī)避動靜態(tài)障礙物以保證航行的安全性。規(guī)避動靜態(tài)障礙物以保證航行的安全性。規(guī)避動靜態(tài)障礙物以保證航行的安全性。
