王紅都,高 楓,黎 明,付東飛

(中國海洋大學(xué) 工程學(xué)院,山東 青島,266100)

0 引言

為了精確執(zhí)行復(fù)雜水下作業(yè)工作,通常會(huì)為自主水下航行器(autonomous undersea vehicle,AUV)[1-3]搭載水下機(jī)械臂,構(gòu)成水下機(jī)器人機(jī)械臂系統(tǒng)(undersea vehicle manipulator system,UVMS)[4-6]。但水下環(huán)境的不確定性和UVMS 的強(qiáng)非線性和強(qiáng)耦合性,使得UVMS 完成跟蹤預(yù)期軌跡變得具有挑戰(zhàn)性,甚至不能保證系統(tǒng)穩(wěn)定性。此外,UVMS 還需要滿足扭矩、安全和機(jī)械等約束條件[7],否則會(huì)對(duì)UVMS 造成一定程度的損壞甚至故障。因此,設(shè)計(jì)具有良好抗干擾性能和滿足約束條件的控制器對(duì)UVMS 非常重要。

UVMS 的建模與控制方面迄今已取得了很大的研究進(jìn)展。Fossen[8]采用牛頓-歐拉公式建立了UVMS 運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)模型。Tarn 等[9]提出了一種新的基于拉格朗日方程和牛頓-歐拉公式的模型。UVMS 的抗干擾控制方面也取得了一定的研究進(jìn)展。Lin 等[10]針對(duì)UVMS 提出了一種基于多觀測器的滑模預(yù)設(shè)性能控制方法,利用擴(kuò)張狀態(tài)觀測器和滑模干擾觀測器來估計(jì)并提前進(jìn)行補(bǔ)償,最后采用滑?？刂破鬟M(jìn)行控制,達(dá)到了良好的軌跡跟蹤效果。Antonelli 等[11]用觀測器來估計(jì)跟蹤控制律中的系統(tǒng)速度,使UVMS 有效地跟蹤期望軌跡并在跟蹤性能與計(jì)算負(fù)荷中進(jìn)行了權(quán)衡簡化。為了進(jìn)一步解決UVMS 中的末端執(zhí)行器跟蹤問題,Antonelli 等[12]還提出了一種新型的自適應(yīng)控制律,將整體控制問題分解,制定模塊化控制方案,減少了計(jì)算負(fù)擔(dān)。Wang 等[13]采用離散時(shí)間延遲估計(jì)技術(shù),提出了一種新的非線性魯棒控制方案,對(duì)未知?jiǎng)恿W(xué)進(jìn)行了估計(jì)補(bǔ)償,并獲得閉環(huán)控制系統(tǒng)的期望動(dòng)態(tài)特性,在任務(wù)空間中獲得了0.064 m 的末端精度。然而,上述算法雖然能實(shí)現(xiàn)UVMS 良好跟蹤性能,但無法保證約束條件的成立。

模型預(yù)測控制(model predictive control,MPC)作為一種最優(yōu)控制策略,可以有效處理復(fù)雜系統(tǒng)的約束問題[14-25]。MPC 在每一采樣時(shí)刻,根據(jù)當(dāng)前獲得的測量信息,在線求解一個(gè)有限時(shí)域開環(huán)優(yōu)化問題,并將得到的控制序列的第1 個(gè)元素作用于被控對(duì)象。Shen 等[18]針對(duì)水下機(jī)器人系統(tǒng),將路徑規(guī)劃與跟蹤控制集成在一起,采用后退水平優(yōu)化方案,利用b 樣條進(jìn)行路徑規(guī)劃并使用魯棒模型預(yù)測控制器,通過仿真實(shí)驗(yàn)證明了其有效性,并分析了閉環(huán)系統(tǒng)的穩(wěn)定性。Dai 等[19]提出基于管的魯棒模型預(yù)測控制(robust MPC,RMPC),用于機(jī)器人機(jī)械臂系統(tǒng)的時(shí)變軌跡跟蹤控制,對(duì)名義系統(tǒng)的約束條件進(jìn)行收緊,MPC 在以名義系統(tǒng)軌跡為中心的管內(nèi)驅(qū)動(dòng)實(shí)際系統(tǒng)的軌跡。進(jìn)一步提出了一種帶有卡爾曼濾波目標(biāo)觀測器的魯棒快速管模型預(yù)測控制,用于解決運(yùn)動(dòng)約束冗余UVMS 在存在建模不確定性、噪聲和外部干擾情況下的移動(dòng)目標(biāo)抓取問題,并通過仿真實(shí)驗(yàn)驗(yàn)證了其有效性[20]。需要指出的是,現(xiàn)有的UVMS 的MPC 方法能夠解決條件約束問題,但不能很好解決干擾抑制與補(bǔ)償問題。

