欠驅(qū)動(dòng)自主水下航行器航跡追蹤控制

陳 偉,蘆 海,楊 雯,吳青帥,冷建興*

(1.浙江大學(xué) 海洋學(xué)院，舟山 316021)(2.海南聯(lián)網(wǎng)二回項(xiàng)目管理有限公司,海口 570100)(3.中國(guó)南方電網(wǎng)超高壓輸電公司,海口 570206)

自主水下航行器作為高技術(shù)海洋工程裝備在大海作業(yè)中應(yīng)用廣泛，不僅可以在深海海底進(jìn)行海洋觀察和資源探測(cè)，也可用于近海巡查海底電纜和維護(hù)石油管道.自主水下航行器的發(fā)展具有廣泛的研究意義，發(fā)展前景廣闊美好，如今依然存在一些挑戰(zhàn).水下航行器要想實(shí)現(xiàn)無(wú)人無(wú)線遠(yuǎn)距離自主航行，對(duì)于控制策略有著極高的要求，尤其應(yīng)該注意它在海中運(yùn)行時(shí)候具有模型非線性、高耦合、海洋環(huán)境隨時(shí)間變化、波浪海流干擾不確定性等特點(diǎn)[1-2].

航跡控制需要水下航行器追蹤一條隨時(shí)間變化預(yù)先設(shè)計(jì)好的航行軌跡，必要情況下需要對(duì)起始和到達(dá)時(shí)候的速度有所要求.對(duì)于水下航行器的常見(jiàn)控制方法有PID控制、滑模控制、反步控制和模糊控制等，文獻(xiàn)[3]通過(guò)實(shí)驗(yàn)和仿真的辦法對(duì)幾種常見(jiàn)控制策略進(jìn)行比較，并且提出一種改進(jìn)型滑模控制器.水下航行器面對(duì)的外界干擾難以準(zhǔn)確實(shí)時(shí)測(cè)量，文獻(xiàn)[4]基于擴(kuò)展型觀測(cè)器和高階滑模控制提出一種自適應(yīng)干擾觀測(cè)器，有效應(yīng)對(duì)時(shí)變不確定性干擾.文獻(xiàn)[5]考慮到墻壁等邊界對(duì)航行器的干擾，提出一種慢速時(shí)變干擾觀測(cè)器.文獻(xiàn)[6]考慮到位置約束，并對(duì)欠驅(qū)動(dòng)航行器設(shè)計(jì)一個(gè)主動(dòng)反步法控制器抑制干擾影響.文獻(xiàn)[7]提出了一種有限時(shí)間觀察器，能有效解決模型中部分動(dòng)力未知的困難.為了對(duì)動(dòng)力學(xué)不確定參數(shù)和外界干擾起到自適應(yīng)補(bǔ)償?shù)男Ч墨I(xiàn)[8]設(shè)計(jì)一種滑模自適應(yīng)控制器，文獻(xiàn)[9]結(jié)合視線法設(shè)計(jì)非線性干擾觀測(cè)器,文獻(xiàn)[10]設(shè)計(jì)一種雙回路積分滑模控制器.文獻(xiàn)[11]在反步法和滑模控制的基礎(chǔ)上，通過(guò)降維擴(kuò)展型狀態(tài)觀測(cè)器對(duì)外部擾動(dòng)做出補(bǔ)償，也考慮到輸入的有界性.文獻(xiàn)[12]基于導(dǎo)航誤差設(shè)計(jì)了一種魯棒性較強(qiáng)的模糊自適應(yīng)控制器，可以兼顧執(zhí)行器存在飽和現(xiàn)象的限制條件.文獻(xiàn)[13]在反步法控制器的基礎(chǔ)上考慮到水下航行器在全驅(qū)動(dòng)和欠驅(qū)動(dòng)系統(tǒng)之間平滑切換的調(diào)節(jié)過(guò)程.文獻(xiàn)[14]設(shè)計(jì)了終端滑模控制方案，最后在關(guān)聯(lián)擾動(dòng)下表現(xiàn)出良好的穩(wěn)定性和魯棒性.文獻(xiàn)[15]針對(duì)欠驅(qū)動(dòng)水下航行器系統(tǒng)設(shè)計(jì)了一個(gè)自適應(yīng)快速非奇異終端滑模控制方案.文獻(xiàn)[16]基于反步法控制和仿生速度準(zhǔn)則將實(shí)際航行器速度變化變得更為平滑.文獻(xiàn)[17]在反步滑模控制基礎(chǔ)上，使用模糊控制的算法獲得滑模增益系數(shù)，最終取得良好的魯棒性.文獻(xiàn)[18]利用基于李亞普諾夫模型的在線預(yù)測(cè)系統(tǒng)有效提高了控制器的表現(xiàn)性能.

