基于積分模型預(yù)測控制的全向移動機(jī)器人軌跡跟蹤研究①

顏佳晴 魯聰達(dá) 蔡穎杰 彭 翔 潘 婷 周圣云

(*浙江工業(yè)大學(xué)機(jī)械工程學(xué)院 杭州310023)

(**深圳怡豐機(jī)器人科技有限公司 深圳518116)

0 引言

近年來移動機(jī)器人的應(yīng)用越來越廣泛。從港口的集裝箱裝卸、電子商務(wù)包裹分揀、智能化倉儲到智能停車,移動機(jī)器人開始進(jìn)入人們的生活并改變了生活方式。因此,移動機(jī)器人越來越受到研究人員的關(guān)注。目前,移動機(jī)器人的研究內(nèi)容主要包括模型、控制、導(dǎo)航定位、避障、路徑規(guī)劃、點穩(wěn)定等。[1-5]

移動機(jī)器人的控制是指通過輸出控制率來驅(qū)動移動機(jī)器人按照預(yù)設(shè)的路徑行駛[6]。模型預(yù)測控制(model predictive control,MPC)作為一種現(xiàn)代智能控制,是移動機(jī)器人控制算法的研究熱點。基于運動學(xué)模型的預(yù)測控制策略已經(jīng)應(yīng)用于移動機(jī)器人和自動駕駛汽車的路徑跟蹤[6-8]。廣義預(yù)測控制被用來解決路徑跟隨問題[9]。在這些研究中,預(yù)測控制僅輸出角速度,而線速度則是通過特殊的速度發(fā)生器產(chǎn)生。隨著研究的深入,當(dāng)今的模型預(yù)測控制不僅是簡單的控制速度和角速度,越來越多的研究集中在如何優(yōu)化MPC 控制器上。例如,文獻(xiàn)[10]通過將協(xié)調(diào)器添加到MPC 控制器,可以通過線性返回移動機(jī)器人中的跟蹤誤差來選擇合適的權(quán)重參數(shù)。文獻(xiàn)[11]考慮到摩擦力對MPC 控制器的影響,設(shè)計了一種模型預(yù)測控制器進(jìn)行摩擦補償,以達(dá)到較好的控制效果。

在移動機(jī)器人領(lǐng)域,解決摩擦補償是一個重要的問題。在摩擦補償?shù)难芯恐?提出了許多控制方法,提出了模型技術(shù)控制中的各種摩擦模型。這些方法可以分為基于模型的技術(shù)[12-14]和基于非模型的技術(shù)。在基于模型的補償方案中,使用靜態(tài)或動態(tài)摩擦模型來緩解由于摩擦效應(yīng)而導(dǎo)致的性能下降。在基于非模型的補償方案中,在控制設(shè)計中不使用摩擦模型。摩擦補償是通過適當(dāng)選擇控制增益參數(shù)或使用非基于模型的觀測器來實現(xiàn)的[15]。但是,基于摩擦模型的模型預(yù)測控制的計算量十分巨大,不能滿足實際應(yīng)用中可靠、穩(wěn)定、快速響應(yīng)的要求。非模型的補償方案中解決摩擦的非線性影響,是基于改進(jìn)PID 控制器。除了需要精確的模型外,這些技術(shù)還具有補償摩擦以外干擾的優(yōu)勢[11]。但是移動機(jī)器人在移動中除了摩擦以外,還有其他外部干擾所形成的穩(wěn)態(tài)誤差影響控制器的性能。為了消除模型預(yù)測控制中的穩(wěn)態(tài)誤差并減少處理器的計算量,并在移動機(jī)器人的跟蹤控制中獲得令人滿意的性能,本文設(shè)計了一種具有積分作用的模型預(yù)測控制器,稱為積分模型預(yù)測控制(integral model predictive control,IMPC)。本文的主要工作概括如下。

(1)設(shè)計具有積分作用的模型預(yù)測控制器。通過在傳統(tǒng)模型預(yù)測控制器中添加積分作用,消除MPC 中的穩(wěn)態(tài)誤差,并獲得更好的軌跡跟蹤效果。

(2)在分析四差速全向移動機(jī)器人運動特性基礎(chǔ)上,對差速驅(qū)動單元進(jìn)行運動學(xué)分析,建立了完整的四差速全向移動機(jī)器人的運動學(xué)模型。

