基于終端約束MPC的輪式移動(dòng)機(jī)器人軌跡跟蹤控制*

徐佳寶,張國(guó)良,羅國(guó)攀

(四川輕化工大學(xué)a.自動(dòng)化與信息工程學(xué)院;b.人工智能四川省重點(diǎn)實(shí)驗(yàn)室,宜賓 644000)

0 引言

近年來(lái),輪式移動(dòng)機(jī)器人(wheeled mobile robot,WMR)在各個(gè)領(lǐng)域的廣泛應(yīng)用引起了學(xué)者極大關(guān)注[1-2]。而WMR控制任務(wù)主要分為路徑規(guī)劃[3]與軌跡跟蹤,為了實(shí)現(xiàn)WMR軌跡跟蹤,許多學(xué)者提出了不同的控制方法。CHWA[4]基于WMR運(yùn)動(dòng)學(xué)模型,設(shè)計(jì)了滑膜控制器,即使WMR在控制中有較大的初始誤差,也能得到較好的控制。TINH等[5]考慮WMR參數(shù)和非參數(shù)兩種不確定情況,結(jié)合自適應(yīng)控制與神經(jīng)網(wǎng)絡(luò)方法,解決了移動(dòng)機(jī)器人重心偏移、模型不確定等因素的影響。YANG等[6]研究了WMR在不平坦地面,設(shè)計(jì)雙閉環(huán)策略的軌跡跟蹤控制;通過(guò)內(nèi)環(huán)估計(jì)外部擾動(dòng),外環(huán)輸出期望速度,實(shí)現(xiàn)了WMR平穩(wěn)轉(zhuǎn)彎。HUA等[7]采用基于滑膜的增強(qiáng)變結(jié)構(gòu)對(duì)WMR進(jìn)行軌跡跟蹤,提高了機(jī)器人控制能力,降低了姿態(tài)誤差,且線速度和角速度可控。

由于WMR是一個(gè)非完整約束系統(tǒng),而模型預(yù)測(cè)控制(model predictive control,MPC)具有前饋-反饋的結(jié)構(gòu),可以處理帶約束的非線性輸入輸出系統(tǒng)[8-9]。劉卉等[10]提出一種非完整約束輪式移動(dòng)機(jī)器人的MPC控制方案,通過(guò)將二次規(guī)劃用于WMR誤差模型連續(xù)線性化求解線性MPC,提升系統(tǒng)穩(wěn)定性。XIAO等[11]開發(fā)了一種基于神經(jīng)網(wǎng)絡(luò)優(yōu)化的魯棒預(yù)測(cè)控制來(lái)穩(wěn)定WMR,通過(guò)各種動(dòng)態(tài)條件下實(shí)驗(yàn)證明了所提出方案能提高M(jìn)PC計(jì)算效率。PACHECO等[12]基于PID控制和MPC相結(jié)合的控制方案實(shí)現(xiàn)了機(jī)器人的局部路徑跟蹤。

基于上述分析,一方面,這些控制方案未充分考慮輪式機(jī)器人預(yù)測(cè)未來(lái)動(dòng)態(tài)模型的非線性特性;另一方面,由于時(shí)域約束的存在,系統(tǒng)性能會(huì)受到限制。在這種情況下,會(huì)導(dǎo)致WMR較低的跟蹤精度。所以研究一種更適合處理非線性和時(shí)域約束問(wèn)題的控制方案,以改善輪式機(jī)器人的性能是有必要的。

針對(duì)非線性和時(shí)域約束問(wèn)題,首先,利用狀態(tài)誤差方程作為預(yù)測(cè)未來(lái)動(dòng)態(tài)的模型,并設(shè)計(jì)帶終端約束的成本函數(shù),同時(shí)引入時(shí)域約束;其次,設(shè)計(jì)合適的終端域和終端懲罰矩陣,確保系統(tǒng)在有限預(yù)測(cè)時(shí)域內(nèi)收斂至期望軌跡;然后,在Lyapunov穩(wěn)定性框架內(nèi),分析優(yōu)化問(wèn)題的可行性和系統(tǒng)穩(wěn)定性;最后,通過(guò)與TMPC方案在兩種工況下進(jìn)行仿真實(shí)驗(yàn)對(duì)比,驗(yàn)證了所設(shè)計(jì)的終端約束MPC控制方案的有效性。

1 系統(tǒng)模型

當(dāng)輪式移動(dòng)機(jī)器人的運(yùn)動(dòng)速度相對(duì)較低,運(yùn)動(dòng)學(xué)模型可以提精度來(lái)進(jìn)行軌跡跟蹤。選擇WMR的運(yùn)動(dòng)學(xué)模型如圖1所示。