文中研究的水下航行器屬于欠驅(qū)動(dòng)控制系統(tǒng)，主要對(duì)二維平面內(nèi)三自由度系統(tǒng)航跡控制進(jìn)行探討.針對(duì)其中運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)間的各自由度狀態(tài)量相互耦合情況，對(duì)耦合項(xiàng)進(jìn)行建立數(shù)學(xué)模型，并以此為控制建模基礎(chǔ)；針對(duì)航行器控制過(guò)程中需要考慮的坐標(biāo)系、模型參數(shù)和系統(tǒng)狀態(tài)量進(jìn)行定義和模型確定；針對(duì)控制系統(tǒng)的設(shè)計(jì)思路、控制方程的確切形式和系統(tǒng)穩(wěn)定性的理論驗(yàn)證；針對(duì)控制系統(tǒng)進(jìn)行仿真，探究其控制性能并驗(yàn)證理論推導(dǎo).

1 航行器模型

航行器總共擁有6個(gè)自由度，包含x、y、z3個(gè)平動(dòng)自由度和ζx、ζy、ζz3個(gè)旋轉(zhuǎn)自由度.但文中主要考慮航行器在平面運(yùn)動(dòng)過(guò)程中的二維路徑跟蹤控制，也就是x、y、ζz3個(gè)自由度.一般來(lái)說(shuō)，水下航行器裝備尾部螺旋槳或噴水裝置作為船身方向驅(qū)動(dòng)器，通過(guò)尾鰭等裝置作為船體平面內(nèi)旋轉(zhuǎn)驅(qū)動(dòng)器.兩個(gè)驅(qū)動(dòng)器卻要用來(lái)控制平面內(nèi)3個(gè)自由度，也就導(dǎo)致了欠驅(qū)動(dòng)的難題.

在模型建立之前，先通過(guò)圖1介紹文中采用的3個(gè)坐標(biāo)系{e}、{b}、g0gggggg.大地坐標(biāo)系{e}是慣性坐標(biāo)系，坐標(biāo)系原點(diǎn)是Oe，坐標(biāo)軸分別是xyz；航行器實(shí)際運(yùn)動(dòng)坐標(biāo)系{b}隨船體本身運(yùn)動(dòng)，坐標(biāo)原點(diǎn)為船體重心Ob,坐標(biāo)軸為ubwb；理想狀態(tài)船體坐標(biāo)系g0gggggg以位于理想軌跡上的航行器重心作為原點(diǎn)Od，坐標(biāo)軸為xdxd.在不同坐標(biāo)系中的向量坐標(biāo)表示通過(guò)右下角下標(biāo)標(biāo)識(shí)，若無(wú)標(biāo)識(shí)則默認(rèn)處于慣性坐標(biāo){e}.

圖1 航行器運(yùn)動(dòng)狀態(tài)和坐標(biāo)系Fig.1 AUV′s motion state and coordinate system

其中ED、EB、BD分別表示坐標(biāo)系原點(diǎn)間位移量，顯然可得：

ED=EB+BD

(1)

大地坐標(biāo)系{e}和理想航行器姿態(tài)坐標(biāo)系g0gggggg間坐標(biāo)轉(zhuǎn)換矩陣R為：

(2)

式中：ζD為x軸與xd軸夾角.

將式(1)對(duì)于時(shí)間t進(jìn)行求導(dǎo)可以得到：

(3)

兩邊同時(shí)乘上坐標(biāo)轉(zhuǎn)換矩陣R得到：

(4)

(5)

(6)

(7)

式中：ζD為x軸與xd軸夾角；xp、yp為理想路徑P的橫縱坐標(biāo)；c(s)為理想路徑曲率；s為航行器路程.

ED、EB、BD分別在坐標(biāo)系g0gggggg或大地坐標(biāo)系{e}中可以表示為：

(8)

式中xd、yd分別為航行器實(shí)際位置與理想路徑位置在xdyd兩軸方向上的距離.

結(jié)合公式(5、8)可以得到：

(9)

將式(2、8)帶入到式(9)可得：

(10)

經(jīng)過(guò)化簡(jiǎn)整理之后得到：

(11)