因此,文中旨在提出一種基于擴(kuò)張狀態(tài)觀測器(extended state observer,ESO)的RMPC 方法,以同時(shí)解決條件約束與抗干擾問題。首先,定義忽略不確定因素和干擾的名義系統(tǒng),在名義系統(tǒng)的基礎(chǔ)上設(shè)計(jì)RMPC 算法。然后,使用ESO 來估計(jì)總干擾并提前進(jìn)行補(bǔ)償,以提高UVMS的系統(tǒng)魯棒性和抗干擾能力。接著利用反饋線性化方法求出作用于真實(shí)系統(tǒng)的控制器,并進(jìn)行穩(wěn)定性分析。最后,通過仿真實(shí)驗(yàn)驗(yàn)證了其有效性。

1 UVMS 建模

1.1 動(dòng)力學(xué)建模

文中采用的UVMS 結(jié)構(gòu)如圖1 所示,包括6 自由度的水下機(jī)器人和2 關(guān)節(jié)的機(jī)械臂,其動(dòng)力學(xué)模型描述如下:

圖1 水下機(jī)器人機(jī)械臂系統(tǒng)結(jié)構(gòu)圖Fig.1 Structure of UVMS

式中:q=[qv,qm]T包含水下機(jī)器人本體的位姿向量qv=[x,y,z,ψ]T和2個(gè)機(jī)械臂關(guān)節(jié)的角度qm=[θ1,θ2]T;ψ為機(jī)器人的偏航角;M(q)為系統(tǒng)的慣性力和力矩;C(q,)為 系統(tǒng)的科氏力矩陣;D(q,)為水動(dòng)力阻尼矩陣;G(q)為 恢復(fù)力矩陣;F(q,)表示耦合作用;τc為輸入力矩;τd1為未知干擾。

令γ=[u,v,w,r,p1,p2]T表示UVMS 在機(jī)體坐標(biāo)系下的速度向量。由式(1)可得在機(jī)體坐標(biāo)系下的系統(tǒng)動(dòng)力學(xué)模型為

式 中:Mγ=JTMJ;Cγ=JTCJ+MγJ-1;Dγ=JTDJ;Gγ=JTG;Fγ=JTF;τγ=JTτc;J為機(jī)體坐標(biāo)系與大地坐標(biāo)系下的速度轉(zhuǎn)換關(guān)系,有

考慮建模誤差引起的不確定性因素,將參數(shù)矩陣分成已知與未知部分,即

由式(3)與式(5)可以得到模型

將式(6)改寫為每個(gè)自由度的表達(dá)式為

令x1=q,x2=γ,x=[x1,x2]T,將系統(tǒng)模型以如下增廣的狀態(tài)空間形式描述:

1.2 誤差動(dòng)力學(xué)模型

令qR=[xR,yR,zR,ψR(shí),θR1,θR2]T表示UVMS 的期望軌跡,其速度表達(dá)式為

進(jìn)一步可得跟蹤誤差關(guān)系為

可分別表示為前饋控制與反饋控制,則知

詳細(xì)推導(dǎo)見附錄A。

在機(jī)體坐標(biāo)系下,期望軌跡的動(dòng)態(tài)方程為

對(duì)le=γ-vf進(jìn)行微分,并與式(15)相結(jié)合得

其中

