基于改進GMRES 算法的水冷壁運動機器人路徑跟蹤模型預測控制①

黃 巍 占紅武 胥 芳

(浙江工業大學特種裝備制造與先進加工技術教育部/浙江省重點實驗室 杭州310023)

0 引言

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

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

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

1 非線性模型

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

圖1 路徑跟蹤模型

圖2 水冷管壁圖

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

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

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

其中,fl與fr分別表示左右履帶電機的輸入驅動轉矩,m與I分別為NWMR 的質量及其慣性矩。在實際工作中,考慮到由于外部因素導致速度突變引起的運行不穩定,利用虛擬控制量表示NWMR 的加速度與角加速度,對動力學系統簡化為

根據式(2)～式(4)擴展系統狀態,可得閉環系統狀態方程為

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

則式(5)中的閉環系統可用如下非線性表達式表示。

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

2 快速非線性模型預測控制方法

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

2.1 問題描述

針對模型建立的NMPC 問題,在不失一般性的情況下,可將其描述為Bolza 形式的最優控制問題,同時考慮在預測范圍內的階段目標函數L以及出現在范圍末端的終端目標函數V。

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

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

其中,Qf為終端狀態權重矩陣。右側第2 項為拉格朗日被積函數,表達式為

上式中Q=diag{q1,…,q5}、R=diag{r1,r2} 均表示權重矩陣,qi、ri為非負常數。在上述性能指標函數中,第1 項表示NWMR 系統模型,第2 項為其邊界條件。

2.2 不等式約束

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

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

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

為避免當虛擬輸入變量為0 時導致的C/GMRES優化更新時的奇異性,需要在代價函數中引入懲罰因子,則拉格朗日函數修改為

為避免懲罰因子過大而對尋找最優解產生影響,故選擇ki為小的正常數。

2.3 最優化條件

通過Karush-Kuhn-Tucker (KKT)最優性條件導出求解動態OCP 的最優性條件,利用廣義哈密頓函數表示:

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

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

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

將FB 函數作為互補條件,與最優化條件組成約束條件為

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

建立狀態和共態向量的代數約束:

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

2.4 優化求解

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

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

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

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

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

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

其中As是正穩態矩陣。對式(33)～式(35)進行合并化簡帶入后,得到關于的線性方程:

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

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

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

化簡可得:

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

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

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

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

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

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

化簡后可得:

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

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

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

根據式(34)可以解得:

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

2.5 算法優化

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

考慮非線性方程:

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

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

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

通過調整yk使得rk與AUkyk垂直時,‖rk‖F取得最小值,即

結合式(59)可得到:

求得近似解:

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

3 仿真結果

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

圓形路徑定義如下:

式中,?為路徑參數,確定了虛擬目標在世界坐標系下的位置,初始狀態和終端狀態分別選取為

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

約束參數中設置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。

伯努利雙紐線路徑定義為

初始狀態與終端狀態分別選取為

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

約束參數中設置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。

其他相關參數為ε=0.01,h=0.001,As=Dh=1/h=1000,m=1.5,R=0.048,I=0.0348,C/GMRES 算法內部參數設置初始控制輸入與乘子的誤差容限rtol=1.0e-8,GMRES 的迭代數kmax=10。

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

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

圖3 圓形路徑跟蹤仿真結果

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

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

表1 平均計算時長

表2 平均跟蹤誤差

4 結論

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

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