專(zhuān)利名稱(chēng)：基于迭代的自主水下航行器三維曲線(xiàn)路徑跟蹤控制方法
技術(shù)領(lǐng)域：
本發(fā)明涉及欠驅(qū)動(dòng)自主水下航行器的三維空間運(yùn)動(dòng)控制技術(shù)領(lǐng)域。
背景技術(shù)：
海底地形的勘探和測(cè)繪對(duì)深海資源的開(kāi)發(fā)有著重要的意義，欠驅(qū)動(dòng)自主水下航行器AUV (Autonomous Underwater Vehicle)由于具有良好的機(jī)動(dòng)性和續(xù)航能力，在海洋勘探開(kāi)發(fā)中扮演著重要的角色，隨著AUV在海洋工程領(lǐng)域應(yīng)用的不斷深入，使得對(duì)AUV在水下三維空間的運(yùn)動(dòng)控制技術(shù)的研究提出了新的挑戰(zhàn)，考慮到受到航行經(jīng)濟(jì)性或負(fù)載能力制約，通常執(zhí)行機(jī)構(gòu)配置為縱向尾部推進(jìn)器、水平方向舵和垂直升降舵，AUV大多未配備橫向和垂向輔助推進(jìn)器，使得控制輸入的維數(shù)遠(yuǎn)小于運(yùn)動(dòng)自由度數(shù)，為欠驅(qū)動(dòng)系統(tǒng)，無(wú)法設(shè)計(jì)光滑時(shí)不變控制律實(shí)現(xiàn)反饋控制，同時(shí)由于受到復(fù)雜多變的海洋環(huán)境作用，使得AUV動(dòng)力學(xué) 模型具有較高的非線(xiàn)性、不確定性和模型自身存在的耦合性，這也成為欠驅(qū)動(dòng)AUV三維空間跟蹤控制器設(shè)計(jì)的難點(diǎn)。目前，國(guó)內(nèi)外針對(duì)欠驅(qū)動(dòng)AUV的三維空間運(yùn)動(dòng)控制的研究較少，研究大多針對(duì)解耦的水平面運(yùn)動(dòng)子系統(tǒng)和垂直面深度控制子系統(tǒng)分別設(shè)計(jì)控制器，進(jìn)而實(shí)現(xiàn)對(duì)欠驅(qū)動(dòng)自主水下航行器在水下三維空間的運(yùn)動(dòng)控制，由于忽略了模型的耦合作用，設(shè)計(jì)的控制器無(wú)法實(shí)現(xiàn)欠驅(qū)動(dòng)AUV對(duì)空間任意平滑曲線(xiàn)的跟蹤控制。這里討論的跟蹤控制問(wèn)題具體為水下三維空間的路徑跟蹤控制問(wèn)題，水下三維空間路徑的描述通過(guò)參數(shù)化方程進(jìn)行描述，不同于三維軌跡跟蹤控制問(wèn)題中的軌跡方程以時(shí)間作為參數(shù)，克服了傳統(tǒng)軌跡跟蹤控制問(wèn)題中由于引入具有同構(gòu)動(dòng)力學(xué)模型的“虛擬AUV”受到環(huán)境干擾作用引起閉環(huán)跟蹤系統(tǒng)具有不穩(wěn)定動(dòng)態(tài)，本發(fā)明中通過(guò)設(shè)計(jì)期望路徑上“虛擬向?qū)А钡囊苿?dòng)速度作為跟蹤系統(tǒng)額外的控制輸入，由于“虛擬向?qū)А敝痪哂羞\(yùn)動(dòng)學(xué)特性而無(wú)具體動(dòng)力學(xué)模型，因此狀態(tài)不受外界擾動(dòng)的影響，能夠保證跟S示系統(tǒng)的穩(wěn)定性和動(dòng)態(tài)性能。P. Encarnacao 等在論文《3D Path Following for Autonomous UnderwaterVehicle)) (Proceedings of the 39th IEEE Conference on Decision and Control, IEEEPress, 2000, Sydney.)利用正交投影的思想建立AUV在期望路徑坐標(biāo)系(Serret-Frenet)下的三維路徑跟蹤誤差模型，由于存在奇異值點(diǎn)，對(duì)AUV的初始位置有約束，無(wú)法實(shí)現(xiàn)AUV跟蹤的全局收斂，而本專(zhuān)利建立的AUV載體坐標(biāo)系下的三維跟蹤誤差模型不存在奇異值問(wèn)題，因此能夠保證AUV跟蹤誤差的全局收斂性；《基于自適應(yīng)Backst印ping的欠驅(qū)動(dòng)AUV三維航跡跟蹤控制》(控制與決策，2012,第38卷第2期)依據(jù)視線(xiàn)法(line-of-sight, L0S)計(jì)算期望跟蹤視線(xiàn)角，基于自適應(yīng)反步法設(shè)計(jì)跟蹤控制器，針對(duì)離散的航跡點(diǎn)的跟蹤控制，并未給出三維航跡跟蹤的誤差方程，無(wú)法實(shí)現(xiàn)對(duì)三維空間光滑曲線(xiàn)的跟蹤；且跟蹤導(dǎo)引策略為視線(xiàn)法(Line of Sight, L0S)而本專(zhuān)利采用的為虛擬向?qū)Р呗?Virtual Guidance),通過(guò)跟蹤期望路徑上的“虛擬向?qū)А秉c(diǎn)實(shí)現(xiàn)AUV收斂于期望路徑；文獻(xiàn)《基于離散滑模預(yù)測(cè)的欠驅(qū)動(dòng)AUV三維航跡跟蹤控制》(控制與決策，2011，第26卷第10期)中給出了在期望路徑上虛擬向?qū)ё鴺?biāo)系下的AUV三維路徑跟蹤誤差方程的形式，將AUV假定為虛擬質(zhì)點(diǎn)，假設(shè)運(yùn)動(dòng)方向與合速度矢量方向一致，獲得的三維路徑跟蹤誤差方程需要AUV運(yùn)動(dòng)的側(cè)漂角和攻角精確可量測(cè)，這在實(shí)際應(yīng)用中是存在困難，只能夠通過(guò)對(duì)橫向和垂向運(yùn)動(dòng)速度的測(cè)量，進(jìn)而計(jì)算出側(cè)漂角和攻角，而由于沿三軸線(xiàn)速度的測(cè)量較困難，導(dǎo)致最終的控制器解算存在潛在的中斷可能?！痘诜蔷€(xiàn)性迭代滑模的欠驅(qū)動(dòng)UUV三維航跡跟蹤控制》(自動(dòng)化學(xué)報(bào)，2012，第38卷第2期)基于工程控制器解耦的思想設(shè)計(jì)非線(xiàn)性迭代滑模航跡跟蹤控制器，由于被控對(duì)象模型為六自由度耦合運(yùn)動(dòng)模型，因此針對(duì)縱向速度、艏向控制和縱傾控制分別設(shè)計(jì)的解耦控制器只能通過(guò)魯棒項(xiàng)抑制模型中的耦合作用，當(dāng)模型各自由度之間的耦合作用較明顯時(shí)，控制器只能通過(guò)輸出較高的控制器增益為代價(jià)消除耦合作用，引起控制器輸出飽和信號(hào)，解耦的控制器只能夠保證三個(gè)獨(dú)立控制子系統(tǒng)的漸近穩(wěn)定性，而無(wú)法保證整個(gè)控制系統(tǒng)的漸近穩(wěn)定性，而本專(zhuān)利提出的三維航跡跟蹤控制器能夠保證整個(gè)系統(tǒng)全局漸近穩(wěn)定性；《自主式水下航行器三維路徑跟蹤的神經(jīng)網(wǎng)絡(luò)H00魯棒自適應(yīng)控制方法》(控制理論與應(yīng)用，2012，第29卷第3期)基于正交投影Serret-Frenet坐標(biāo)系建立AUV三維路徑跟蹤誤差方程，運(yùn)用H00魯棒控制思想設(shè)計(jì)控制器，同時(shí)引入神經(jīng)網(wǎng)絡(luò)補(bǔ)償模型不確定性，但由于基于正交投影Serret-Frenet坐標(biāo)系建立AUV三維路徑跟蹤誤差模型存在奇異值點(diǎn)，使得對(duì)AUV的初始條件有約束，即AUV初始位置必須位于跟蹤曲線(xiàn)最小曲率半徑內(nèi)， 因此無(wú)法實(shí)現(xiàn)AUV跟蹤的全局收斂性，而本專(zhuān)利建立在A(yíng)UV載體坐標(biāo)系下表示的三維跟蹤誤差模型不存在奇異值問(wèn)題，因此能夠保證AUV跟蹤誤差的全局收斂性，此外本專(zhuān)利采用迭代方法設(shè)計(jì)控制器不同于H00魯棒控制設(shè)計(jì)思想。以上文獻(xiàn)中涉及的方法均針對(duì)建立在Serret-Frenet坐標(biāo)系下誤差方程進(jìn)行控制器設(shè)計(jì)，由于誤差模型中的狀態(tài)變量無(wú)法直接量測(cè)，導(dǎo)致控制系統(tǒng)對(duì)初始值精確性較為依賴(lài)，而本專(zhuān)利針對(duì)AUV載體坐標(biāo)系建立三維路徑跟蹤誤差方程，基于迭代方法設(shè)計(jì)三維路徑跟蹤控制器避免了采用傳統(tǒng)反步法設(shè)計(jì)控制器時(shí)存在奇異值點(diǎn)，保證系統(tǒng)的全局收斂性。

