基于改進(jìn)GMRES 算法的水冷壁運(yùn)動(dòng)機(jī)器人路徑跟蹤模型預(yù)測控制①

黃 巍 占紅武 胥 芳

(浙江工業(yè)大學(xué)特種裝備制造與先進(jìn)加工技術(shù)教育部/浙江省重點(diǎn)實(shí)驗(yàn)室 杭州310023)

0 引言

近年來,非完整輪式移動(dòng)機(jī)器人[1](non-holonomic wheeled mobile robot,NWMR)在運(yùn)輸、探測、救援等領(lǐng)域引起了廣泛的關(guān)注。如在火電廠中,其應(yīng)用范圍覆蓋了水冷壁面巡查檢測、缺陷定位[2]和主輔機(jī)關(guān)鍵部件裂化評估維護(hù)[3]。人們對NWMR的運(yùn)動(dòng)控制[4]進(jìn)行了更加深入的研究,提出了各種控制方法,其中包括神經(jīng)網(wǎng)絡(luò)控制法[5-6]、滑膜控制[7]和非連續(xù)反饋控制法[8]。但當(dāng)考慮控制輸入以及約束處理時(shí),以上算法很難達(dá)到較為直觀的效果。然而,非線性模型預(yù)測控制法(nonlinear model predictive controller,NMPC)卻可以很好地解決這個(gè)問題,該控制方法以當(dāng)前時(shí)間間隔下NWMR 狀態(tài)的預(yù)測量為初始條件,基于反饋控制律對系統(tǒng)的未來軌跡進(jìn)行預(yù)測,利用控制序列對性能指標(biāo)函數(shù)進(jìn)行優(yōu)化。在此框架下構(gòu)造的最優(yōu)化問題具有高度的靈活性[9]、良好的動(dòng)態(tài)性及魯棒性,因此在處理多變量多約束問題時(shí)表現(xiàn)優(yōu)秀,吸引了大量研究者的目光。文獻(xiàn)[10]設(shè)計(jì)了一種非線性模型預(yù)測控制算法(NMPC)用于解決誤差動(dòng)力學(xué)的軌跡跟蹤控制問題。文獻(xiàn)[11]針對自主水面艦艇的非線性MPC 軌跡跟蹤控制問題,為提高算法效率引入了啟發(fā)式方法來處理輸入約束。文獻(xiàn)[12]針對NMPC 求解優(yōu)化問題的滾動(dòng)可行性問題,將參考軌跡作為性能指標(biāo)的終端約束以此保證算法收斂。

然而,NMPC 所帶來的高計(jì)算負(fù)荷不可避免地與較小的采樣周期產(chǎn)生沖突。為了提高運(yùn)算速度,滿足實(shí)時(shí)性要求,許多研究者都致力設(shè)計(jì)有效的在線算法,諸如快速梯度下降[13]、計(jì)算除法[14]、延遲補(bǔ)償[15]及數(shù)值連續(xù)法[16]。作為結(jié)合連續(xù)法[17]和Krylov 子空間法[18]的算法,連續(xù)/廣義極小殘余算法[19](continuation/general minimum residual,C/GMRES)已經(jīng)發(fā)展成為一種求解NMPC 的快速算法。該算法通過確定控制序列在每個(gè)時(shí)間間隔的增量,而非迭代求解,提高了運(yùn)算速率。

本文研究了水冷壁爬壁機(jī)器人(water wall climbing robot,WWCR)的非線性模型預(yù)測控制(NMPC)路徑跟蹤問題。利用WWCR 實(shí)時(shí)狀態(tài)與虛擬目標(biāo)狀態(tài)之間的誤差為模型,建立路徑跟蹤問題NMPC 框架,基于龐特里亞金極小值原理,轉(zhuǎn)換為Bolza 形式的NMPC 最優(yōu)控制問題。鑒于文獻(xiàn)[20]的C/GMRES 算法同時(shí)具備數(shù)值連續(xù)法與Krylov 子空間方法的優(yōu)點(diǎn),引入該算法對NMPC 進(jìn)行計(jì)算求解。然而其算法未能完全考慮NWMR 路徑跟蹤問題中狀態(tài)與輸入向量不等式約束,本文通過引入正則平滑函數(shù)(Fischer-Burmeister,FB),將不等式約束與其互補(bǔ)條件聯(lián)立構(gòu)建最優(yōu)化條件方程,引入多重打靶法,將誤差分布到狀態(tài)向量與共態(tài)向量之中,以此提升算法的控制精度。在求解過程中,對約束和控制輸入向量進(jìn)行延拓,提出一種壓縮變量的優(yōu)化方法,對方程進(jìn)行簡化處理,減小線性方程大小,同時(shí)對C/GMRES 算法中的殘量求取進(jìn)行優(yōu)化,避免Hessenberg 矩陣的分解運(yùn)算,降低了算法的計(jì)算負(fù)載,加快算法運(yùn)行速度。最后進(jìn)行仿真,驗(yàn)證該算法的可行性。