圖1 WMR運(yùn)動(dòng)學(xué)模型

在圖1中,存在兩個(gè)笛卡爾坐標(biāo)系,{X,O,Y}是固定在地面上的全局坐標(biāo)系,{x,o,y}是輪式機(jī)器人上的局部坐標(biāo)系,θ為其前進(jìn)方向與X夾角。

(1)

式中:χ(t)=[x(t)y(t)θ(t)]T表示輪式機(jī)器人在全局坐標(biāo)系中的位置和方向,u(t)=[v(t)ω(t)]T表示控制輸入向量,其中,v(t)為線速度,ω(t)為角速度。

定義輪式機(jī)器人系統(tǒng)式(1)的跟蹤誤差向量為χe(t)=[xe(t)ye(t)θe(t)]T,參考輪式機(jī)器人位置與方向表示為χr(t)=[xr(t)yr(t)θr(t)]T。χe(t)的具體表達(dá)式為:

(2)

對(duì)式(2)進(jìn)行求導(dǎo),得到狀態(tài)跟蹤誤差模型:

(3)

式中:μ1(t)=vr(t)cosθe(t)-v(t),μ2(t)=ur(t)sinθe(t),μ3(t)=ωr(t)-ω(t)。由于約束條件的存在,將式中的誤差變量μe(t)=[μ1(t)μ2(t)μ3(t)]T約束定義為:

|μe(t)|≤μe,i,max(t),i=1,2,3

(4)

2 終端約束模型預(yù)測(cè)控制策略

2.1 優(yōu)化問(wèn)題描述

將狀態(tài)誤差模型改寫成如下表達(dá)式:

(5)

令δ為采樣步長(zhǎng),在每個(gè)采樣時(shí)刻t=jδ,j=0,1…設(shè)計(jì)終端約束MPC策略的成本函數(shù)為:

(6)

式中:J(χe(t),μe(t))表示成本函數(shù),Tp表示有限預(yù)測(cè)時(shí)域。階段成本函數(shù)和終端成本函數(shù)表達(dá)式為:

式中:Q、R表示正定對(duì)稱加權(quán)矩陣,終端懲罰矩陣P與終端成本函數(shù)P(χe(t+Tp|t))相關(guān)。在t時(shí)刻,通過(guò)最小化狀態(tài)跟蹤誤差函數(shù),從而找到最優(yōu)控制序列,那么在終端約束MPC控制器中優(yōu)化問(wèn)題表示為:

(7)

式中:Ω表示狀態(tài)跟蹤誤差的終端約束中的終端域,Ω的選取將在下一小節(jié)進(jìn)行討論。本文用帶“*”和“-”分別代表最優(yōu)控制序列和可行控制序列。用最新的測(cè)量值求解優(yōu)化問(wèn)題得到最優(yōu)控制序列表示為:

(8)

(9)

在t+δ時(shí)刻,需要重新讀取當(dāng)前狀態(tài),在得到可行控制序列之前,為了確保有解,需預(yù)選如下的可行控制序列:

(10)

式中:κ(χe(τ|t+δ))是終端約束中的終端控制器。因此可行的預(yù)測(cè)軌跡通過(guò)計(jì)算得到:

(11)

2.2 終端約束的設(shè)計(jì)

通過(guò)引入終端約束,保證優(yōu)化問(wèn)題的穩(wěn)定性與可行性。終端約束由終端域和終端懲罰矩陣構(gòu)成。終端約束示意圖如圖2所示。

圖2 終端約束示意圖

假設(shè)存在終端控制器μe(t)=κχe(t)使得模型(3)漸進(jìn)穩(wěn)定,則選擇狀態(tài)誤差

(12)

的一個(gè)不變域作為終端域。利用終端成本函數(shù)來(lái)限制無(wú)限時(shí)域的成本函數(shù),即:

(13)

此時(shí)將時(shí)間域[t,∞]分為兩個(gè)部分:有限時(shí)域[t,t+Tp]和無(wú)限時(shí)域[t+Tp,∞]。此時(shí)對(duì)應(yīng)的成本函數(shù)為:

J(χe(t),μe(t))

(14)

這表明優(yōu)化問(wèn)題(7)中的成本函數(shù)是在時(shí)間域趨于無(wú)窮的成本函數(shù)的一個(gè)上界。

將證明這樣選擇的終端域和終端懲罰矩陣將保證WMR的穩(wěn)定性。將跟蹤誤差系統(tǒng)(4)寫成為:

(15)