式中,Fu,Fv,Fw,Fr,Fp1,Fp2見附錄B 式(63)～(68)。

結(jié)合式(15)與(17)得到增廣的誤差系統(tǒng)

在式(19)的零平衡點(diǎn)處,即當(dāng)誤差動(dòng)力學(xué)狀態(tài)為零,結(jié)合式(16),顯然Fu=FuR,Fv=FvR,Fw=FwR,Fr=FrR,F1=F1R,F2=F2R,實(shí)際的控制輸 入量與期望的輸入量一致。如此可見,可以將期望軌跡的跟蹤問題轉(zhuǎn)化為誤差系統(tǒng)(19)在平衡點(diǎn)的鎮(zhèn)定問題,并可用MPC 實(shí)現(xiàn)目標(biāo)鎮(zhèn)定。

2 UVMS RMPC 設(shè)計(jì)

2.1 RMPC 算法

針對(duì)系統(tǒng)(10),忽略系統(tǒng)的不確定性與干擾,可以得到如下名義模型

則其誤差動(dòng)態(tài)模型為

設(shè)計(jì)代價(jià)函數(shù)

式中:T為預(yù)測時(shí)域;為階段代價(jià),其中Q為半正定權(quán)衡矩陣,R為正定權(quán)衡矩陣;是終端代價(jià),其中P為正定終端權(quán)衡矩陣。

有限時(shí)域最優(yōu)控制問題可以表示為最小代價(jià)函數(shù)問題

選取 δ ∈[t t+T],則

式中: ?為終端區(qū)域;Ue為包含內(nèi)部原點(diǎn)的區(qū)域,對(duì)系統(tǒng)的輸入限制形式如下

誤差控制與實(shí)際控制輸入之間的關(guān)系為

其中

進(jìn)一步選取終端區(qū)域 ?=?1∩?2,其中?1:=,?2由式(32)定義。

整體系統(tǒng)滿足以下約束關(guān)系:

2.2 反饋線性化控制設(shè)計(jì)

在MPC 滾動(dòng)優(yōu)化過程中,預(yù)測狀態(tài)與實(shí)際系統(tǒng)狀態(tài)會(huì)出現(xiàn)偏差,為了使實(shí)際系統(tǒng)狀態(tài)與名義系統(tǒng)狀態(tài)的誤差狀態(tài)量保持穩(wěn)定,利用反饋線性化控制設(shè)計(jì)如下控制器:

式中: ?1∈[t,t+δ];(δ|t)為最優(yōu)名義輸入;(δ|t)為最優(yōu)名義狀態(tài);Ke=[K1K2],其中

選擇ki1<0,ki2<0,且+4ki1>0,i=1,···,6。表示實(shí)際狀態(tài)與名義狀態(tài)的誤差狀態(tài),則可知

將控制器(34)代入(36)中,再結(jié)合式(10)與(20),得到

2.3 ESO 設(shè)計(jì)

ESO可以根據(jù)系統(tǒng)的輸入輸出估計(jì)出控制器的反饋,同時(shí)可以將UVMS 內(nèi)部模型的不確定項(xiàng)、耦合項(xiàng)、模型內(nèi)部干擾及水下外部干擾等視作一個(gè)總的干擾,對(duì)這個(gè)總擾動(dòng)進(jìn)行估計(jì)并提前在控制部分進(jìn)行補(bǔ)償。因此,對(duì)于系統(tǒng)內(nèi)部與外部的參數(shù)攝動(dòng)與時(shí)變干擾都可以進(jìn)行有效補(bǔ)償,提高了系統(tǒng)的魯棒性。

定義f1=γ,f2=τd,將系統(tǒng)模型(3)表示為

針對(duì)式(38),構(gòu)造如下ESO

ESO 的補(bǔ)償控制輸入項(xiàng)

式中,eob=f1-z1。z1,z2為f1,f2的估計(jì)值。進(jìn)一步可得

進(jìn)一步可知

式中,λmax表示的最大特征值?？芍?當(dāng)t→∞時(shí),xd(t)→0。

2.4 整體控制器設(shè)計(jì)