1 非線性模型

本文實(shí)驗(yàn)對象主要針對履帶式爬壁機(jī)器人(NWMR),定義圖1 所示的世界坐標(biāo)系{q},建立路徑跟蹤模型,工作環(huán)境如圖2 所示。

圖1 路徑跟蹤模型

圖2 水冷管壁圖

定義NWMR 所處位置為(sq,yq),其速度為v,角速度為ω。

其中X=[sq yq θq]T為狀態(tài)向量,U=[v ω]T為控制輸入向量。

定義虛擬目標(biāo)所處坐標(biāo)系為{r},需要跟蹤的路徑為[sr yr θr]T。Ur=[vr ωr]表示參考控制信號矢量?？紤]到在實(shí)際跟蹤時(shí)由于外部因素導(dǎo)致的局部跟蹤誤差,定義誤差狀態(tài)由Xe=[se ye θe]T表示,由此可得NWMR 的動(dòng)態(tài)誤差模型:

其中,fl與fr分別表示左右履帶電機(jī)的輸入驅(qū)動(dòng)轉(zhuǎn)矩,m與I分別為NWMR 的質(zhì)量及其慣性矩。在實(shí)際工作中,考慮到由于外部因素導(dǎo)致速度突變引起的運(yùn)行不穩(wěn)定,利用虛擬控制量表示NWMR 的加速度與角加速度,對動(dòng)力學(xué)系統(tǒng)簡化為

根據(jù)式(2)～式(4)擴(kuò)展系統(tǒng)狀態(tài),可得閉環(huán)系統(tǒng)狀態(tài)方程為

令x=[s,y,θ,v,ω]T為NWMR 的狀態(tài)向量,控制輸入向量為u=[u1,u2]T。

則式(5)中的閉環(huán)系統(tǒng)可用如下非線性表達(dá)式表示。

根據(jù)上述建立的模型,便可將路徑跟蹤問題轉(zhuǎn)換為尋根問題,即通過尋找相對最優(yōu)的虛擬輸入u合適的控制率,滿足Xe=0。

2 快速非線性模型預(yù)測控制方法

基于目標(biāo)函數(shù)的內(nèi)點(diǎn)[21](inner point,IP)重新構(gòu)造的轉(zhuǎn)換方法,將潛在的優(yōu)化問題轉(zhuǎn)化為等式約束進(jìn)行求解,是處理NMPC 的常用方法之一。雖然在一定程度上導(dǎo)致了兩點(diǎn)邊值問題(twopoint boundary value problem,TP-BVP),但是這種基于PMP 的間接法仍然是求解OCP 問題較為有效且精度較高的方法。作為一種非線性滾動(dòng)時(shí)域控制的快速數(shù)值算法,GMRES 算法避免了傳統(tǒng)的牛頓型數(shù)值算法中復(fù)雜的Riccati 微分方程,其基于序列公式,在每個(gè)采樣時(shí)間僅進(jìn)行一次求解,同時(shí)利用前向差分替代部分方程近似值,以此來提高運(yùn)算速度。針對算法內(nèi)部的標(biāo)準(zhǔn)基構(gòu)造進(jìn)行改進(jìn),加快了計(jì)算速度。對于約束問題,引入Fischer-Burmeister 正則化平滑算法(FBRS)[22],利用FB 函數(shù)將最優(yōu)性的必要條件映射到非光滑方程組,以增加算法的穩(wěn)定性。融合多重打靶法來提高計(jì)算精度,并對殘量求取進(jìn)行優(yōu)化處理。

