黃 巍 占紅武 胥 芳

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

0 引言

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

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

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

1 非線性模型

本文實驗對象主要針對履帶式爬壁機(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]表示參考控制信號矢量?？紤]到在實際跟蹤時由于外部因素導(dǎo)致的局部跟蹤誤差,定義誤差狀態(tài)由Xe=[se ye θe]T表示,由此可得NWMR 的動態(tài)誤差模型:

其中,fl與fr分別表示左右履帶電機(jī)的輸入驅(qū)動轉(zhuǎn)矩,m與I分別為NWMR 的質(zhì)量及其慣性矩。在實際工作中,考慮到由于外部因素導(dǎo)致速度突變引起的運行不穩(wěn)定,利用虛擬控制量表示NWMR 的加速度與角加速度,對動力學(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)點[21](inner point,IP)重新構(gòu)造的轉(zhuǎn)換方法,將潛在的優(yōu)化問題轉(zhuǎn)化為等式約束進(jìn)行求解,是處理NMPC 的常用方法之一。雖然在一定程度上導(dǎo)致了兩點邊值問題(twopoint boundary value problem,TP-BVP),但是這種基于PMP 的間接法仍然是求解OCP 問題較為有效且精度較高的方法。作為一種非線性滾動時域控制的快速數(shù)值算法,GMRES 算法避免了傳統(tǒng)的牛頓型數(shù)值算法中復(fù)雜的Riccati 微分方程,其基于序列公式,在每個采樣時間僅進(jìn)行一次求解,同時利用前向差分替代部分方程近似值,以此來提高運算速度。針對算法內(nèi)部的標(biāo)準(zhǔn)基構(gòu)造進(jìn)行改進(jìn),加快了計算速度。對于約束問題,引入Fischer-Burmeister 正則化平滑算法(FBRS)[22],利用FB 函數(shù)將最優(yōu)性的必要條件映射到非光滑方程組,以增加算法的穩(wěn)定性。融合多重打靶法來提高計算精度,并對殘量求取進(jìn)行優(yōu)化處理。

2.1 問題描述

針對模型建立的NMPC 問題,在不失一般性的情況下,可將其描述為Bolza 形式的最優(yōu)控制問題,同時考慮在預(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 項為拉格朗日被積函數(shù),表達(dá)式為

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

2.2 不等式約束

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

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

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

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

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

2.3 最優(yōu)化條件

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

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

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

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

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

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

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

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

2.4 優(yōu)化求解

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

同時,拆分時需要滿足以下3 個條件:

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

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

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

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

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

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

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

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

化簡可得:

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

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

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

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

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

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

化簡后可得:

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

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

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

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

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

2.5 算法優(yōu)化

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

考慮非線性方程:

為提高該控制算法的穩(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垂直時,‖rk‖F(xiàn)取得最小值,即

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

求得近似解:

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

3 仿真結(jié)果

在仿真模擬中使用為個人電腦的CPU 為英特爾Core i7 -9750H,6 核,時鐘頻率為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,采樣時間選擇為Δτ=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,采樣時間選擇為Δτ=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è)定運行時間為10 s,由圖3(b)可知,針對固定曲率的路徑進(jìn)行跟蹤,3種算法都表現(xiàn)出良好的跟蹤效果,在穩(wěn)定跟蹤后,跟蹤誤差均能控制在0.01 m 范圍內(nèi)。相比之下,本文所述算法的跟蹤結(jié)果在0.5 s 時已經(jīng)跟蹤至目標(biāo)路徑且趨于穩(wěn)定,最優(yōu)性誤差峰值最低,比另外2 種算法更為敏感。從圖3(d)～(g)可知,跟蹤速度與加速度在接近3 s 時開始收斂,達(dá)到期望值,3 種算法表現(xiàn)相當(dāng)。

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

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

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

表1 平均計算時長

表2 平均跟蹤誤差

4 結(jié)論

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

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