基于拉蓋爾函數(shù)的AUV自適應(yīng)預(yù)測(cè)軌跡跟蹤

閆景昊， 王偉然， 楊冠軍， 朱志宇， 曾慶軍， 智鵬飛

(江蘇科技大學(xué)電子信息學(xué)院，江蘇 鎮(zhèn)江 212000)

0 引言

無(wú)人自治水下機(jī)器人(AUV)作為人類(lèi)探索和開(kāi)發(fā)海洋的利器，已經(jīng)引起世界各國(guó)研究者的廣泛關(guān)注[1]。近年來(lái)，AUV已被廣泛應(yīng)用于科學(xué)、工業(yè)、商業(yè)和軍事等領(lǐng)域。由于工作環(huán)境中存在復(fù)雜的外部干擾，所以在這種情況下運(yùn)行時(shí)，AUV要求運(yùn)動(dòng)控制器具有較強(qiáng)的抗干擾能力和快速的反應(yīng)能力。但是，運(yùn)動(dòng)控制過(guò)程中如數(shù)據(jù)計(jì)算量過(guò)于龐大，會(huì)導(dǎo)致AUV響應(yīng)速度過(guò)慢，嚴(yán)重時(shí)會(huì)偏離預(yù)設(shè)軌跡甚至于失聯(lián)。如何減小數(shù)據(jù)計(jì)算量，提高AUV的控制精度、動(dòng)態(tài)性能以及復(fù)雜環(huán)境下的適應(yīng)能力，成為當(dāng)前研究的熱點(diǎn)[2-4]。

目前，多數(shù)關(guān)于軌跡跟蹤控制文獻(xiàn)[5-7]中，常用的控制方法有滑?？刂芠8-9]、反步控制[10-11]、自適應(yīng)控制[12-13]等。復(fù)雜水文條件下水下機(jī)器人軌跡跟蹤時(shí)，物理模型的限制會(huì)對(duì)控制器性能造成一定影響，而上述幾種控制方法均沒(méi)有涉及此問(wèn)題。模型預(yù)測(cè)控制(MPC) 可以將控制約束和狀態(tài)約束納入優(yōu)化計(jì)算。此外，其可以運(yùn)用于線(xiàn)性系統(tǒng)和非線(xiàn)性系統(tǒng)對(duì)象的控制器設(shè)計(jì)，在水下機(jī)器人軌跡跟蹤中具有廣泛的應(yīng)用[14-15]。ZHANG等[16]設(shè)計(jì)了一種基于MPC的三維軌跡跟蹤方法，針對(duì)AUV的軌跡跟蹤控制有較好的效果，對(duì)MPC在A(yíng)UV中的軌跡跟蹤有一定的參考價(jià)值；YAN等[17]基于MPC設(shè)計(jì)了雙閉環(huán)三維軌跡跟蹤算法，外環(huán)運(yùn)動(dòng)學(xué)控制器生成速度指令后傳遞給內(nèi)環(huán)動(dòng)力學(xué)控制器進(jìn)一步生成控制力矩，并考慮系統(tǒng)的輸入和狀態(tài)約束，但是未考慮外部水流等復(fù)雜干擾對(duì)模型參數(shù)的影響;PENG等[18]設(shè)計(jì)了擴(kuò)展?fàn)顟B(tài)觀(guān)測(cè)器來(lái)估計(jì)位置的內(nèi)部動(dòng)態(tài)特性和外部干擾，但是只在二維垂直面進(jìn)行了驗(yàn)證，并未考慮三維空間內(nèi)存在復(fù)雜干擾時(shí)的運(yùn)動(dòng)情況；文獻(xiàn)[19-20]針對(duì)具有參數(shù)不確定性和擾動(dòng)的離散線(xiàn)性系統(tǒng)設(shè)計(jì)了自適應(yīng)預(yù)測(cè)控制算法，對(duì)帶有參數(shù)不確定性的AUV系統(tǒng)具有一定的參考價(jià)值；文獻(xiàn)[21-22]針對(duì)預(yù)測(cè)控制器的動(dòng)態(tài)響應(yīng)能力設(shè)計(jì)了基于拉蓋爾函數(shù)的MPC，極大地提高了計(jì)算速度，但并未考慮系統(tǒng)參數(shù)不確定性對(duì)控制效果的影響。從以上分析中可以看出，參數(shù)不確定性和龐大的計(jì)算量在一定程度上限制了MPC控制器的性能。因此，如何補(bǔ)償參數(shù)不確定性和減小計(jì)算量提高響應(yīng)速度對(duì)AUV的軌跡跟蹤具有重要意義。