定理1:對(duì)于模型(3)在平衡點(diǎn)處是可穩(wěn)的,則有以下兩個(gè)定理:

(1)如果Q*=Q+κTRκ為對(duì)稱正定的,則對(duì)任何滿足κα>-λmax(AK)的非負(fù)數(shù)κα∈[0,∞),Lyapunov方程

(AK+καI)TP+P(AK+καI)=-Q*

(16)

有唯一正定解。

(2)存在一個(gè)正常數(shù)α∈(0,∞),使得終端域的定義為:

(17)

則Ω滿足以下條件:

①在Ω內(nèi),輪式機(jī)器人系統(tǒng)滿足狀態(tài)約束,終端控制器μe(t)=κχe(t)滿足控制約束。

③對(duì)于給定的χe(t)∈Ω,沿著模型(3)任何始于χe(t)的軌跡,使得成本函數(shù)值有上界:

(18)

證明:由于Q*>0,通過(guò)Laypunov穩(wěn)定性判別法分析,如果(AK+καI)的所有特征值都具有負(fù)實(shí)部,則有唯一的對(duì)稱正定解。由于AK是漸進(jìn)穩(wěn)定的,能夠滿足κα∈[0,-λmax(AK))的任何常數(shù)κα均能保證(AK+καI)的所有特征值都具有負(fù)實(shí)部。因此,定理1得證。

由于平衡點(diǎn)是處于約束內(nèi)部,所以對(duì)于任何固定的P>0,總可以找到一個(gè)正數(shù)α∈(0,∞)使得在終端域內(nèi),WMR滿足狀態(tài)約束。

(19)

式中:與φ(χe(t))相關(guān)的項(xiàng)存在這樣的上界:

(20)

(21)

成立,則不等式(20)變?yōu)?

(22)

將式(22)帶入式(19),結(jié)合式(16)得:

(23)

由于P>0,由式可知Q*>0。可以得出,從終端域起始的任何軌跡都保持在Ω內(nèi)。同理,如果令χe(t)=χe(t+Tp),則式(13)和式(18)是等效的。所以,式(16)求解出的P作為終端懲罰矩陣可以使得區(qū)域Ω作為終端域滿足終端約束。

為保證終端約束MPC策略優(yōu)化問(wèn)題的可行性和系統(tǒng)穩(wěn)定性,設(shè)計(jì)的終端約束需要滿足如下引理:

引理1:對(duì)于狀態(tài)跟蹤誤差系統(tǒng)(3),在時(shí)間段τ∈[t+Tp,t+δ+Tp]內(nèi),終端域Ω和終端控制器μe(τ|t+δ)=κ(χe(τ|t+δ))要滿足不等式:

(24)

3 系統(tǒng)可行性與穩(wěn)定性分析

3.1 系統(tǒng)可行性分析

優(yōu)化問(wèn)題的可行性意味著至少存在一個(gè)控制序列使得滿足優(yōu)化問(wèn)題(7),同時(shí)滿足約束條件。

(25)

在預(yù)選的控制序列第2部分,即κ(χe(τ|t+δ)),也在終端域內(nèi)。在時(shí)間段[t+Tp,t+δ+Tp]內(nèi),狀態(tài)跟蹤誤差為:

(26)

由于在終端域Ω內(nèi),κ(χe(τ|t+δ))也滿足控制約束。由數(shù)學(xué)歸納法可知,在t+δ時(shí)刻也可行,這說(shuō)明優(yōu)化問(wèn)題(7)在t>0是可行的。

3.2 系統(tǒng)穩(wěn)定性分析

定理2:對(duì)于WMR軌跡跟蹤問(wèn)題,假設(shè)t=0時(shí)刻優(yōu)化問(wèn)題(7)是存在可行解的,通過(guò)求解Lyapunov方程獲得的終端懲罰矩陣P和終端域Ω滿足約束條件。對(duì)任何時(shí)刻t≥0和τ∈(t,t+δ],狀態(tài)跟蹤誤差(3)漸進(jìn)趨于零點(diǎn)。

證明:在時(shí)間段[t,t+Tp)內(nèi),對(duì)任何的τ∈(t,t+δ],這段時(shí)間的可行解由式(9)獲得,表達(dá)式為:

(27)

那么,可行成本函數(shù)表示為:

(28)

根據(jù)式(10),在時(shí)間段τ∈(t,t+δ],構(gòu)建一個(gè)可行的控制序列,即:

(29)