(3)將設(shè)計的IMPC 控制器應(yīng)用于四差速全向移動機(jī)器人,與傳統(tǒng)MPC 控制器進(jìn)行對比測試。

1 全向移動機(jī)器人運動學(xué)分析

1.1 差動驅(qū)動模塊的運動學(xué)分析

全向移動機(jī)器人的測試平臺如圖1 所示。與傳統(tǒng)全向移動機(jī)器人相比,該測試平臺使用差速驅(qū)動作為驅(qū)動形式,通過兩個輪子的速度差實現(xiàn)驅(qū)動模塊的直行或轉(zhuǎn)彎。全向移動機(jī)器人通過4 對差速驅(qū)動單元的共同協(xié)作實現(xiàn)移動機(jī)器人的全向運行,因此稱為四差速全向移動機(jī)器人。

圖1 全向移動機(jī)器人的測試平臺

差速驅(qū)動模塊在ti和ti+1兩個時刻的位姿如圖2所示。差速驅(qū)動單元左右兩個驅(qū)動輪之間的距離為l,左輪速度為vl,右輪速度為vr,兩個相鄰時刻所轉(zhuǎn)過的角度為θ,d為右輪比左輪多行駛的距離。

圖2 差速模塊相鄰兩個時刻的位姿

通過幾何分析得到差速驅(qū)動模塊左右輪的速度與其中心點線速度v和角速度ω之間的關(guān)系:

式(1)表示通過差速驅(qū)動單元左右驅(qū)動輪的速度,可以計算得到差速驅(qū)動單元參考點的線速度和角速度。式(2)表示通過差速驅(qū)動單元參考點的線速度和角速度,可以計算得出驅(qū)動單元的左右輪速。

1.2 全向移動機(jī)器人整體分析

圖3 為四差速全向移動機(jī)器人的整體結(jié)構(gòu)和坐標(biāo)系。移動機(jī)器人4 組差速驅(qū)動單元均勻分在車體四周,每組驅(qū)動單元有2 個驅(qū)動輪。X-Y為全局坐標(biāo)系,XR-YR為局部坐標(biāo)系。設(shè)全局坐標(biāo)系與局部坐標(biāo)系之間的夾角為φ。對差速驅(qū)動進(jìn)行運動學(xué)分析,得出單舵輪驅(qū)動與差速驅(qū)動可以通過式(1)和式(2)實現(xiàn)vl、vr與v、ω之間的相互轉(zhuǎn)換,因此差速驅(qū)動等效為舵輪驅(qū)動。

圖3 四差速全向移動機(jī)器人的整體結(jié)構(gòu)和坐標(biāo)系

假設(shè)四差速全向移動機(jī)器人2 個前部驅(qū)動模塊的速度與轉(zhuǎn)角相同,2 個后部驅(qū)動模塊的速度與轉(zhuǎn)角也相同。根據(jù)假設(shè),移動機(jī)器人前部2 個差速驅(qū)動單元用一個單舵輪替換,用A表示;同理,移動機(jī)器人后部2 個差速驅(qū)動單元用一個單舵輪替換,用B表示;C表示移動機(jī)器人的運動質(zhì)心(參考點),A、B兩點到C點的距離分別由lf和lr表示。φ表示移動機(jī)器人車頭方向,V表示質(zhì)心的速度,速度V與車輛軸線所形成的角度用β表示。簡化后的四差速全向移動機(jī)器人模型如圖4 所示,此模型稱為單車模型。

圖4 單車模型

通過對單車模型的幾何關(guān)系分析,得到:

因為移動的機(jī)器人的角速度表示為θ,由此可得出移動機(jī)器人角速度為

聯(lián)立式(3)和式(4),可以得出:

因此,單車模型的運動學(xué)方程表示為

式(6)為四差速全向移動機(jī)器人的運動學(xué)模型。3 個輸入量分別是δf、δr和V。式(7)為β角的計算公式,移動機(jī)器人的航向角φ可由定位模塊提供。

2 積分模型預(yù)測控制器設(shè)計

積分模型預(yù)測控制器的設(shè)計主要分為2 部分。第1 部分是MPC 控制器根據(jù)參考位姿pr=[xr,yr,θr]T與p=[x,y,θ]T移動機(jī)器人當(dāng)前位姿之間的誤差來輸出的線速度和角速度。第2 部分是積分作用設(shè)計,用于補償移動機(jī)器人運行中摩擦等穩(wěn)態(tài)誤差。IMPC 的結(jié)構(gòu)如圖5 所示,外部虛線框為IMPC,內(nèi)部虛線框為傳統(tǒng)MPC。

