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

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

0 引言

無人自治水下機(jī)器人(AUV)作為人類探索和開發(fā)海洋的利器，已經(jīng)引起世界各國研究者的廣泛關(guān)注[1]。近年來，AUV已被廣泛應(yīng)用于科學(xué)、工業(yè)、商業(yè)和軍事等領(lǐng)域。由于工作環(huán)境中存在復(fù)雜的外部干擾，所以在這種情況下運(yùn)行時，AUV要求運(yùn)動控制器具有較強(qiáng)的抗干擾能力和快速的反應(yīng)能力。但是，運(yùn)動控制過程中如數(shù)據(jù)計(jì)算量過于龐大，會導(dǎo)致AUV響應(yīng)速度過慢，嚴(yán)重時會偏離預(yù)設(shè)軌跡甚至于失聯(lián)。如何減小數(shù)據(jù)計(jì)算量，提高AUV的控制精度、動態(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ī)器人軌跡跟蹤時，物理模型的限制會對控制器性能造成一定影響，而上述幾種控制方法均沒有涉及此問題。模型預(yù)測控制(MPC) 可以將控制約束和狀態(tài)約束納入優(yōu)化計(jì)算。此外，其可以運(yùn)用于線性系統(tǒng)和非線性系統(tǒng)對象的控制器設(shè)計(jì)，在水下機(jī)器人軌跡跟蹤中具有廣泛的應(yīng)用[14-15]。ZHANG等[16]設(shè)計(jì)了一種基于MPC的三維軌跡跟蹤方法，針對AUV的軌跡跟蹤控制有較好的效果，對MPC在AUV中的軌跡跟蹤有一定的參考價值；YAN等[17]基于MPC設(shè)計(jì)了雙閉環(huán)三維軌跡跟蹤算法，外環(huán)運(yùn)動學(xué)控制器生成速度指令后傳遞給內(nèi)環(huán)動力學(xué)控制器進(jìn)一步生成控制力矩，并考慮系統(tǒng)的輸入和狀態(tài)約束，但是未考慮外部水流等復(fù)雜干擾對模型參數(shù)的影響;PENG等[18]設(shè)計(jì)了擴(kuò)展?fàn)顟B(tài)觀測器來估計(jì)位置的內(nèi)部動態(tài)特性和外部干擾，但是只在二維垂直面進(jìn)行了驗(yàn)證，并未考慮三維空間內(nèi)存在復(fù)雜干擾時的運(yùn)動情況；文獻(xiàn)[19-20]針對具有參數(shù)不確定性和擾動的離散線性系統(tǒng)設(shè)計(jì)了自適應(yīng)預(yù)測控制算法，對帶有參數(shù)不確定性的AUV系統(tǒng)具有一定的參考價值；文獻(xiàn)[21-22]針對預(yù)測控制器的動態(tài)響應(yīng)能力設(shè)計(jì)了基于拉蓋爾函數(shù)的MPC，極大地提高了計(jì)算速度，但并未考慮系統(tǒng)參數(shù)不確定性對控制效果的影響。從以上分析中可以看出，參數(shù)不確定性和龐大的計(jì)算量在一定程度上限制了MPC控制器的性能。因此，如何補(bǔ)償參數(shù)不確定性和減小計(jì)算量提高響應(yīng)速度對AUV的軌跡跟蹤具有重要意義。

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

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

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

(1)

(2)

(3)

2 AUV軌跡跟蹤控制策略

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

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

(4)

(5)

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

(6)

在實(shí)際運(yùn)動過程中，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ù)測控制中帶有約束的優(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ù)測控制器

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

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

(9)

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

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

(10)

(11)

(12)

(13)

(14)

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

(15)

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

(16)

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

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

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

(17)

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

L(k+1)=AlL(k)

(19)

式中:

(20)

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

(21)

即

(22)

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

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

(23)

根據(jù)式 (22),(23)對傳統(tǒng)預(yù)測控制的優(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ù)測時域Np足夠大時,有

(26)

(27)

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

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

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

3.1 三維曲線軌跡跟蹤仿真

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

(28)

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

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

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

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

從圖1中可看出，在存在復(fù)雜干擾的情況下進(jìn)行三維折線軌跡跟蹤任務(wù)時，采用MPC的系統(tǒng)在初始時刻的實(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í)際軌跡與參考軌跡之間幾乎沒有誤差，抗干擾能力大大增強(qiáng)。

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

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

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

表1 運(yùn)行時間對比

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

4 結(jié)論

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