在大地坐標(biāo)系之中可得：

(12)

式中：U為航行器速度矢量大小；r為航行器旋轉(zhuǎn)角速度；β為航行器水平和豎直方向速度矢量間的夾角.顯然有：

(13)

式中u、v分別為航行器速度U在航行器實(shí)際運(yùn)動(dòng)坐標(biāo)系中向ub、wb軸方向上投影的分速度.

將式(12、13)整合可以得到：

(14)

(15)

(16)

如圖1，ωU為速度U與x軸夾角ζU變化角速度；ωD為x軸與xd軸夾角ζD變化角速度.

式(15)為航行器的運(yùn)動(dòng)學(xué)模型，左邊變量分別為航行器位置姿態(tài)和理想路徑情況之間的誤差.

觀察運(yùn)動(dòng)學(xué)模型可知，位置誤差的變化速度由航行器本身速度狀態(tài)和預(yù)設(shè)理想路徑變化情況共同決定.此時(shí)，預(yù)設(shè)理想路徑是已知信息但無(wú)法在控制過(guò)程中改變，控制器需要做的是通過(guò)改變航行器的速度矢量來(lái)達(dá)到理想位置.

然后具體到位置和狀態(tài)變化的表達(dá)，都是由該自由度方向上的分速度來(lái)決定.

在只考慮平面內(nèi)三自由度情況，對(duì)于航行器動(dòng)力學(xué)關(guān)系有[19]：

(17)

式中：u、v、r分別為航行器速度U在航行器實(shí)際運(yùn)動(dòng)坐標(biāo)系中向ub、wb軸方向上投影的分速度和航行器本身旋轉(zhuǎn)角速度；m1、m2、m3為考慮附加質(zhì)量等情況下質(zhì)量矩陣中的3個(gè)等效質(zhì)量參數(shù)；d1、d2、d3為航行器3個(gè)自由度方向上的阻尼系數(shù)；τuτr為驅(qū)動(dòng)器施加在航行器上的驅(qū)動(dòng)力或力矩.Fd1、Fd2、Fd3為3個(gè)自由度上的外界干擾.

第二，從宏觀的角度上進(jìn)行分析。對(duì)于一個(gè)國(guó)家企業(yè)的金融結(jié)構(gòu)基本情況一般可以從產(chǎn)業(yè)結(jié)構(gòu)、融資結(jié)構(gòu)、資產(chǎn)結(jié)構(gòu)以及市場(chǎng)和金融的開(kāi)放這五個(gè)主要的方面進(jìn)行分析。由此就能夠看出來(lái)該結(jié)構(gòu)的演變過(guò)程就是一個(gè)國(guó)家金融結(jié)構(gòu)的演變以及市場(chǎng)結(jié)構(gòu)的變化還有資產(chǎn)在整個(gè)國(guó)民經(jīng)濟(jì)發(fā)展中的比重等等，它的演變過(guò)程是一個(gè)國(guó)家金融發(fā)展的過(guò)程，是一種動(dòng)態(tài)過(guò)程，圖1是其演變和我國(guó)人均GDP的變化曲線。

觀察平面內(nèi)運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)方程，會(huì)發(fā)現(xiàn)雖然3個(gè)自由度上的位置變化是互不干擾的，但是在動(dòng)力學(xué)方面某一自由度上的速度會(huì)對(duì)另外一個(gè)自由度上的加速度有所影響.可見(jiàn)，模型之中存在相互耦合的特點(diǎn)，主要的體現(xiàn)是在加速度受到其他自由度上運(yùn)動(dòng)狀態(tài)影響.

2 控制器模型

在前面模型建立的基礎(chǔ)上進(jìn)行控制器設(shè)計(jì)，控制器設(shè)計(jì)的思路是采用視線法和反步法設(shè)計(jì)出航行器運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)控制模型.其中在設(shè)計(jì)位置控制器的時(shí)候，不妨假設(shè)航行器速度ud恒定，并以此為基礎(chǔ)得到理想的姿態(tài)角δ和角速度rd.

由于控制目標(biāo)是希望航行器按照預(yù)設(shè)的路徑進(jìn)行跟蹤運(yùn)動(dòng)，所以最終呈現(xiàn)出來(lái)的是位置控制.位置變化狀態(tài)方程式(15)的輸入是速度和角速度.這里首先希望航行器角度ζ能夠與預(yù)設(shè)姿態(tài)角δ趨向一致，不妨令李雅普諾夫函數(shù)V1為：

