基于ESO 的水下機器人機械臂系統魯棒模型預測控制

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

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

0 引言

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

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

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

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

1 UVMS 建模

1.1 動力學建模

文中采用的UVMS 結構如圖1 所示,包括6 自由度的水下機器人和2 關節的機械臂,其動力學模型描述如下:

圖1 水下機器人機械臂系統結構圖Fig.1 Structure of UVMS

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

令γ=[u,v,w,r,p1,p2]T表示UVMS 在機體坐標系下的速度向量。由式(1)可得在機體坐標系下的系統動力學模型為

式 中:Mγ=JTMJ;Cγ=JTCJ+MγJ-1;Dγ=JTDJ;Gγ=JTG;Fγ=JTF;τγ=JTτc;J為機體坐標系與大地坐標系下的速度轉換關系,有

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

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

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

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

1.2 誤差動力學模型

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

進一步可得跟蹤誤差關系為

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

詳細推導見附錄A。

在機體坐標系下,期望軌跡的動態方程為

對le=γ-vf進行微分,并與式(15)相結合得

其中

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

結合式(15)與(17)得到增廣的誤差系統

在式(19)的零平衡點處,即當誤差動力學狀態為零,結合式(16),顯然Fu=FuR,Fv=FvR,Fw=FwR,Fr=FrR,F1=F1R,F2=F2R,實際的控制輸 入量與期望的輸入量一致。如此可見,可以將期望軌跡的跟蹤問題轉化為誤差系統(19)在平衡點的鎮定問題,并可用MPC 實現目標鎮定。

2 UVMS RMPC 設計

2.1 RMPC 算法

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

則其誤差動態模型為

設計代價函數

式中:T為預測時域;為階段代價,其中Q為半正定權衡矩陣,R為正定權衡矩陣;是終端代價,其中P為正定終端權衡矩陣。

有限時域最優控制問題可以表示為最小代價函數問題

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

式中: ?為終端區域;Ue為包含內部原點的區域,對系統的輸入限制形式如下

誤差控制與實際控制輸入之間的關系為

其中

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

整體系統滿足以下約束關系:

2.2 反饋線性化控制設計

在MPC 滾動優化過程中,預測狀態與實際系統狀態會出現偏差,為了使實際系統狀態與名義系統狀態的誤差狀態量保持穩定,利用反饋線性化控制設計如下控制器:

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

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

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

2.3 ESO 設計

ESO可以根據系統的輸入輸出估計出控制器的反饋,同時可以將UVMS 內部模型的不確定項、耦合項、模型內部干擾及水下外部干擾等視作一個總的干擾,對這個總擾動進行估計并提前在控制部分進行補償。因此,對于系統內部與外部的參數攝動與時變干擾都可以進行有效補償,提高了系統的魯棒性。

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

針對式(38),構造如下ESO

ESO 的補償控制輸入項

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

進一步可知

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

2.4 整體控制器設計

整體控制器包括2 個部分,一個是對通過優化問題(24)求優后的最優名義控制輸入進行反饋線性化的控制輸入部分(34);另一個是ESO 對系統干擾的估計后,為進一步抵消干擾,對控制輸入的補償部分(40)。作用于實際系統的整體RMPC 控制律為

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

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

2.5 可行性與穩定性分析

首先對優化問題的可行性進行簡要分析,給出如下引理。

引理: 若開環優化問題(24)在t=0 時刻可行,意味著在t≥0時刻都是可行的,且系統狀態滿足終端約束。

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

在求解下一時刻的優化問題時,構造如下候選控制輸入

該函數包括最優名義控制輸入項及終端局部反饋控制項。對于第1 部分的控制輸入而言,由于t時刻最優名義控制輸入滿足控制約束且系統狀態也滿足狀態約束,顯然t+δ時刻也是滿足的。第2 部分的局部反饋控制器控制的終端區域是1 個不變集,也是滿足約束條件的。綜上,可行性的分析是具有遞推性的。

然后對系統穩定性進行分析,選取最優值函數作為李雅普諾夫函數,考慮2 個時刻間的差值:

所以?J≤0,由此得出系統是漸進穩定的。

3 仿真實驗

3.1 參數選取

選取采樣間隔為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。機械臂長度為 0.15 m,預測時域為8 ?,Q=0.4I12,R=0.01I4。將比例-積分-微分(proportion-integral-differential,PID)控制器的比例、積分和微分參數分別設定為

選擇2 種工作場景:

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

3.2 結果分析

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

圖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 干擾估計情況Fig.5 The situation of disturbance by ESO in case 1

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

圖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 干擾估計情況Fig.8 The situation of disturbance by ESO in case 2

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

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

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

4 結束語

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

附錄A

附錄B