整體控制器包括2 個(gè)部分,一個(gè)是對(duì)通過優(yōu)化問題(24)求優(yōu)后的最優(yōu)名義控制輸入進(jìn)行反饋線性化的控制輸入部分(34);另一個(gè)是ESO 對(duì)系統(tǒng)干擾的估計(jì)后,為進(jìn)一步抵消干擾,對(duì)控制輸入的補(bǔ)償部分(40)。作用于實(shí)際系統(tǒng)的整體RMPC 控制律為

至此,RMPC 整體的算法流程圖如圖2 所示。

圖2 魯棒模型預(yù)測控制算法流程Fig.2 Robust model predictive control algorithm flow

2.5 可行性與穩(wěn)定性分析

首先對(duì)優(yōu)化問題的可行性進(jìn)行簡要分析,給出如下引理。

引理: 若開環(huán)優(yōu)化問題(24)在t=0 時(shí)刻可行,意味著在t≥0時(shí)刻都是可行的,且系統(tǒng)狀態(tài)滿足終端約束。

證明: 假設(shè)在某一時(shí)刻t優(yōu)化問題存在1 個(gè)可行解,則該名義最優(yōu)控制輸入滿足輸入約束條件,且將控制輸入作用于系統(tǒng)生成的最優(yōu)名義狀態(tài)也滿足狀態(tài)約束,系統(tǒng)狀態(tài)軌跡末端進(jìn)入終端域。在t+δ時(shí)刻優(yōu)化問題的初始狀態(tài)也是t時(shí)刻的最優(yōu)名義輸入,即x(t+δ)=(t)。

在求解下一時(shí)刻的優(yōu)化問題時(shí),構(gòu)造如下候選控制輸入

該函數(shù)包括最優(yōu)名義控制輸入項(xiàng)及終端局部反饋控制項(xiàng)。對(duì)于第1 部分的控制輸入而言,由于t時(shí)刻最優(yōu)名義控制輸入滿足控制約束且系統(tǒng)狀態(tài)也滿足狀態(tài)約束,顯然t+δ時(shí)刻也是滿足的。第2 部分的局部反饋控制器控制的終端區(qū)域是1 個(gè)不變集,也是滿足約束條件的。綜上,可行性的分析是具有遞推性的。

然后對(duì)系統(tǒng)穩(wěn)定性進(jìn)行分析,選取最優(yōu)值函數(shù)作為李雅普諾夫函數(shù),考慮2 個(gè)時(shí)刻間的差值:

所以?J≤0,由此得出系統(tǒng)是漸進(jìn)穩(wěn)定的。

3 仿真實(shí)驗(yàn)

3.1 參數(shù)選取

選取采樣間隔為0.01,Mx=147,My=147,Mz=147,Mψ=8,Im1=10,Im2=10,Xu=0,Yv=0,Zw=0,Nr=0,Du=-148,Dv=-148,Dw=-148,Dr=-180,Dm1=Dm2=5。機(jī)械臂長度為 0.15 m,預(yù)測時(shí)域?yàn)? ?,Q=0.4I12,R=0.01I4。將比例-積分-微分(proportion-integral-differential,PID)控制器的比例、積分和微分參數(shù)分別設(shè)定為

選擇2 種工作場景:

且2 種場景下的外部干擾分別給定:

3.2 結(jié)果分析