第1部分是[τ,t+Tp),這是在t時(shí)刻通過(guò)求解優(yōu)化問(wèn)題(7)得到控制序列去掉第1個(gè)采樣周期后的部分。在第2部分,即在[t+Tp,τ+Tp)時(shí)間段內(nèi)的控制。根據(jù)引理1,將不等式(24)從t+Tp到τ+Tp進(jìn)行積分,得到:

(30)

利用式(28),計(jì)算成本函數(shù)為:

(31)

將式(30)帶入到式(31)結(jié)合式(29)得到:

(32)

這就說(shuō)明成本函數(shù)在時(shí)間段τ∈[t,t+δ)是不增的。考慮在t時(shí)刻和t+δ時(shí)刻的更新表達(dá)式:

(33)

對(duì)不等式(24)從t+Tp到t+δ+Tp進(jìn)行積分,得到:

(34)

將不等式(34)帶入到式(33),得到:

(35)

則可以得到成本函數(shù)是趨于零點(diǎn)的,定理2證明完畢,說(shuō)明系統(tǒng)是穩(wěn)定的。

4 系統(tǒng)實(shí)驗(yàn)分析

研究?jī)?nèi)容通過(guò)MATLAB進(jìn)行仿真。在Simulink中搭建的終端約束MPC控制器模塊,對(duì)WMR進(jìn)行仿真實(shí)驗(yàn)。本文采用文獻(xiàn)[15]的MPC控制方案作為對(duì)比實(shí)驗(yàn),即TMPC控制方案。假設(shè)WMR跟蹤的軌跡提前給出,即不考慮WMR的軌跡規(guī)劃。

為使得系統(tǒng)控制效果最優(yōu),需要對(duì)終端約束MPC控制器參數(shù)進(jìn)行優(yōu)化,給定加權(quán)矩陣Q、R。通過(guò)定理1,求解優(yōu)化問(wèn)題(7),使用極點(diǎn)配置法,得到穩(wěn)定可以獲得終端懲罰矩陣P,同時(shí)需要滿足引理1,即可得到終端約束MPC參數(shù)。對(duì)于TMPC控制方案作為對(duì)照采用相同的控制參數(shù)。給出終端約束MPC控制方案和TMPC控制方案仿真控制參數(shù)如表1所示。

表1 控制參數(shù)對(duì)比

將WMR各項(xiàng)的誤差平均值及方差值進(jìn)行對(duì)比,同時(shí)采用狀態(tài)誤差的平方和誤差(sum of the squared errors,SSE)以評(píng)估兩種方案的控制性能。其定義為:

(36)

計(jì)算得出的結(jié)果越大,這說(shuō)明實(shí)際值與從參考值的差距就越大。計(jì)算得出的結(jié)果越小,則說(shuō)明控制性能效果越好。

4.1 圓形軌跡跟蹤仿真

給定圓形軌跡為:

xr(t)=5sin(0.2t),yr(t)=5-5cos(0.2t)

(37)

式中:t∈[0,35]。設(shè)定終端約束MPC中的線速度v和角速度ω的分別為v=1 m/s和ω=0.6 rad/s。設(shè)定線速度增量和角速度增量為|Δv|≤0.1 m/s,|Δω|≤0.3 rad/s在仿真過(guò)程中,WMR的期望軌跡的初始狀態(tài)為[x(0)y(0)θ(0)]T=[0,2,0]T,將實(shí)際軌跡起點(diǎn)與期望軌跡起點(diǎn)作為初始誤差,仿真結(jié)果如圖3所示。

圖3 圓形軌跡跟蹤對(duì)比圖

從圖3中觀察可知,使用終端約束MPC控制方案和文獻(xiàn)[15]中的TMPC控制方案都能實(shí)現(xiàn)WMR圓形軌跡的跟蹤。然而,當(dāng)存在初始位置誤差的情況下,終端約束MPC的跟蹤性能表現(xiàn)更優(yōu),而TMPC則稍遜一籌。這主要在于終端域的引入,它確保了WMR在有限時(shí)域內(nèi)跟蹤到參考軌跡上。圖4a和圖4b為跟蹤誤差圖。

(a) 終端約束MPC軌跡誤差圖 (b) TMPC軌跡跟蹤誤差圖

從圖4可以看出,在終端約束MPC控制方案下,WMR收斂到零的速度比TMPC控制方案更快。在終端約束MPC跟蹤到期望軌跡為調(diào)節(jié)時(shí)間t=7.2 s,而在TMPC控制方案下,其調(diào)節(jié)時(shí)間為t=15.59 s。當(dāng)存在初始誤差時(shí),終端約束MPC其對(duì)轉(zhuǎn)向過(guò)程的響應(yīng)更快,提高了WMR的軌跡跟蹤精度。