2.1 問題描述

針對模型建立的NMPC 問題,在不失一般性的情況下,可將其描述為Bolza 形式的最優(yōu)控制問題,同時(shí)考慮在預(yù)測范圍內(nèi)的階段目標(biāo)函數(shù)L以及出現(xiàn)在范圍末端的終端目標(biāo)函數(shù)V。

其中,式(9) 作為邊界條件,x0和xf分別表示NMWR 的初始狀態(tài)和終止?fàn)顟B(tài)。式(10)表示約束,考慮虛擬輸入向量u的引入,定義非線性系統(tǒng)的不等式約束g(x,u) 如下所示。

式(7)中,V(x(t+T)) 表示終端約束,表達(dá)式為

其中,Qf為終端狀態(tài)權(quán)重矩陣。右側(cè)第2 項(xiàng)為拉格朗日被積函數(shù),表達(dá)式為

上式中Q=diag{q1,…,q5}、R=diag{r1,r2} 均表示權(quán)重矩陣,qi、ri為非負(fù)常數(shù)。在上述性能指標(biāo)函數(shù)中,第1 項(xiàng)表示NWMR 系統(tǒng)模型,第2 項(xiàng)為其邊界條件。

2.2 不等式約束

針對不等式約束,對計(jì)算求解帶來很大的負(fù)擔(dān),許多文獻(xiàn)通過各種辦法將不等式轉(zhuǎn)變?yōu)榈仁郊s束來計(jì)算,例如引入外部障礙函數(shù)[23]、勢壘函數(shù)[24]、輔助變量法[25]。本文通過引入FBRS,同時(shí)使用來自對偶空間的信息生成不同的搜索方向,即算法同時(shí)在原始空間和對偶空間中運(yùn)行找到原始OCP 及其對偶的解。使用非線性互補(bǔ)(NCP)函數(shù)將互補(bǔ)條件式(10)映射到方程組。定義廣義半光滑函數(shù)為

其中ε >0,為正則參數(shù)。將拉格朗日算子μ及不等式約束g(x,u) 作為互補(bǔ)條件代入半光滑函數(shù),得到:

需要注意的是附加的拉格朗日乘子μ必須加入到輸入變量Ut中迭代計(jì)算,記為

為避免當(dāng)虛擬輸入變量為0 時(shí)導(dǎo)致的C/GMRES優(yōu)化更新時(shí)的奇異性,需要在代價(jià)函數(shù)中引入懲罰因子,則拉格朗日函數(shù)修改為

為避免懲罰因子過大而對尋找最優(yōu)解產(chǎn)生影響,故選擇ki為小的正常數(shù)。

2.3 最優(yōu)化條件

通過Karush-Kuhn-Tucker (KKT)最優(yōu)性條件導(dǎo)出求解動(dòng)態(tài)OCP 的最優(yōu)性條件,利用廣義哈密頓函數(shù)表示:

其中,λ表示共態(tài)向量。為了解決TP-BVP 中,控制輸入函數(shù)為未知量的情況,將預(yù)測范圍T分為N個(gè)階段,達(dá)到利用有限個(gè)參數(shù)來表示控制輸入函數(shù)的目的。使用歐拉-拉格朗日方程變分法來離散化求解動(dòng)態(tài)OCP 最優(yōu)化的必要條件。離散化如下:

其中Δτ=T/N。分別用和表示在離散視界上的狀態(tài)序列、共態(tài)序列以及控制輸入序列。通過以上離散化的方法,成功地將NMPC 表示為時(shí)間t時(shí)測量狀態(tài)x(t) 的離散時(shí)間TP-BVP。

重新定義一個(gè)由拉格朗日乘子μ和輸入向量組成的離散化的控制輸入向量U(t) ∈RmuN:

將FB 函數(shù)作為互補(bǔ)條件,與最優(yōu)化條件組成約束條件為