圖5 積分模型預(yù)測控制器結(jié)構(gòu)圖

2.1 模型預(yù)測控制器設(shè)計

移動機(jī)器人運動學(xué)方程的一般式可表示為

其上面的每一點都滿足上述的運動學(xué)方程,用r表示參考量,可表示為

其中,Xr=[xr,yr,φr]T,ur=[vr,ωr,βr]T。

在任意點(Xr,ur) 處泰勒展開,只保留一階項,忽略高階項,得到:

將式(10)減去式(9),得到:

式(11)稱為移動機(jī)器人的線性誤差模型,對式(11)進(jìn)行離散化處理,得到:

采用式(13)作為目標(biāo)函數(shù)。

式中,Np為預(yù)測時域,Nc為控制時域。

為了滿足式(13)目標(biāo)函數(shù)的形式,需將式(12)做一定的轉(zhuǎn)換:

得到一個新的狀態(tài)空間表達(dá)式:

經(jīng)過推導(dǎo),可以得到系統(tǒng)的預(yù)測輸出表達(dá)式:

將式(16)帶入目標(biāo)函數(shù)式(13),可得到完整形式的目標(biāo)函數(shù)表達(dá)式。

控制量的表達(dá)式表示為

控制增量的表達(dá)式表示為

通過對控制量增量的約束代替對控制量的約束,因此在設(shè)計約束條件時也應(yīng)針對控制增量進(jìn)行設(shè)計,以滿足目標(biāo)函數(shù)的要求。因此,需要對式(18)進(jìn)行轉(zhuǎn)換,求得相應(yīng)的轉(zhuǎn)換矩陣。

控制量與控制增量之間關(guān)系為

其中,1Nc為行數(shù)為Nc的列向量,Im為維度為m的單位矩陣,?為克羅內(nèi)克積,u(k -1) 為上一時刻的控制量。

結(jié)合式(19)、式(20)和式(21),式(17)可改寫為

其中,Umax和Umin分別為控制時域內(nèi)的控制量最大值與最小值。

將控制量增量式目標(biāo)函數(shù)轉(zhuǎn)化為二次型形式,結(jié)合約束條件,轉(zhuǎn)化為式(22)的優(yōu)化問題。

對式(23)進(jìn)行求解,可得到一系列控制量增量,如式(24)所示。

將式(24)中的第1 個元素作為控制增量輸入系統(tǒng),可得到:

進(jìn)入下一個控制周期后,不斷重復(fù)以上過程,實現(xiàn)四差速全向移動機(jī)器人的軌跡跟蹤控制。

2.2 積分作用設(shè)計

實際應(yīng)用中,由于移動機(jī)器人存在運行中的摩擦等外部因素影響,整個系統(tǒng)存在穩(wěn)態(tài)誤差。因此,通過對累積誤差增加積分作用,即補償移動機(jī)器人的摩擦和其他外部影響,使移動機(jī)器人獲得更好的響應(yīng)性和魯棒性。

積分作用可累積歷史誤差,但傳統(tǒng)的積分控制在啟動時會在很短的時間內(nèi)對系統(tǒng)輸出造成較大的偏差,很容易引起積分累積,導(dǎo)致控制量超過臨界值。該值對應(yīng)于執(zhí)行器允許的最大動作范圍,這會導(dǎo)致較大的過沖甚至震蕩。使用改進(jìn)的積分器校正移動機(jī)器人的航向角誤差和位置誤差,最后將角速度輸入到移動機(jī)器人。系統(tǒng)啟動時,將使用模型預(yù)測控制來使系統(tǒng)穩(wěn)定。當(dāng)控制量接近給定值時,即當(dāng)移動機(jī)器人的當(dāng)前點接近目標(biāo)點時,加入積分作用消除穩(wěn)態(tài)誤差,提高控制系統(tǒng)精度和魯棒性。

積分作用表示為

移動機(jī)器人的誤差由航向誤差和位置誤差組成,得到:

本文通過預(yù)測模型、滾動優(yōu)化和反饋矯正3 個步驟計算,得出了傳統(tǒng)模型預(yù)測控制器的控制量uM(t)。