本文設(shè)計(jì)了一種基于拉蓋爾函數(shù)的AUV自適應(yīng)預(yù)測(cè)軌跡跟蹤控制方法。首先，建立AUV在三維空間內(nèi)的運(yùn)動(dòng)控制模型，引入預(yù)測(cè)控制方法，設(shè)計(jì)基于預(yù)測(cè)控制器的AUV控制模型；然后，為了消除AUV模型在近似線(xiàn)性化過(guò)程中的誤差和軌跡調(diào)整時(shí)參數(shù)變化所導(dǎo)致的魯棒性下降，在預(yù)測(cè)控制算法的基礎(chǔ)上，引入遞推最小二乘法設(shè)計(jì)自適應(yīng)算法，實(shí)時(shí)在線(xiàn)估算參數(shù)的變化量，實(shí)現(xiàn)自適應(yīng)預(yù)測(cè)控制的參數(shù)調(diào)整。此外，水下環(huán)境存在復(fù)雜的外部水流干擾以及AUV自身預(yù)設(shè)跟蹤軌跡的大幅度改變，這些均需要大量的控制量求解運(yùn)算。本文利用拉蓋爾函數(shù)正交化的特點(diǎn)，將數(shù)量較多的被優(yōu)化控制輸入轉(zhuǎn)化為數(shù)量較少的拉蓋爾系數(shù)，犧牲部分精確性，但是能極大程度減小預(yù)測(cè)控制中的計(jì)算量，從而大幅提高系統(tǒng)的響應(yīng)速度。

1 水下機(jī)器人運(yùn)動(dòng)建模

本文所述的AUV在結(jié)構(gòu)上關(guān)于軸線(xiàn)上下、左右對(duì)稱(chēng)。主推進(jìn)器置于A(yíng)UV艉部，用于實(shí)現(xiàn)對(duì)AUV的縱蕩運(yùn)動(dòng)控制，首尾各兩個(gè)垂向與舷向輔助推進(jìn)器可以實(shí)現(xiàn)對(duì)縱搖艏搖的控制。忽略安裝誤差，通過(guò)主/輔推進(jìn)器的配合，AUV在空間上具有全驅(qū)運(yùn)動(dòng)的特性。AUV在大地坐標(biāo)系中的運(yùn)動(dòng)由6個(gè)自由度組成，即描述直線(xiàn)運(yùn)動(dòng)的3個(gè)平移分量(縱蕩x、橫蕩y、垂蕩z)，描述旋轉(zhuǎn)運(yùn)動(dòng)的3個(gè)旋轉(zhuǎn)分量(橫搖角φ、縱搖角θ和艏搖角ψ)。AUV的體坐標(biāo)系的原點(diǎn)與重心重合，在體坐標(biāo)系下的運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)模型方程分別為

(1)

(2)

(3)

2 AUV軌跡跟蹤控制策略

2.1 軌跡跟蹤預(yù)測(cè)控制模型

設(shè)采樣時(shí)間為T(mén)，在任意時(shí)刻k對(duì)式(3)離散化后得到線(xiàn)性時(shí)不變狀態(tài)方程為

(4)

(5)

其中:a(k)=I5+aT,I5為五階單位矩陣;b(k)=bT;c=diag(I3，02),I3為三階單位矩陣，02為二階零矩陣。

(6)

在實(shí)際運(yùn)動(dòng)過(guò)程中，AUV受到自身轉(zhuǎn)角以及推進(jìn)系統(tǒng)中電機(jī)轉(zhuǎn)速的限制，狀態(tài)向量和輸入向量必須始終在一定的范圍內(nèi)，即

(7)

式中，Umin,Umax和Xmin,Xmax分別為輸入量與狀態(tài)量的下限和上限。優(yōu)化控制器可以使AUV快速且平穩(wěn)地追蹤期望軌跡，傳統(tǒng)預(yù)測(cè)控制中帶有約束的優(yōu)化目標(biāo)函數(shù)為

J=||

CA(k)x(k)+

(8)

s.t.Umin≤U(k)≤Umax,Xmin≤X(k)≤Xmax