相比于將誤差僅分布在F=0 中,多重打靶法傾向于將誤差同時(shí)分布在狀態(tài)向量和共態(tài)向量中時(shí),預(yù)計(jì)將會(huì)減小整體誤差,即算法的精度會(huì)提高,有利于非線性系統(tǒng)的穩(wěn)定性及收斂性。使用多重打靶算法時(shí),需要將狀態(tài)向量x、拉格朗日乘子μ、共態(tài)向量λ以及控制輸入向量u視為獨(dú)立變量。將連續(xù)狀態(tài)向量和共態(tài)向量組合構(gòu)建出一個(gè)新的向量

建立狀態(tài)和共態(tài)向量的代數(shù)約束:

通過建立上述約束,可以保證系統(tǒng)總是相對之前獲得的預(yù)測條件進(jìn)行計(jì)算,并且其初始狀態(tài)將不會(huì)在整個(gè)時(shí)域T上進(jìn)行積分。

2.4 優(yōu)化求解

針對方程組式(26)和式(28),利用t為參數(shù)的連續(xù)法[26],同時(shí)進(jìn)行延拓。首先需對約束條件F與控制輸入向量U進(jìn)行拆分處理:

同時(shí),拆分時(shí)需要滿足以下3 個(gè)條件:

(1) 作為消去與的必要條件之一,和的維數(shù)必須相同。

(2) 在分解時(shí),約束G中不包含中的任何元素,約束F^ 中不包含任何關(guān)于X的元素。且在連續(xù)狀態(tài)方程中沒有中的任何元素。在哈密頓量H中不包含與狀態(tài)向量交叉的項(xiàng),對應(yīng)此式即為在狀態(tài)方程中不存在虛擬輸入u及拉格朗日乘子μ。

(3) 為了在某一時(shí)間步的輸入向量u及拉格朗日乘子μ僅在其對應(yīng)時(shí)間步的哈密頓量H中出現(xiàn),要求當(dāng)與任意向量點(diǎn)乘時(shí)有解,且計(jì)算邊界較小。

對上述的3 個(gè)約束使用延拓法進(jìn)行關(guān)于t的延拓,帶入初始條件(0)、(0) 及X(0) 進(jìn)行積分操作求解,得到:

其中As是正穩(wěn)態(tài)矩陣。對式(33)～式(35)進(jìn)行合并化簡帶入后,得到關(guān)于的線性方程:

為了避免逆矩陣帶來龐大計(jì)算量,提出一種消除式(36)中的矩陣方法。引入一個(gè)虛擬變量ρk∈R2nN,滿足,對其進(jìn)行移項(xiàng)處理后有:

計(jì)算中,為了減少雅可比矩陣與線性方程相乘時(shí)帶來的計(jì)算量,引入前向差分近似概念:

式中,h表示前向差分步長,是一個(gè)小的正實(shí)數(shù)。利用前向差分近似處理式(37)有:

化簡可得:

在上述算法中已經(jīng)提到,多重打靶法相比于原算法的改進(jìn)之處,在于將誤差同時(shí)分布到了狀態(tài)向量和共態(tài)向量約束G中來減小整體誤差。取誤差向量表示為,對X滿足:

定義一個(gè)映射B:RmN ×R2nN ×Rn ×R →R2nN,當(dāng)U、x、t均確定時(shí),B即為G的逆映射。由此,根據(jù)G的定義可得X=B(,E,x,t),且滿足:

通過此方法分別求得xi、λi后,即可通過遞歸計(jì)算求得B(,E,x,t),結(jié)合式(40)可以推導(dǎo)出:

對式(45)進(jìn)行變換即可求得虛擬變量ρk,即可得到的值為

上式雖然看似比較復(fù)雜,其實(shí)可以僅僅使用和B直接表示獲得。

對式(48)前向差分近似處理,得到:

化簡后可得:

根據(jù)上文所提的映射B,可以解得X+hω:

由條件式(3)可知求解時(shí)計(jì)算邊界很小,則式(36)右側(cè)可以化簡為

綜上,利用GMRES 算法對式(36)計(jì)算,快速求解獲得。根據(jù)映射B及式(51),無需通過GMRES即可求得:

根據(jù)式(34)可以解得:

在求得及之后,利用歐拉法(Euier)分別對其積分,更新控制輸入U(xiǎn)及狀態(tài)向量X。

2.5 算法優(yōu)化