式(28)表示當(dāng)前控制量等于上一時刻的控制量加上控制量的增量。在加入積分作用后,得到:

本文通過積分作用校正移動機(jī)器人的航向角誤差和位置誤差,系統(tǒng)啟動時,將使用模型預(yù)測控制來使系統(tǒng)穩(wěn)定,當(dāng)控制量接近給定值時,即當(dāng)移動機(jī)器人的當(dāng)前點接近目標(biāo)點時,采用積分控制消除穩(wěn)態(tài)誤差,根據(jù)所設(shè)定的積分器的作用時間,將式(29)進(jìn)行進(jìn)一步優(yōu)化。

式中,eref表示誤差臨界值。至此,式(30)為本文所設(shè)計的積分模型預(yù)測控制器。

3 實驗

3.1 積分模型預(yù)測控制器實驗設(shè)計和過程

圖6 是進(jìn)行軌跡跟蹤實驗測試的場地。測試場地長約35.5 m,寬約16 m,地面平整,無較大的起伏和縫隙,摩擦系數(shù)較高。在場地柱子周邊和沿墻壁一側(cè)都安裝有激光反射柱,為激光導(dǎo)航提供定位信息。

圖6 測試場地

圖7 是通過激光定位模塊所繪制出的測試場地激光特征圖,定位模塊可以將環(huán)境中的絕大部分信息掃描進(jìn)地圖,例如反光板、墻面、柱子等。在移動機(jī)器人進(jìn)行定位時,定位模塊可以根據(jù)當(dāng)前位置信息與所掃描的特征地圖進(jìn)行匹配,進(jìn)而確定移動機(jī)器人的當(dāng)前位置,提供給移動機(jī)器人準(zhǔn)備的位置信息。

圖7 測試場地特征圖

實驗前確定全向移動機(jī)器人在環(huán)境中的當(dāng)前位置,找到與當(dāng)前位置最近的參考點并初始化。通過調(diào)試個人電腦可以給移動機(jī)器人下發(fā)本地任務(wù),移動機(jī)器人根據(jù)任務(wù)信息搜索最近路徑并開始移動,在移動過程中會不斷記錄當(dāng)前位置與理論位置之間的位置誤差和航向角誤差,輸出外部文本文件。積分模型預(yù)測控制器則會不斷優(yōu)化輸出,不斷減小機(jī)器人行進(jìn)中的誤差。

將設(shè)計的積分模型控制器應(yīng)用于四差速全向移動機(jī)器人平臺進(jìn)行軌跡跟蹤測試,驗證四差速移動機(jī)器人平臺的可行性,也驗證積分模型預(yù)測控制在實際移動機(jī)器人運動軌跡跟蹤控制中的有效性。在本文測試實驗中設(shè)計3 種不同的軌跡進(jìn)行測試,分別是直線、圓角矩形和“8”字形,綜合分析移動機(jī)器人模型和模型預(yù)測控制算法的性能,圖8 所示為移動機(jī)器人正進(jìn)行圓角矩形軌跡跟蹤測試。

圖8 移動機(jī)器人圓角矩形測試

3.2 積分模型預(yù)測控制實驗數(shù)據(jù)分析

3.2.1 直線軌跡

直線軌跡跟蹤是最基本的軌跡跟蹤問題之一。在移動機(jī)器人的移動過程中,能夠以最快的速度運行,而直線運行時發(fā)生的抖動也最容易觀察到。因此,IMPC 控制的穩(wěn)定性和魯棒性通過直線路徑驗證。

在直線軌跡測試中,移動機(jī)器人以1.2 m/s 的固定速度自主行進(jìn)了14.5 m 長的直線路徑。圖9為兩種算法在直線測試中的跟蹤結(jié)果,實線為參考軌跡,虛線為IMPC 跟蹤軌跡,點線為MPC 跟蹤軌跡,從圖9 中觀察到兩種算法都能順利按照預(yù)設(shè)軌跡行進(jìn)。通過對直線跟蹤結(jié)果進(jìn)行局部放大,觀察到兩種算法的跟蹤效果,IMPC 跟蹤軌跡與MPC 跟蹤軌跡相比,更加靠近參考軌跡,即誤差更小。圖10和圖11 顯示了IMPC 和MPC 在直線路徑上的橫向誤差和航向誤差的比較,其中點劃線為MPC 誤差,實線為IMPC 誤差。從圖10 和圖11 中明顯觀察到,IMPC 算法能夠大大減小移動機(jī)器人行駛中的橫向誤差和航向誤差。在直線運行時,傳統(tǒng)的MPC控制器的橫向誤差穩(wěn)定在-10 mm,航向誤差穩(wěn)定在0.7°。在加入積分作用后,IMPC可以大幅減小系統(tǒng)的穩(wěn)態(tài)誤差,橫向誤差在0 mm 附近波動,航向誤差在0 °附近波動。