從表2可以具體得出終端約束MPC控制方案相較于TMPC控制方案的狀態(tài)跟蹤誤差都較小,并且計(jì)算出的SSE更小。圖5展示了兩種控制方案的控制量對(duì)比。

表2 圓形軌跡仿真對(duì)比

圖5 圓形軌跡控制量對(duì)比圖

從圖5中可以看出,終端約束MPC的線速度v和角速度ω均滿足輸入約束條件。相反,TMPC的線速度和角速度部分超出了預(yù)設(shè)的約束條件。這表明,相較于TMPC,終端約束MPC在預(yù)測(cè)范圍內(nèi)更好地滿足了約束條件,從而提高了系統(tǒng)的穩(wěn)定性。

4.2 雙移線軌跡跟蹤仿真

為凸顯終端約束MPC控制方案的優(yōu)勢(shì),在雙移線工況下進(jìn)行仿真。給定雙移線軌跡為:

(38)

式中:t∈[0,10],且:

(39)

式中:yr(t)是xr(t)的非線性函數(shù)。在雙移線工況中,設(shè)定終端約束MPC中的線速度v和角速度ω分別為v=1 m/s和ω=0.3 rad/s。設(shè)定線速度增量和角速度增量仍然為|Δv|≤0.1 m/s,|Δω|≤0.3 rad/s。

在雙移線軌跡跟蹤仿真中,設(shè)定WMR初始狀態(tài)為[x(0)y(0)θ(0)]T=[-1,-2,0]T,仿真結(jié)果如圖6所示。

圖6 雙移線軌跡跟蹤對(duì)比圖

在圖6中可以看出,終端約束MPC控制方案能夠從設(shè)定的位置穩(wěn)定地跟蹤上參考軌跡。而當(dāng)參考路徑曲率較大時(shí),TMPC控制方案會(huì)存在較大的跟蹤誤差,終端約束MPC能較好的跟蹤上參考軌跡。

圖7a和圖7b顯示了兩種控制方案在雙移線工況下的跟蹤誤差情況。終端約束MPC調(diào)節(jié)時(shí)間為t=2.35 s,而文獻(xiàn)[15]的TMPC控制方案的調(diào)節(jié)時(shí)間為t=4.6 s,之后仍然存在穩(wěn)態(tài)誤差。終端約束MPC控制方案在橫向誤差上比TMPC控制方案大1 cm,然而在其他方面,終端約束MPC的表現(xiàn)都優(yōu)于TMPC。同時(shí),從SSE的角度來(lái)看,終端約束MPC相較于TMPC控制方案具有較小的誤差,從而表現(xiàn)出更好的控制效果。兩種控制方案如表3所示。

表3 雙移線軌跡仿真對(duì)比

(a) 終端約束MPC跟蹤誤差圖 (b) TMPC軌跡跟蹤誤差圖

圖8為雙移線軌跡控制量對(duì)比圖,終端約束MPC滿足控制約束。線速度兩種控制方案差別不大,而角速度,TMPC最大角速度為4 rad/s,超出了約束范圍,而終端約束MPC被限制在|Δω|≤0.3 rad/s內(nèi)。

圖8 雙移線軌跡跟蹤控制量對(duì)比圖

因此,兩種工況下都驗(yàn)證了終端約束MPC基于更強(qiáng)的穩(wěn)定性。說(shuō)明終端約束MPC在處理具有復(fù)雜動(dòng)態(tài)特性和約束條件時(shí)具有更好的性能。

5 結(jié)論

本文針對(duì)WMR軌跡跟蹤控制問(wèn)題,提出了一種終端約束MPC控制策略。首先,通過(guò)分析WMR的運(yùn)動(dòng)學(xué)方程,推導(dǎo)出狀態(tài)誤差方程作為預(yù)測(cè)模型。在此基礎(chǔ)上,充分考慮非線性和時(shí)域約束特性,將終端約束引入成本函數(shù)。終端域和終端懲罰矩陣共同構(gòu)成了終端約束的穩(wěn)定性條件;隨后,在Lyapunov穩(wěn)定性框架內(nèi),探討了終端約束MPC控制策略的可行性和穩(wěn)定性;最后,仿真實(shí)驗(yàn)結(jié)果驗(yàn)證了所設(shè)計(jì)控制策略的有效性。

對(duì)于今后的研究中,將考慮算法的實(shí)時(shí)性、使用動(dòng)力學(xué)模型作為預(yù)測(cè)模型等,在此基礎(chǔ)上進(jìn)一步完善本文所提出的控制方案,以提高軌跡跟蹤控制性能。