多重打靶法的引入提高算法精度的同時(shí),也帶入了較為沉重的計(jì)算負(fù)擔(dān),主要集中在對每個(gè)時(shí)間步上非線性函數(shù)的雅可比矩陣的計(jì)算,比使用傳統(tǒng)的GMRES 算法計(jì)算效率大幅提高。然而,該算法在對Hessenberg 矩陣進(jìn)行正交分解時(shí),依然涉及比較復(fù)雜繁瑣的計(jì)算。因此,此算法在此處仍可進(jìn)行優(yōu)化。很多學(xué)者都提出了基于GMRES 算法的改進(jìn)算法[27]。然而在一些情況無法避免地出現(xiàn)震蕩現(xiàn)象。本文通過對Krylov 子空間的第2 個(gè)元素開始進(jìn)行Arnoldi 過程處理,將原本的n × m維Hessenberg矩陣,改進(jìn)為上三角矩陣Wk,避免正交分解處理,就能減少計(jì)算量。

考慮非線性方程:

為提高該控制算法的穩(wěn)定性[28],可將子空間Kk(A,r0)=span{r0,Ar0,…,Ak-1r0} 的元素,即傳統(tǒng)GMRES 算法的基替換為如下形式:

其中初始參量為r0=b -AM0,其余參量滿足ri=b-AMi∈Kk(A,r0)。將向量組Uk進(jìn)行Arnoldi 處理:

式中Vk=(v1,v2,…,vk)T,為AKk(A,r0) 的標(biāo)準(zhǔn)正交基,令wi,j=(Auj,vi) 為上三角矩陣,取‖vj+1‖F(xiàn)=wi,j+1,求得的Wk為上三角矩陣,取Mk=M0+tk,其中tk∈Uk,即tk=Ukyk,yk∈Rk×1。假設(shè)經(jīng)過k次迭代后,有

通過調(diào)整yk使得rk與AUkyk垂直時(shí),‖rk‖F(xiàn)取得最小值,即

結(jié)合式(59)可得到:

求得近似解:

改進(jìn)之后的GMRES 算法減少了正交分解所帶來的額外的計(jì)算需求,提高了算法的運(yùn)行效率與穩(wěn)定性。

3 仿真結(jié)果

在仿真模擬中使用為個(gè)人電腦的CPU 為英特爾Core i7 -9750H,6 核,時(shí)鐘頻率為4.5 GHz。在Matlab 2016a 環(huán)境下對圓形(固定曲率)與伯努利雙紐線(變曲率)路徑進(jìn)行跟蹤仿真模擬。

圓形路徑定義如下:

式中,?為路徑參數(shù),確定了虛擬目標(biāo)在世界坐標(biāo)系下的位置,初始狀態(tài)和終端狀態(tài)分別選取為

初始輸入為u=[0 0]T,仿真參數(shù)設(shè)置中,T=1 s,N=10,采樣時(shí)間選擇為Δτ=0.1,權(quán)重矩陣取:

約束參數(shù)中設(shè)置vmin=0 m/s,vmax=0.16 m/s,ωmax=0.8 rad/s,u1min=-0.08 m/s2,u1max=0.08 m/s2,u2min=-0.6 rad/s2,u2max=0.6 rad/s2。

伯努利雙紐線路徑定義為

初始狀態(tài)與終端狀態(tài)分別選取為

初始輸入為u=[0 0]T,仿真參數(shù)設(shè)置中,T=1 s,N=10,采樣時(shí)間選擇為Δτ=0.1,權(quán)重矩陣取:

約束參數(shù)中設(shè)置vmin=0 m/s,vmax=1 m/s,ωmax=0.8 rad/s,u1min=-0.2 m/s2,u1max=0.3 m/s2,u2min=-0.4 rad/s2,u2max=1 rad/s2。

其他相關(guān)參數(shù)為ε=0.01,h=0.001,As=Dh=1/h=1000,m=1.5,R=0.048,I=0.0348,C/GMRES 算法內(nèi)部參數(shù)設(shè)置初始控制輸入與乘子的誤差容限r(nóng)tol=1.0e-8,GMRES 的迭代數(shù)kmax=10。