發(fā)明內(nèi)容
本發(fā)明的目的在于提供一種能提高路徑跟蹤精度的基于迭代的自主水下航行器三維路徑跟蹤控制方法。本發(fā)明的目的是這樣實(shí)現(xiàn)的步驟I.初始化，給定三維空間期望跟蹤路徑參數(shù)化方程描述，給定AUV初始位置和姿態(tài)信息，給定期望跟蹤路徑參數(shù)的初始值，給定期望路徑上“虛擬向?qū)А背跏嘉恢煤统跏家苿?dòng)速度信息；步驟2.計(jì)算初始時(shí)刻AUV當(dāng)前位置與期望路徑上“虛擬向?qū)А秉c(diǎn)在A(yíng)UV載體坐標(biāo)系下的相對(duì)跟蹤誤差；步驟3.計(jì)算期望路徑上“虛擬向?qū)А秉c(diǎn)的期望移動(dòng)速度、AUV運(yùn)動(dòng)學(xué)跟蹤控制律(如縱向移動(dòng)速度、AUV的轉(zhuǎn)艏角速度和縱傾角速度虛擬控制律)； 步驟4.在運(yùn)動(dòng)學(xué)等價(jià)控制律的基礎(chǔ)上，采用迭代設(shè)計(jì)思想，推導(dǎo)欠驅(qū)動(dòng)自主水下航行器AUV的三維路徑跟蹤的動(dòng)力學(xué)控制律，即根據(jù)AUV具體水動(dòng)力數(shù)學(xué)模型解算出最終的執(zhí)行指令信號(hào)(如推進(jìn)器推力、縱傾控制力矩和轉(zhuǎn)艏控制力矩)；步驟5.計(jì)算當(dāng)前AUV位置n11 = (x, y, z)與標(biāo)定的轉(zhuǎn)向點(diǎn)WPk= (xk, yk, zk)之間的距離J = ^{x-xkf+(y-ykf+(z- J若小于設(shè)定的航跡切換半徑R，則表示完成當(dāng)前
指定路徑的跟蹤任務(wù)停止航行或切換下一個(gè)期望航跡，否則繼續(xù)步驟2。本發(fā)明相對(duì)現(xiàn)有技術(shù)具有如下的優(yōu)點(diǎn)及效果I.基于A(yíng)UV載體坐標(biāo)系下建立三維路徑跟蹤誤差方程，結(jié)合AUV的運(yùn)動(dòng)特性，避免了在期望路徑上虛擬向?qū)ё鴺?biāo)系下的AUV三維路徑跟蹤誤差方程時(shí)，將AUV假定為虛擬質(zhì)點(diǎn)，假設(shè)運(yùn)動(dòng)方向與合速度矢量方向一致，獲得的三維路徑跟蹤誤差方程需要AUV運(yùn)動(dòng)的側(cè)漂角和攻角精確可量測(cè)，這在實(shí)際應(yīng)用中是存在困難，只能夠通過(guò)對(duì)橫向和垂向運(yùn)動(dòng)速度的測(cè)量，進(jìn)而計(jì)算AUV的側(cè)漂角和攻角，而由于沿三軸線(xiàn)速度的測(cè)量較困難，導(dǎo)致最終的控制器解算存在潛在的中斷可能的不足。2.引入期望路徑上“虛擬向?qū)А秉c(diǎn)的移動(dòng)速度作為額外的控制輸入，保證實(shí)際應(yīng)用 中在具有較大跟蹤誤差時(shí)，跟蹤系統(tǒng)具有良好的動(dòng)態(tài)性能，避免控制器輸出較高的增益信號(hào)和推力飽和現(xiàn)象；采用迭代設(shè)計(jì)思想，將AUV三維路徑跟蹤控制系統(tǒng)，分為運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)兩部分設(shè)計(jì)等價(jià)控制器，基于李雅普諾夫穩(wěn)定性理論保證三維路徑跟蹤誤差閉環(huán)系統(tǒng)的穩(wěn)定性，且控制器對(duì)海洋環(huán)境作用引起的模型參數(shù)不確定性具有一定魯棒性。