式中:Yd(k)為給定的參考輸出,Y(k)=[yd(k+1|k)yd(k+2|k)…yd(k+Np|k)]T；Q=diag(Q1,Q2,…,QNP),為誤差控制加權(quán)矩陣；R=diag(R1,R2,…,RNc),為輸入控制加權(quán)矩陣。由式(8)還能看出，控制器的控制量計(jì)算添加了AUV速度和狀態(tài)的約束，使得優(yōu)化求解變得復(fù)雜，降低了控制器的計(jì)算速度。

2.2 自適應(yīng)預(yù)測(cè)控制器

由于MPC控制器在設(shè)計(jì)時(shí)，非線(xiàn)性的AUV運(yùn)動(dòng)方程在參考軌跡點(diǎn)進(jìn)行了近似線(xiàn)性化，從預(yù)測(cè)模型可以看出模型參數(shù)隨時(shí)間變化劇烈，這導(dǎo)致控制器魯棒性變差，難以取得良好的控制效果。為了解決上述問(wèn)題，本文根據(jù)遞推最小二乘法設(shè)計(jì)自適應(yīng)更新算法[23-24]。在每個(gè)控制周期開(kāi)始時(shí)更新修正控制模型，使預(yù)測(cè)模型方程可以更好地描述水下機(jī)器人實(shí)時(shí)軌跡跟蹤時(shí)的狀態(tài)。

根據(jù)系統(tǒng)預(yù)測(cè)方程式(6)設(shè)計(jì)估計(jì)系統(tǒng)，即

(9)

將預(yù)測(cè)方程和估計(jì)系統(tǒng)方程重寫(xiě)為緊湊形式，即

x(k+1)=Θ(k)Φ(k)

(10)

(11)

(12)

(13)

(14)

采用梯度法使代價(jià)函數(shù)最小化可得

(15)

式中，λ>0,為自適應(yīng)更新率。此外，為了使系統(tǒng)帶有約束的目標(biāo)函數(shù)式(8)成立，λ還要滿(mǎn)足

(16)

式中:0<γ<2；Umax為控制量U的上限；Φmax為不確定參數(shù)Φ的上界。

2.3 基于拉蓋爾函數(shù)的自適應(yīng)預(yù)測(cè)控制器

由于A(yíng)UV控制系統(tǒng)采樣迅速，要實(shí)現(xiàn)更好的控制效果和穩(wěn)定性,往往在設(shè)計(jì)預(yù)測(cè)控制器時(shí)采用更長(zhǎng)的預(yù)測(cè)時(shí)域和控制時(shí)域,這樣就不可避免地在每個(gè)周期內(nèi)都存在著龐大的計(jì)算量。因此設(shè)計(jì)基于拉蓋爾函數(shù)的預(yù)測(cè)控制，通過(guò)參數(shù)化控制序列，減少預(yù)測(cè)時(shí)域內(nèi)約束的數(shù)量，從而減少每個(gè)控制周期內(nèi)涉及的參數(shù)數(shù)量，以達(dá)到優(yōu)化和減少控制器的運(yùn)算量、提高系統(tǒng)整體的計(jì)算速度、增強(qiáng)實(shí)時(shí)性的目的[25]。拉蓋爾函數(shù)的z傳遞函數(shù)為

(17)

式中:N為拉蓋爾函數(shù)的階數(shù)；α為拉蓋爾函數(shù)的極點(diǎn)，稱(chēng)為比例因子，取值范圍為0≤α<1；z為z變換中的相關(guān)變量。將式(17)中各項(xiàng)進(jìn)行逆z變換后得到離散時(shí)間拉蓋爾函數(shù)

L(k)=[l1(k)l2(k)l3(k)…lN(k)]T

(18)

式中,l1(k)表示Γ1(z)的逆z變換，即l1(k)=z-1·(Γ1(z))。式(18)的遞推形式可寫(xiě)為

L(k+1)=AlL(k)

(19)

式中:

(20)

特別地，當(dāng)α=0時(shí)，式(20)所示的拉蓋爾函數(shù)轉(zhuǎn)化為脈沖函數(shù)。假設(shè)當(dāng)前時(shí)刻為k0，則在ki時(shí)刻后的控制量可以由拉蓋爾函數(shù)表示為

(21)

即

(22)

由式(22)可知，通過(guò)選擇合適的α，N參數(shù)值，能在保證系統(tǒng)動(dòng)態(tài)性能的同時(shí)減少設(shè)計(jì)參數(shù)的數(shù)量，從而減小計(jì)算量。