仿真中引入另外2 種算法與本文所提算法進(jìn)行對比。第1 種選擇Matlab 自帶的SQP 算法;第2 種為MSC[29]算法,通過障礙函數(shù)法對不等式進(jìn)行處理;第3 種為本文提出的改進(jìn)算法(NMPC)。其中ref 表示虛擬目標(biāo)的參考路徑。跟蹤誤差表示為,最優(yōu)性誤差‖F(xiàn)‖,定義為式(26)中F的2 -范數(shù)。

對圓形路徑的跟蹤仿真中,設(shè)定運(yùn)行時(shí)間為10 s,由圖3(b)可知,針對固定曲率的路徑進(jìn)行跟蹤,3種算法都表現(xiàn)出良好的跟蹤效果,在穩(wěn)定跟蹤后,跟蹤誤差均能控制在0.01 m 范圍內(nèi)。相比之下,本文所述算法的跟蹤結(jié)果在0.5 s 時(shí)已經(jīng)跟蹤至目標(biāo)路徑且趨于穩(wěn)定,最優(yōu)性誤差峰值最低,比另外2 種算法更為敏感。從圖3(d)～(g)可知,跟蹤速度與加速度在接近3 s 時(shí)開始收斂,達(dá)到期望值,3 種算法表現(xiàn)相當(dāng)。

圖3 圓形路徑跟蹤仿真結(jié)果

對伯努利雙紐線路徑跟蹤仿真中,設(shè)定仿真運(yùn)行時(shí)間為10 s,由圖4(a)和(b)可知,針對變曲率路徑3 種控制效果差異較為明顯。SQP 算法在曲率變化較大的區(qū)域會(huì)出現(xiàn)較為明顯的跟蹤誤差,且其最優(yōu)性誤差峰值較大,對于拐角區(qū)域的路徑并不敏感。相比較而言MSC 與本文的算法控制效果更佳,在拐角區(qū)域基本可以穩(wěn)定跟蹤。同時(shí)針對不同預(yù)測步長N,對3 種算法的控制更新平均計(jì)算時(shí)長和平均誤差進(jìn)行了仿真,結(jié)果如表1 和表2 所示。在N=5的條件下,MSC 與SQP 滿足在采樣周期中進(jìn)行跟蹤。然而,隨著采樣步長的增加,兩種算法的計(jì)算速率開始降低,在N=10 的條件下,平均計(jì)算時(shí)間分別達(dá)到了0.132 s和0.2134 s,都超過了采樣周期,代表著其無法在采樣時(shí)間要求0.1 s 內(nèi)獲得NMPC 的最優(yōu)控制輸入,在實(shí)際工作時(shí)可能無法準(zhǔn)確進(jìn)行路徑跟蹤。相較而言,本文算法在計(jì)算速率上展現(xiàn)出了優(yōu)勢,即使在采樣步長N=50 的情況下,依舊可以僅消耗采樣時(shí)長10% 以內(nèi)的時(shí)間有效求解NMPC,同時(shí)滿足運(yùn)行的實(shí)時(shí)性要求,雖然相比較MSC 其誤差有所增加,但依舊滿足精度要求。在針對更加復(fù)雜的非線性系統(tǒng)時(shí),本文所述算法的提升會(huì)更加顯著。

圖4 伯努利雙紐線路徑跟蹤仿真結(jié)果

表1 平均計(jì)算時(shí)長

表2 平均跟蹤誤差

4 結(jié)論

針對非完整移動(dòng)機(jī)器人路徑跟蹤問題,本文通過定義NWMR 與虛擬目標(biāo)的跟蹤誤差,建立Bolza形式的NMPC 最優(yōu)控制問題框架。引入半平滑函數(shù),將不等式約束融入最優(yōu)化必要條件之中,通過使用GMRES 算法計(jì)算求解廣義哈密頓函數(shù)。同時(shí)引入多重打靶法及改變殘量求取方式以提高運(yùn)算精度及運(yùn)算速率。仿真結(jié)果表明,本文所述的控制算法可以在滿足實(shí)時(shí)性的前提下,保證更高的計(jì)算效率及精度。

然而本文并未對實(shí)際環(huán)境中的外界因素,如摩擦圓極限條件下的打滑情況[30]進(jìn)行考慮,因此針對如何提高該算法的魯棒性將是未來的研究方向。