圖I是本發(fā)明基于虛擬向?qū)У腁UV三維路徑跟蹤示意圖。圖2是本發(fā)明AUV三維路徑跟蹤控制器解算流程圖。圖3是本發(fā)明AUV三維路徑跟蹤非線(xiàn)性控制器框圖。圖4 10為本發(fā)明設(shè)計(jì)AUV三維曲線(xiàn)路徑跟蹤控制仿真對(duì)比曲線(xiàn)。從圖4中可以看出本發(fā)明設(shè)計(jì)控制方法在A(yíng)UV與期望跟蹤三維路徑初始距離較大時(shí)仍能實(shí)現(xiàn)精確跟蹤控制，圖5和圖6分別為AUV三維跟蹤軌跡在水平面和垂直面得投影曲線(xiàn)，可以看出跟蹤三個(gè)方向上的跟蹤誤差逐漸減小，在圖7中的AUV三維跟蹤誤差曲線(xiàn)最終收斂到零，驗(yàn)證了本發(fā)明設(shè)計(jì)控制方法的有效性；圖8 9為AUV狀態(tài)變量的變化曲線(xiàn)；圖10為AUV控制輸入響應(yīng)曲線(xiàn)。
具體實(shí)施例方式下面舉例對(duì)本發(fā)明做更詳細(xì)的描述對(duì)于步驟I中給定期望跟蹤路徑Q上的虛擬向?qū)在固定坐標(biāo)系的坐標(biāo)可以表示為某一標(biāo)量參數(shù)SGR的函數(shù)為⑷=[- ⑷，{s)J(I)為了保證被跟蹤路徑的光滑性，要求Xd(S)，yd(s)，Zd(S) 二階偏導(dǎo)數(shù)存在。定義虛擬向?qū)c(diǎn)P的速度Up方向?yàn)檠厍€(xiàn)路徑的切線(xiàn)方向與固定坐標(biāo)系水平軸的夾角Vd為￥d = arctan(y/ d/x/ d) (2)速度向量up與固定坐標(biāo)系垂直軸的夾角0d定義為
權(quán)利要求
1.一種基于迭代的自主水下航行器三維曲線(xiàn)路徑跟蹤控制方法，其特征是 步驟I.初始化，給定三維空間期望跟蹤路徑參數(shù)化方程描述，給定AUV初始位置和姿態(tài)信息，給定期望跟蹤路徑參數(shù)的初始值，給定期望路徑上“虛擬向?qū)А背跏嘉恢煤统跏家苿?dòng)速度信息； 步驟2.計(jì)算初始時(shí)刻AUV當(dāng)前位置與期望路徑上“虛擬向?qū)А秉c(diǎn)在A(yíng)UV載體坐標(biāo)系下的相對(duì)跟蹤誤差； 步驟3.計(jì)算期望路徑上“虛擬向?qū)А秉c(diǎn)的期望移動(dòng)速度、AUV運(yùn)動(dòng)學(xué)跟蹤控制律，包括縱向移動(dòng)速度、AUV的轉(zhuǎn)艏角速度和縱傾角速度虛擬控制律； 步驟4.在運(yùn)動(dòng)學(xué)等價(jià)控制律的基礎(chǔ)上，采用迭代，推導(dǎo)欠驅(qū)動(dòng)自主水下航行器AUV的三維路徑跟蹤的動(dòng)力學(xué)控制律，即根據(jù)AUV具體水動(dòng)力數(shù)學(xué)模型解算出最終的執(zhí)行指令信號(hào)，包括推進(jìn)器推力、縱傾控制力矩和轉(zhuǎn)艏控制力矩； 步驟5.計(jì)算當(dāng)前AUV位置nn= (x, y, z)與標(biāo)定的轉(zhuǎn)向點(diǎn)WPk= (xk, yk, zk)之間的距離d =^{x-xk)2 + (y-yk f+(z-zk f ,若小于設(shè)定的航跡切換半徑R，則表示完成當(dāng)前指定路徑的跟蹤任務(wù)停止航行或切換下一個(gè)期望航跡，否則繼續(xù)步驟2。
2.根據(jù)權(quán)利要求I所述的基于迭代的自主水下航行器三維曲線(xiàn)路徑跟蹤控制方法，其特征是給定期望跟蹤路徑Q上的虛擬向?qū)在固定坐標(biāo)系的坐標(biāo)表示為某一標(biāo)量參數(shù)sGR的函數(shù)為
3.根據(jù)權(quán)利要求I所述的基于迭代的自主水下航行器三維曲線(xiàn)路徑跟蹤控制方法，其特征是根據(jù)下式計(jì)算三維路徑跟蹤誤差
4.根據(jù)權(quán)利要求I所述的基于迭代的自主水下航行器三維曲線(xiàn)路徑跟蹤控制方法，其特征是根據(jù)下式分別計(jì)算運(yùn)動(dòng)學(xué)虛擬控制器 (1)期望路徑上虛擬向?qū)c(diǎn)P的期望移動(dòng)速度計(jì)算
5.根據(jù)權(quán)利要求I所述的基于迭代的自主水下航行器三維曲線(xiàn)路徑跟蹤控制方法，其特征是根據(jù)AUV具體水動(dòng)力數(shù)學(xué)模型解算出最終的執(zhí)行指令信號(hào)的具體過(guò)程為 根據(jù)AUV實(shí)測(cè)水動(dòng)力系數(shù)，忽略橫搖運(yùn)動(dòng)對(duì)模型的影響，得到AUV自由度數(shù)學(xué)模型 如下
全文摘要
本發(fā)明提供的是一種基于迭代的自主水下航行器三維曲線(xiàn)路徑跟蹤控制方法。步驟1.初始化；步驟2.計(jì)算初始時(shí)刻AUV當(dāng)前位置與期望路徑上“虛擬向?qū)А秉c(diǎn)在A(yíng)UV載體坐標(biāo)系下的相對(duì)跟蹤誤差；步驟3.計(jì)算期望路徑上“虛擬向?qū)А秉c(diǎn)的期望移動(dòng)速度、AUV運(yùn)動(dòng)學(xué)跟蹤控制律；步驟4.在運(yùn)動(dòng)學(xué)等價(jià)控制律的基礎(chǔ)上，采用迭代，推導(dǎo)欠驅(qū)動(dòng)自主水下航行器AUV的三維路徑跟蹤的動(dòng)力學(xué)控制律；步驟5.計(jì)算當(dāng)前AUV位置ηn＝(x,y,z)與標(biāo)定的轉(zhuǎn)向點(diǎn)WPk＝(xk,yk,zk)之間的距離若小于設(shè)定的航跡切換半徑R，則表示完成當(dāng)前指定路徑的跟蹤任務(wù)停止航行或切換下一個(gè)期望航跡，否則繼續(xù)步驟2。本發(fā)明能夠提高AUV的路徑跟蹤精度。
文檔編號(hào)G05D1/10GK102768539SQ201210211449
公開(kāi)日2012年11月7日 申請(qǐng)日期2012年6月26日 優(yōu)先權(quán)日2012年6月26日
發(fā)明者于樂(lè), 呂洪莉, 王宏健, 陳興華, 陳子印 申請(qǐng)人:哈爾濱工程大學(xué)