此時(shí)，基于拉蓋爾函數(shù)預(yù)測(cè)第m步的輸出變量為

(23)

根據(jù)式 (22),(23)對(duì)傳統(tǒng)預(yù)測(cè)控制的優(yōu)化目標(biāo)函數(shù)式(8)進(jìn)行重構(gòu)可得到

(24)

式中，利用拉蓋爾函數(shù)的標(biāo)準(zhǔn)正交性質(zhì)

(25)

根據(jù)拉蓋爾函數(shù)的標(biāo)準(zhǔn)正交性，當(dāng)預(yù)測(cè)時(shí)域Np足夠大時(shí),有

(26)

(27)

通過(guò)使用拉蓋爾函數(shù)，可以減少預(yù)測(cè)范圍內(nèi)約束的數(shù)量，從而減小計(jì)算負(fù)荷。

3 仿真結(jié)果及分析

為了驗(yàn)證本文所提方法的有效性和可靠性，采用了Matlab/Simulink軟件對(duì)AUV在三維空間內(nèi)的運(yùn)動(dòng)來(lái)完成仿真。

3.1 三維曲線(xiàn)軌跡跟蹤仿真

本文首先進(jìn)行三維折線(xiàn)軌跡跟蹤仿真。三維折線(xiàn)參考軌跡為

(28)

設(shè)水下機(jī)器人的初始位置在大地坐標(biāo)系中為坐標(biāo)原點(diǎn)，對(duì)應(yīng)的初始狀態(tài)即為[00000]T。

基于拉蓋爾函數(shù)的自適應(yīng)預(yù)測(cè)控制器(LF-AMPC)、自適應(yīng)預(yù)測(cè)控制器(AMPC)、傳統(tǒng)預(yù)測(cè)控制器(MPC)在復(fù)雜干擾下的三維折線(xiàn)軌跡跟蹤性能如圖1所示，其細(xì)節(jié)如圖2所示。

圖1 復(fù)雜干擾下的三維折線(xiàn)軌跡跟蹤仿真Fig.1 Simulation of three-dimensional,broken-line trajectory tracking under complex interference

圖2 復(fù)雜干擾下的三維折線(xiàn)軌跡跟蹤仿真細(xì)節(jié)Fig.2 Simulation details of three-dimensional,broken-line trajectory tracking under complex interference

從圖1中可看出，在存在復(fù)雜干擾的情況下進(jìn)行三維折線(xiàn)軌跡跟蹤任務(wù)時(shí)，采用MPC的系統(tǒng)在初始時(shí)刻的實(shí)際軌跡與參考軌跡之間的誤差較大，并且抗干擾能力較差，響應(yīng)速度明顯落后于LF-AMPC和AMPC。采用AMPC的系統(tǒng)響應(yīng)速度較采用MPC的系統(tǒng)有較大提升，并且抗干擾能力明顯增強(qiáng)，實(shí)際軌跡與參考軌跡間的誤差大大減少。而采用LF-AMPC的系統(tǒng)的響應(yīng)速度較AMPC進(jìn)一步加快，并且實(shí)際軌跡與參考軌跡之間幾乎沒(méi)有誤差，抗干擾能力大大增強(qiáng)。