在仿真階段,將PID 和非奇異終端滑模控制器(non-singular terminal sliding mode controller,NTSMC)進(jìn)行對(duì)比,仿真結(jié)果如圖3～8 所示。在工況1 下,航行器按照預(yù)定的軌跡進(jìn)行運(yùn)動(dòng),整個(gè)過程中機(jī)械臂關(guān)節(jié)角度保持不變。為了與文中所提出的RMPC 進(jìn)行對(duì)比,采用了傳統(tǒng)的PID 控制器與SMC 分別對(duì)UVMS 進(jìn)行控制。圖3 給出了在不同控制器作用下,UVMS 本體在工況1 下的三維跟蹤情況。從圖3 中可以看出,基于ESO 的RMPC 有很好的抗干擾效果與跟蹤效果。圖4 左邊一列給出了各個(gè)自由度上軌跡跟蹤情況,右邊給出了相應(yīng)的跟蹤誤差。從圖4 中可知,在文中方法作用下,航行器每個(gè)自由度都具有較好的跟蹤效果,且機(jī)械臂可以保持在特定的角度,航行器與機(jī)械臂的跟蹤誤差都趨于穩(wěn)定。圖5 展示了ESO 對(duì)每個(gè)自由度上總干擾的估計(jì)情況,從中可以看出,ESO 可以較為準(zhǔn)確地估計(jì)干擾,其中黑色虛線表示i(i=1,···,6)個(gè)自由度上的總干擾。

圖3 工況1 下UVMS 本體三維跟蹤情況Fig.3 The 3D tracking situation of UVMS in case 1

圖4 工況1 下軌跡跟蹤情況Fig.4 UVMS tracking situation for case 1

圖5 工況1 下ESO 干擾估計(jì)情況Fig.5 The situation of disturbance by ESO in case 1

在工況2 下,機(jī)械臂不再保持特定的關(guān)節(jié)角度,在航行器小范圍的運(yùn)動(dòng)下,機(jī)械臂按照期望軌跡進(jìn)行運(yùn)動(dòng)。圖6 表示UVMS 本體在工況1 下的三維跟蹤情況,從中可以看出,在航行器按照期望軌跡進(jìn)行小范圍移動(dòng)時(shí),RMPC 方法相比于其他2 種控制器仍具有更好的跟蹤與抗干擾效果。圖7 的左右2 列分別為各個(gè)自由度上軌跡跟蹤情況與跟蹤誤差,從中可以看出,在RMPC 作用下,每個(gè)自由度都擁有良好跟蹤性能,且機(jī)械臂進(jìn)行工作時(shí)可以很好地跟蹤期望軌跡,且跟蹤誤差是收斂的。圖8 給出了工況2 下ESO 對(duì)每個(gè)自由度上總干擾的估計(jì)情況。

圖6 工況2 下的UVMS 本體三維跟蹤情況Fig.6 The 3D tracking situation of UVMS in case 2

圖7 工況2 下UVMS 跟蹤情況Fig.7 UVMS tracking situation for case 2

圖8 工況2 下ESO 干擾估計(jì)情況Fig.8 The situation of disturbance by ESO in case 2

為了進(jìn)一步對(duì)比各個(gè)控制器的控制效果,利用每個(gè)自由度跟蹤誤差的均方根(root mean square,RMS)指標(biāo)進(jìn)行數(shù)值比較,如表1 與表2 所示。表1顯示的是工況1 下各控制器對(duì)期望軌跡的跟蹤誤差情況,表2 顯示的是工況2 下每個(gè)控制器對(duì)期望軌跡的跟蹤誤差情況。可以明顯地看到文中所提算法作用于每個(gè)自由度的誤差RMS 更小,跟蹤效果更好。

表1 工況1 下各控制器的跟蹤誤差RMS 指標(biāo)Table 1 The tracking error of each controller in case 1

表2 工況2 下各控制器跟蹤誤差RMS 指標(biāo)Table 2 The tracking error of each controller in case 2

4 結(jié)束語

文中針對(duì)UVMS 提出了一種基于ESO 的RMPC 方法。首先對(duì)名義模型進(jìn)行RMPC 算法設(shè)計(jì),結(jié)合ESO 對(duì)干擾提前進(jìn)行估計(jì)并進(jìn)行補(bǔ)償,在此基礎(chǔ)上利用反饋線性化方法求出作用于真實(shí)系統(tǒng)的RMPC 控制器,最后通過仿真實(shí)驗(yàn)驗(yàn)證了算法的有效性。未來將進(jìn)一步研究多關(guān)節(jié)UVMS的RMPC 跟蹤方法并應(yīng)用于實(shí)際UVMS 系統(tǒng),并開展相應(yīng)的水池試驗(yàn)或湖試。

附錄A

附錄B