圖9 直線測試跟蹤結(jié)果

圖10 直線測試橫向誤差

圖11 直線測試航向誤差

3.2.2 圓角矩形

在圓角矩形測試中,移動機(jī)器人以1 m/s 的平均速度自動行進(jìn)36 m 長的圓角矩形路徑。圖12 為兩種算法在相同路徑下的跟蹤結(jié)果,從圖12 中觀察到兩種算法都能順利按照預(yù)設(shè)軌跡行進(jìn)。通過對圓角矩形左上部分進(jìn)行局部放大,觀察到兩種算法的跟蹤效果,其中實線為參考軌跡,虛線為IMPC 跟蹤軌跡,點線為MPC 跟蹤軌跡,IMPC 跟蹤軌跡與MPC跟蹤軌跡相比,更加靠近參考軌跡。圖13 和圖14顯示了IMPC 和MPC 在圓角矩形路徑上的橫向誤差和航向誤差的比較,其中點劃線為MPC 誤差,實線為IMPC 誤差。很明顯,當(dāng)運行一個圓角矩形時,IMPC 的橫向誤差穩(wěn)定在5 mm 左右,航向誤差穩(wěn)定在0.2 °左右,因此IMPC 具有更好的跟蹤性能。

圖12 圓角矩形測試跟蹤結(jié)果

圖13 圓角矩形測試橫向誤差

圖14 圓角矩形測試航向誤差

3.2.3 8 字曲線

在8 字形曲線測試中,移動機(jī)器人以0.8 m/s的速度自動行駛28 m 長的8 字形路徑。圖15 顯示了8 型路徑實驗的IMPC 和MPC 控制器的路徑跟蹤結(jié)果,其中圖中實線為參考軌跡,虛線為IMPC 跟蹤軌跡,點線為MPC 跟蹤軌跡。通過對跟蹤結(jié)果的局部放大,觀察到IMPC 算法的實際路徑更接近參考路徑。圖16 和圖17 顯示了IMPC 和MPC 在圓角矩形路徑上的橫向誤差和航向誤差的比較,其中點劃線為MPC 誤差,實線為IMPC 誤差。通過圖16 和圖17 觀察到,IMPC 控制器可以顯著減少運動過程中移動機(jī)器人的航向誤差和橫向誤差,并且誤差波動較小、駕駛更加穩(wěn)定。加上積分作用后,航向誤差的最大值為0.5 °,最小誤差為0 °。橫向誤差的最大值為-4.7 mm,最小誤差為-1.1 mm。平均誤差為0.2 °和-3.2 mm。

圖15 8 字形測試跟蹤結(jié)果

圖16 8 字形測試橫向誤差

圖17 8 字形測試航向誤差

4 結(jié)論

本文提出了一種積分模型預(yù)測控制方法,并以四差速全向移動機(jī)器人作為研究對象,對移動機(jī)器人的軌跡跟蹤控制進(jìn)行了研究分析,利用機(jī)器人學(xué)、運動學(xué)、控制理論和計算機(jī)學(xué)等理論知識,結(jié)合大實車實驗等綜合性研究,設(shè)計出了跟蹤性能強(qiáng)、魯棒性好的軌跡跟蹤系統(tǒng)。首先,建立運動學(xué)模型并線性化非線性移動機(jī)器人模型;其次,設(shè)計積分模型預(yù)測控制,包括MPC 設(shè)計和積分作用設(shè)計;最后,測試了直線、圓角矩形和8 字形3 個不同軌跡的實驗。實驗結(jié)果表明,與傳統(tǒng)MPC 相比,IMPC 的橫向誤差和航向誤差較小,實際路徑更接近參考路徑,具有更好的跟蹤性和穩(wěn)定性。