圖2(a)是復(fù)雜干擾下的AUV三維折線(xiàn)軌跡跟蹤仿真的起始時(shí)刻細(xì)節(jié)圖。從圖中可看出，在該階段，AUV的實(shí)際位置與預(yù)測(cè)軌跡的初始位置有較大差異，采用LF-AMPC控制器的AUV能夠迅速響應(yīng)，在X軸上運(yùn)動(dòng)2 m時(shí)已跟蹤上參考軌跡，并且實(shí)際軌跡與參考軌跡的誤差幾乎為0 m。由此可以看出，采用LF-AMPC控制器的AUV在響應(yīng)速度與跟蹤精度上的優(yōu)越性。圖2(b)為復(fù)雜干擾下的AUV三維折線(xiàn)軌跡跟蹤仿真左轉(zhuǎn)彎90°時(shí)的細(xì)節(jié)圖。從圖中可看出，采用LF-AMPC的AUV轉(zhuǎn)彎時(shí)與參考軌跡間的誤差約為0.186 m，并且轉(zhuǎn)彎后實(shí)際軌跡與參考軌跡間的誤差最小，約為0.153 m。由此可以看出，在轉(zhuǎn)彎前后參數(shù)變化較大的情況下AUV能夠保持良好的跟蹤效果，系統(tǒng)魯棒性有了明顯提升。圖2(c) 為復(fù)雜干擾下的AUV三維折線(xiàn)軌跡跟蹤仿真直行時(shí)的細(xì)節(jié)圖。從圖中可看出，在受到干擾時(shí)實(shí)際軌跡與參考軌跡的平均誤差最小，約為0.138 m，抗干擾能力有了較大增強(qiáng)。圖2(d) 為復(fù)雜干擾下的AUV三維折線(xiàn)軌跡跟蹤仿真右轉(zhuǎn)彎90°時(shí)的細(xì)節(jié)圖。從圖中可以看出，采用LF-AMPC的AUV實(shí)際軌跡較為平緩，與參考軌跡間的平均誤差最小，約為0.144 m，并且能夠快速完成轉(zhuǎn)彎動(dòng)作，跟蹤效果最好，響應(yīng)速度與控制精度得到明顯提高。從以上分析可以看出，采用LF-AMPC的AUV跟蹤效果明顯優(yōu)于采用AMPC和MPC的AUV系統(tǒng)，其魯棒性、抗干擾能力、響應(yīng)速度均得到了明顯提升。

3.2 控制器計(jì)算時(shí)間對(duì)比

在使用Matlab/Simulink聯(lián)合仿真時(shí)，本文采用“set_param(‘mymodel’,‘Profile’,‘on’)”代碼來(lái)獲得仿真程序執(zhí)行時(shí)間的報(bào)告,具體數(shù)據(jù)如表1所示。

表1 運(yùn)行時(shí)間對(duì)比

從表1中可看出，在針對(duì)圖1設(shè)定仿真時(shí)間為100 s的情況下,采用MPC控制器的系統(tǒng)實(shí)際運(yùn)行時(shí)間為51 s,而采用AMPC控制器的系統(tǒng)運(yùn)行時(shí)間為63 s，較傳統(tǒng)MPC控制器增加了約23.5%。這是因?yàn)椴捎肕PC控制器的系統(tǒng)結(jié)構(gòu)相對(duì)簡(jiǎn)單，采用AMPC控制器的系統(tǒng)加入了自適應(yīng)算法增加了系統(tǒng)的復(fù)雜性，雖然改善了控制效果，但在一定程度上犧牲了實(shí)時(shí)性，所以采用AMPC控制器的系統(tǒng)實(shí)際運(yùn)行時(shí)間相對(duì)較長(zhǎng)。采用LF-AMPC控制器的系統(tǒng)實(shí)際運(yùn)行時(shí)間為27 s。運(yùn)行時(shí)間相比于采用傳統(tǒng)MPC控制器和AMPC控制器的系統(tǒng)分別減少47.06%和57.14%。采用LF-AMPC控制器的系統(tǒng)雖然也存在自適應(yīng)算法，但引入的拉蓋爾函數(shù)使得系統(tǒng)求解速度變快，所以L(fǎng)F-AMPC控制器能夠同時(shí)改善控制效果并加快計(jì)算速度。

4 結(jié)論

針對(duì)復(fù)雜水文條件下無(wú)人自治水下機(jī)器人(AUV)軌跡跟蹤時(shí)面對(duì)復(fù)雜擾動(dòng)而偏離預(yù)定軌跡的問(wèn)題，以拉蓋爾函數(shù)和最小二乘法為參考設(shè)計(jì)了一種基于拉蓋爾函數(shù)的AUV自適應(yīng)預(yù)測(cè)軌跡跟蹤控制方法。仿真結(jié)果對(duì)比表明，利用最小二乘法可有效減小系統(tǒng)誤差，增強(qiáng)AUV控制系統(tǒng)的抗干擾性和魯棒性，并且超調(diào)量小，控制得到改善。通過(guò)拉蓋爾函數(shù)對(duì)傳統(tǒng)預(yù)測(cè)控制進(jìn)行參數(shù)重構(gòu)能夠有效地減小控制器的計(jì)算負(fù)擔(dān)，提高控制器的計(jì)算速度，增強(qiáng)動(dòng)態(tài)性。此外，本文只研究了基于運(yùn)動(dòng)學(xué)方程的控制手段，后續(xù)工作將深入到動(dòng)力學(xué)方程，使研究更加完善。