(18)

對(duì)V1進(jìn)行求導(dǎo)可以得到：

(19)

(20)

代入式(19)之后整理得到：

(21)

在確定目標(biāo)角速度rd作為角度姿態(tài)的狀態(tài)方程控制輸入之后，還需要確定位置狀態(tài)方程的控制輸入使得位置誤差隨時(shí)間增長(zhǎng)而穩(wěn)定趨向于零.于是不妨令李雅普諾夫函數(shù)V2為：

(22)

對(duì)V2進(jìn)行求導(dǎo)可以得到：

(23)

(24)

(25)

代入之后整理得到：

綜合公式(20，24，25)得到運(yùn)動(dòng)學(xué)控制方程：

(26)

在獲得理想的位姿角δ和角速度rd結(jié)果之后，實(shí)際控制輸入的是驅(qū)動(dòng)器的力或者力矩.這就需要通過(guò)對(duì)航行器動(dòng)力學(xué)進(jìn)行控制器設(shè)計(jì)，令航行器實(shí)際旋轉(zhuǎn)角度ζ和旋轉(zhuǎn)角速度r與預(yù)設(shè)的姿態(tài)角δ和角速度rd分別對(duì)應(yīng)一致，就可以滿足路徑跟蹤的需求.

因此，不妨令李雅普諾夫函數(shù)V3為：

(27)

對(duì)V3進(jìn)行求導(dǎo)可以得到：

(28)

(29)

代入之后整理得到：

(30)

通過(guò)反步法的思路去尋找設(shè)計(jì)合適的控制器，在動(dòng)力學(xué)控制環(huán)節(jié)考慮到動(dòng)力學(xué)各個(gè)自由度之間耦合的情況.將耦合項(xiàng)中基本信息u、v、r通過(guò)航行器上傳感器進(jìn)行測(cè)量得到，在輸入力或力矩之中做一個(gè)補(bǔ)償將耦合項(xiàng)作用抵消，以達(dá)到抗耦合干擾的效果.

綜上所述，在理論上控制輸入符合式(26，29)即可以保證系統(tǒng)的穩(wěn)定性，并滿足水下航行器對(duì)路線進(jìn)行跟蹤的需求，至于實(shí)際運(yùn)用效果還需要仿真和實(shí)際試驗(yàn)來(lái)進(jìn)行進(jìn)一步的驗(yàn)證和改進(jìn).

3 仿真分析

對(duì)控制器進(jìn)行仿真分析，對(duì)航行器控制模型的參數(shù)進(jìn)行適當(dāng)選取整理之后得到航行器模型為：

式中：Fd1、Fd2、Fd3選擇方差為0.005的高斯白噪聲；τu、τr屬于控制輸入的力或者力矩.

對(duì)控制器參數(shù)進(jìn)行取值，具體數(shù)值大小為：

k3=2，k4=1.25，k5=1

3.1 三角函數(shù)型路徑

初始條件設(shè)計(jì)：

r=0.1，u=0.7，x=-2.9，

y=8，ζ=0.3

預(yù)設(shè)理想路線：

xp=s-1，yp=6cos(0.2s)

yp=6cos(0.2(xp+1)).

在圖2中，起始時(shí)候水下航行器與理想起點(diǎn)存在差距，這是和設(shè)定的初始橫向位置x=-2.9，縱向位置y=8一致的.后面可以明顯觀察到水下航行器的實(shí)際路線逐漸基本和理想路線保持一致，并且在終止時(shí)刻和理想狀態(tài)趨向一致.其中圖3、4專門將xd、yd隨時(shí)間t的變化趨勢(shì)進(jìn)行比較，和前面路線圖結(jié)果對(duì)照一致.經(jīng)過(guò)了大概6～7 s，在xd軸方向上誤差趨向于零，后面基本穩(wěn)定于零附近波動(dòng)；經(jīng)過(guò)了大概10～12 s，在yd軸方向上誤差趨向于零，后面基本穩(wěn)定于零附近波動(dòng).圖5顯示的是每一時(shí)刻，航行器實(shí)際位置與該時(shí)刻理想路線點(diǎn)位置的絕對(duì)距離d.大約在10 s，航行器絕對(duì)位置誤差趨向于零，并在后續(xù)運(yùn)動(dòng)中基本穩(wěn)定地在零附近波動(dòng).可見(jiàn)根據(jù)仿真結(jié)果顯示，文中設(shè)計(jì)的控制器符合預(yù)期設(shè)計(jì)初衷，也滿足水下航行器自主航跡追蹤的需求.

圖2 航行器實(shí)際路徑和理想路徑對(duì)比Fig.2 Comparison of actual path and ideal path of AUV

圖3 航行器位置誤差xdFig.3 AUV′s error xd

圖4 航行器位置誤差ydFig.4 AUV′s Error yd

圖5 航行器位置點(diǎn)和理想路線位置點(diǎn)的距離誤差Fig.5 Distance error between the position point of the vehicle and the position point of the ideal route

3.2 線性函數(shù)型和圓形路徑

初始條件設(shè)計(jì)：

r=0.1，u=0.7，x=1，y=12.5，ζ=0.3

預(yù)設(shè)理想路線：

路徑以時(shí)間為標(biāo)準(zhǔn)從上到下依次分為三段，預(yù)先設(shè)定航行器從點(diǎn)(0,10)直線行進(jìn)到點(diǎn)(30,40),再行進(jìn)到點(diǎn)(40,40)，然后以點(diǎn)(0,10)為圓心做直徑為10的圓形環(huán)繞運(yùn)行.

在圖6中，起始時(shí)候水下航行器與理想起點(diǎn)存在差距，這是和設(shè)定的初始位置x=1、y=12.5一致的.后面可以明顯觀察到水下航行器的實(shí)際路線逐漸基本和理想路線保持一致，并且在最后一段圍繞預(yù)設(shè)圓心做圓形環(huán)繞運(yùn)動(dòng).其中圖7、8專門將xd、yd隨時(shí)間的變化趨勢(shì)進(jìn)行比較，和前面路線圖結(jié)果對(duì)照一致.經(jīng)過(guò)了7 s左右，在xd軸方向上誤差趨向于零，中途在82 s左右有一個(gè)小的波動(dòng)，后面基本穩(wěn)定于零附近；經(jīng)過(guò)了14 s左右，在yd軸方向上誤差趨向于零，中途分別在30、82 s左右有一個(gè)小的波動(dòng)，后面基本穩(wěn)定于零附近波動(dòng).圖9顯示的是每一時(shí)刻，航行器實(shí)際位置與該時(shí)刻理想路線點(diǎn)位置的絕對(duì)距離.在12 s左右，航行器絕對(duì)位置誤差趨向于零，中途分別在30、82 s左右有一個(gè)小的波動(dòng)，并在后續(xù)運(yùn)動(dòng)中基本穩(wěn)定地在零附近波動(dòng).分析仿真結(jié)果可以發(fā)現(xiàn)，中間出現(xiàn)的小波動(dòng)主要是在兩段不同路徑轉(zhuǎn)折的時(shí)間點(diǎn)出現(xiàn)，也能夠有效穩(wěn)定地逐漸趨向于零.可見(jiàn)根據(jù)仿真結(jié)果顯示，文中設(shè)計(jì)的控制器符合預(yù)期設(shè)計(jì)初衷，也滿足水下航行器自主航跡追蹤的需求.

圖6 航行器實(shí)際路徑和理想路徑對(duì)比Fig.6 Comparison of actual path and ideal path of AUV

圖7 航行器位置誤差xdFig.7 AUV′s error xd

圖8 航行器位置誤差ydFig.8 AUV′s error yd

圖9 航行器位置點(diǎn)和理想路線位置點(diǎn)的距離誤差Fig.9 Distance error between the position point of the vehicle and the position point of the ideal route

4 結(jié)論

(1)在模型基礎(chǔ)上進(jìn)行了非線性控制器設(shè)計(jì)，通過(guò)視線法和反步法設(shè)計(jì)控制模型結(jié)構(gòu)，基本在理論上解決模型存在部分耦合項(xiàng)的困境.最終對(duì)于模型進(jìn)行數(shù)值仿真驗(yàn)證了控制模型的有效性.

(2)在滿足目標(biāo)要求的前提下，后續(xù)依然有待進(jìn)一步的裝置試驗(yàn)進(jìn)行驗(yàn)證和改進(jìn).航行器本身受到的外界干擾難以實(shí)時(shí)準(zhǔn)確測(cè)量，后續(xù)可以進(jìn)一步考慮設(shè)計(jì)觀測(cè)器進(jìn)行預(yù)測(cè).對(duì)于某些控制中間項(xiàng)的上下限缺乏一定的選擇，可能與實(shí)際工作中的執(zhí)行器工作區(qū)間有所不符.