白國(guó)星 劉 麗 孟 宇 劉思言 劉 立 羅維東

(1.北京科技大學(xué)機(jī)械工程學(xué)院， 北京 100083; 2.吉林省農(nóng)業(yè)機(jī)械研究院， 長(zhǎng)春 130021)

0 引言

路徑跟蹤是車輛、機(jī)器人等移動(dòng)裝備自主行駛控制的一項(xiàng)核心技術(shù)，其任務(wù)是減小移動(dòng)裝備與參考路徑之間的誤差，從而使移動(dòng)裝備沿參考路徑行駛[1]。路徑跟蹤控制的本質(zhì)是一個(gè)多約束條件下的優(yōu)化問(wèn)題。模型預(yù)測(cè)控制(Model predictive control, MPC)能夠顯式地考慮系統(tǒng)約束條件[2-3]，因此被廣泛應(yīng)用于路徑跟蹤控制。目前，基于MPC的路徑跟蹤控制面臨的較為突出的就是實(shí)時(shí)性問(wèn)題[4]。

在一些路徑跟蹤控制研究[5-9]中，MPC的實(shí)時(shí)性問(wèn)題尚未受到重視。不過(guò)這一問(wèn)題得到了越來(lái)越多的關(guān)注，出現(xiàn)了較多的研究成果。文獻(xiàn)[10]使用自動(dòng)控制和動(dòng)態(tài)優(yōu)化工具包(Toolkit for automatic control and dynamic optimization, ACADO Toolkit)在計(jì)算機(jī)代碼層面對(duì)控制器進(jìn)行了優(yōu)化。文獻(xiàn)[11]則通過(guò)連續(xù)/廣義最小殘差(Continuation/Generalized minimal residual, C/GMRES)算法提高了控制器的求解效率。這兩種優(yōu)化方法對(duì)于提高控制器的實(shí)時(shí)性具有十分積極的意義，但與MPC本身的實(shí)時(shí)性能并不相關(guān)。

從MPC的角度出發(fā)，常見(jiàn)的實(shí)時(shí)性優(yōu)化方案是線性化預(yù)測(cè)模型。文獻(xiàn)[12]將這種方法命名為線性模型預(yù)測(cè)控制(Linear model predictive control, LMPC)，并證明LMPC相比非線性模型預(yù)測(cè)控制(Nonlinear model predictive control, NMPC)具有更好的實(shí)時(shí)性。文獻(xiàn)[13-14]的研究重點(diǎn)并非實(shí)時(shí)性，但是也采用該方案設(shè)計(jì)了路徑跟蹤控制器，并命名為線性時(shí)變模型預(yù)測(cè)控制(Linear time-varying model predictive control, LTV-MPC)。此外，文獻(xiàn)[15-23]的研究重點(diǎn)各不相同，但采用的控制方法都是LTV-MPC。

然而，線性化后的預(yù)測(cè)模型無(wú)法精確預(yù)測(cè)機(jī)器人的位姿狀態(tài)，只能通過(guò)當(dāng)前的誤差進(jìn)行預(yù)測(cè)，因此削弱了控制器對(duì)路徑曲率突變和航向突變的響應(yīng)能力[24-26]。對(duì)于農(nóng)用機(jī)器人的路徑跟蹤控制，參考路徑通常由直線和圓弧組成，其曲率變化很大，線性化預(yù)測(cè)模型的實(shí)時(shí)性優(yōu)化方案并不十分適用。本文根據(jù)NMPC的控制周期、控制步數(shù)和計(jì)算復(fù)雜度之間的關(guān)系，提出兩種實(shí)時(shí)性優(yōu)化方案，并通過(guò)Matlab/Simulink中的Desktop Real Time實(shí)時(shí)性仿真框架以及實(shí)驗(yàn)驗(yàn)證，將這兩種方案與線性化預(yù)測(cè)模型的方案進(jìn)行對(duì)比研究。

1 NMPC控制器

在基于MPC的路徑跟蹤控制中，運(yùn)動(dòng)學(xué)模型的應(yīng)用十分廣泛[6,7-9,12-14,18-21]。在這種模型中，通常假設(shè)系統(tǒng)符合非完整約束，車體無(wú)側(cè)向滑動(dòng)。由于農(nóng)用機(jī)器人速度較低，在轉(zhuǎn)向時(shí)受離心加速度等側(cè)向加速度的影響較小，所以可以通過(guò)機(jī)器人的運(yùn)動(dòng)學(xué)模型推導(dǎo)控制器的預(yù)測(cè)模型。

農(nóng)用機(jī)器人的運(yùn)動(dòng)學(xué)模型十分成熟，其一般形式為

(1)

式中x、y——機(jī)器人轉(zhuǎn)動(dòng)中心的橫、縱坐標(biāo)，m

θ——機(jī)器人航向角，rad

ω——機(jī)器人航向角速度，rad/s

v——機(jī)器人縱向速度，m/s

為了便于表達(dá)，式(1)抽象為

(2)

其中

(3)

參照文獻(xiàn)[12-13]，采用歐拉法(Euler method)將式(3)離散化

(4)

式中T——控制器的控制周期

將式(2)代入式(4)可得

X(t+1|t)=X(t|t)+Tf(X(t|t),U(t|t))

(5)

假設(shè)預(yù)測(cè)步數(shù)為Np，控制步數(shù)為Nc，則在預(yù)測(cè)時(shí)域內(nèi)機(jī)器人的位姿狀態(tài)為

(6)

設(shè)路徑規(guī)劃系統(tǒng)給出的參考路徑點(diǎn)序列為Xref(t+i|t)(i=1,2,…,Np)。則優(yōu)化目標(biāo)函數(shù)可設(shè)計(jì)為

(7)

式中Q——誤差懲罰項(xiàng)權(quán)重矩陣

R——控制量增量懲罰項(xiàng)權(quán)重矩陣

其中控制量增量懲罰項(xiàng)的作用是保證控制器較為平穩(wěn)地控制機(jī)器人跟蹤參考路徑，避免機(jī)器人在短時(shí)間內(nèi)過(guò)于頻繁地加減速和轉(zhuǎn)向。

路徑跟蹤控制即等效于在約束條件下求解

(8)

求解后，即可獲得路徑跟蹤控制的輸入變量序列

U*=(U(1),U(2),…,U(Nc))T

(9)

其中第1個(gè)元素U(1)即控制器在下一時(shí)刻輸出的控制變量。

2 實(shí)時(shí)性優(yōu)化方案

由于NMPC控制器每經(jīng)過(guò)一個(gè)控制周期輸出一組控制變量，所以求解式(8)的時(shí)間如果大于控制周期長(zhǎng)度，則控制器的實(shí)時(shí)性就無(wú)法得到保證。因此較為容易得到的實(shí)時(shí)性優(yōu)化方案為減少求解式(8)的計(jì)算復(fù)雜度。

由式(6)可知，機(jī)器人未來(lái)的狀態(tài)，可以通過(guò)當(dāng)前的狀態(tài)和未來(lái)的控制輸入預(yù)測(cè)獲得，而對(duì)于每一步預(yù)測(cè)，理論上的最大搜索次數(shù)由約束條件決定。

(10)

式中ns——單步預(yù)測(cè)中的理論最大搜索次數(shù)

p——搜索步長(zhǎng)

對(duì)于Nc步之后的預(yù)測(cè)，因?yàn)槠淇刂谱兞颗c第Nc步相同，所以理論上NMPC控制器的預(yù)測(cè)時(shí)域內(nèi)最大搜索次數(shù)為

(11)

由式(11)可知，減少控制步數(shù)可以大幅減少預(yù)測(cè)時(shí)域內(nèi)理論最大搜索次數(shù)，因此可以提出如下實(shí)時(shí)性優(yōu)化方案：

(1)方案1：減少控制步數(shù)方案。設(shè)置控制步數(shù)Nc1為1，并對(duì)與控制步數(shù)有關(guān)聯(lián)的預(yù)測(cè)步數(shù)Np1進(jìn)行相應(yīng)調(diào)節(jié)，其他參數(shù)與NMPC控制器一致。

(2)方案2：降低控制頻率方案。由于

(12)

式中Tc——控制時(shí)域

所以如果控制時(shí)域不變，增大控制周期，降低控制頻率，盡管會(huì)增大單步預(yù)測(cè)中的理論最大搜索次數(shù)，但是可以減少控制步數(shù)，從而減少預(yù)測(cè)時(shí)域內(nèi)理論最大搜索次數(shù)。在方案2中，設(shè)置控制周期T2=5T，預(yù)測(cè)步數(shù)Np2=Np/5，控制步數(shù)Nc2=Nc/5，其他參數(shù)與NMPC控制器一致。

3 仿真驗(yàn)證

本文提出的NMPC路徑跟蹤控制器實(shí)時(shí)性優(yōu)化方案通過(guò)Matlab/Simulink中的Desktop Real-Time平臺(tái)進(jìn)行仿真驗(yàn)證，硬件中處理器為Intel?CoreTMi5-8500，內(nèi)存為8 GB。仿真中各控制器的參數(shù)如表1所示，這些參數(shù)是通過(guò)參數(shù)調(diào)整獲得的各控制器性能較佳時(shí)的取值。

表1 機(jī)器人及NMPC控制器的參數(shù)Tab.1 Parameters of robot and NMPC controller

在仿真中，機(jī)器人的行駛速度設(shè)置為3 m/s，參考路徑由直線和圓弧路段組成，其中圓弧路段的半徑為10 m。

由圖1可知，在較大的尺度范圍內(nèi)，所有基于MPC的路徑跟蹤控制器都可以實(shí)現(xiàn)路徑跟蹤，通過(guò)圖2可以看出，NMPC控制下機(jī)器人的行駛軌跡偏離參考路徑最少，本文提出的實(shí)時(shí)性優(yōu)化后的控制器在精度方面稍遜色于NMPC控制器，而常見(jiàn)的LTV-MPC控制器則在面臨參考路徑曲率突變時(shí)出現(xiàn)了較大的誤差。

圖1 仿真行駛軌跡Fig.1 Trajectory of simulation

圖2 仿真行駛軌跡的局部放大圖Fig.2 Partial enlarged view of trajectory of simulation

圖3中顯示的橫向誤差即機(jī)器人距離參考路徑的最短直線距離。NMPC、方案1、方案2和LTV-MPC的最大橫向誤差分別為0.081 1、0.094 7、0.107 4、1.167 4 m。由此可知，相比將預(yù)測(cè)模型線性化，在NMPC控制器的基礎(chǔ)上進(jìn)行實(shí)時(shí)性優(yōu)化，可以較大程度地減少控制精度的下降。

圖3 橫向誤差仿真結(jié)果Fig.3 Displacement error of simulation

圖4中顯示的航向誤差即機(jī)器人行駛方向與參考路徑切線方向的偏差。NMPC、方案1、方案2和LTV-MPC的最大航向誤差分別為0.054 6、0.060 8、0.058 2、0.210 6 rad。在NMPC控制器的基礎(chǔ)上進(jìn)行實(shí)時(shí)性優(yōu)化，也可以較大程度地保證機(jī)器人的航向與參考路徑一致。

圖4 航向誤差仿真結(jié)果Fig.4 Heading error of simulation

圖5顯示的是每一控制周期內(nèi)控制器的解算時(shí)間，由圖5a可知，當(dāng)路徑的航向發(fā)生較大程度的變化時(shí)，NMPC控制器的解算時(shí)間顯著增大，其最大值為0.087 0 s，是控制周期的1.74倍，而且解算時(shí)間超過(guò)控制周期的時(shí)段是連續(xù)的，由此可知此時(shí)NMPC無(wú)法滿足路徑跟蹤控制的實(shí)時(shí)性要求。由圖5b、5d可知，本文提出的優(yōu)化方案1和LTV-MPC控制器的解算時(shí)間較短，其最大值分別為0.036 0、0.010 0 s，均遠(yuǎn)小于控制周期。本文提出的優(yōu)化方案2則沒(méi)有大幅度減少解算時(shí)間，其最大值為0.045 0 s，但是由于控制頻率降低，在該仿真系統(tǒng)中控制周期為0.250 0 s，所以解算時(shí)間遠(yuǎn)小于控制周期，可以滿足實(shí)時(shí)性需求。

圖5 仿真解算時(shí)間Fig.5 Computation time of simulation

4 實(shí)驗(yàn)驗(yàn)證

采用圖6所示的國(guó)興智能履帶式機(jī)器人作為實(shí)驗(yàn)平臺(tái)。實(shí)驗(yàn)系統(tǒng)的主控制器為研華ARK-3500型工控機(jī)，其處理器型號(hào)為Intel Core i5-3610ME，頻率為2.7 GHz。執(zhí)行控制器采用的是樣機(jī)自帶的線控驅(qū)動(dòng)系統(tǒng)，用于實(shí)現(xiàn)行駛速度和航向角速度的控制。定位系統(tǒng)采用的是較為可靠的多傳感器聯(lián)合定位系統(tǒng)Inertial Labs INS-D，在RTK(Real time kinematic)模式下，該系統(tǒng)的坐標(biāo)誤差為0.01 m以內(nèi)，航向誤差為0.03°以內(nèi)。圖7所示為導(dǎo)航系統(tǒng)的回環(huán)檢測(cè)結(jié)果?；丨h(huán)檢測(cè)即人工操作機(jī)器人，使其移動(dòng)一定距離后再返回原點(diǎn)，然后將其初始定位結(jié)果與最終定位結(jié)果之間的偏差作為定位是否可靠的判斷標(biāo)準(zhǔn)。如圖7所示，移動(dòng)距離較短時(shí)，定位系統(tǒng)的累積誤差較小，基本可以忽略不計(jì)。

圖6 農(nóng)用機(jī)器人模型樣機(jī)Fig.6 Model prototype of agricultural robots

圖7 定位系統(tǒng)回環(huán)檢測(cè)結(jié)果Fig.7 Loop detection of positioning system

由于未經(jīng)優(yōu)化的NMPC控制器無(wú)法在控制周期內(nèi)完成優(yōu)化求解，所以不能用于實(shí)驗(yàn)驗(yàn)證。實(shí)驗(yàn)中對(duì)比了方案1、方案2和LTV-MPC，各控制器的參數(shù)與仿真中控制器的參數(shù)相同。由于實(shí)驗(yàn)場(chǎng)地面積有限，參考路徑的彎道半徑設(shè)置為5 m。為了在此情況下保證實(shí)驗(yàn)的安全性，機(jī)器人的行駛速度設(shè)置為1 m/s。

圖8 實(shí)驗(yàn)行駛軌跡Fig.8 Trajectory of experiment

由圖8可知，3種經(jīng)過(guò)實(shí)時(shí)性優(yōu)化的MPC控制器均可以控制機(jī)器人跟蹤參考路徑。圖9顯示了方案1、方案2和LTV-MPC的最大橫向誤差分別為0.019 9、0.075 6、0.238 1 m。圖10顯示了方案1、方案2和LTV-MPC的最大航向誤差分別為0.032 3、0.071 3、0.118 9 rad。

圖11顯示了每一控制周期內(nèi)控制器的解算時(shí)間，方案1、方案2和LTV-MPC的最大解算時(shí)間分別為0.047 0、0.187 0、0.010 0 s。

整體而言，實(shí)驗(yàn)結(jié)果與仿真結(jié)果具有一致的規(guī)律，采用減少控制步數(shù)或者降低控制頻率的方案，可以實(shí)現(xiàn)對(duì)NMPC控制器的實(shí)時(shí)性優(yōu)化。從控制精度來(lái)看，減少控制步數(shù)的效果相對(duì)較好。

圖9 實(shí)驗(yàn)橫向誤差Fig.9 Displacement error of experiment

圖10 實(shí)驗(yàn)航向誤差Fig.10 Heading error of experiment

圖11 實(shí)驗(yàn)解算時(shí)間Fig.11 Computation time of experiment

5 結(jié)論

(1)針對(duì)基于NMPC的機(jī)器人路徑跟蹤控制的實(shí)時(shí)性問(wèn)題，基于模型預(yù)測(cè)控制算法提出了兩種實(shí)時(shí)性優(yōu)化方案，即減少控制步數(shù)方案和降低控制頻率方案。對(duì)經(jīng)過(guò)實(shí)時(shí)性優(yōu)化的NMPC控制器與未經(jīng)優(yōu)化的控制器以及經(jīng)過(guò)預(yù)測(cè)模型線性化的控制器進(jìn)行了對(duì)比。

(2)仿真與實(shí)驗(yàn)結(jié)果表明，未經(jīng)優(yōu)化的NMPC控制器在實(shí)時(shí)性方面表現(xiàn)不佳，仿真中NMPC控制器在控制周期內(nèi)的最大解算時(shí)間大于控制周期，無(wú)法滿足實(shí)時(shí)性需求；采用減少控制步數(shù)方案對(duì)NMPC控制器進(jìn)行優(yōu)化后，仿真和實(shí)驗(yàn)中的最大解算時(shí)間均小于控制周期，能夠滿足實(shí)時(shí)性需求，路徑跟蹤的控制精度較高；采用降低控制頻率方案對(duì)NMPC控制器進(jìn)行優(yōu)化后，仿真和實(shí)驗(yàn)中的最大解算時(shí)間均小于控制周期，能夠滿足實(shí)時(shí)性需求，但控制系統(tǒng)的誤差稍大于采用減少控制步數(shù)方案時(shí)的誤差；采用線性化預(yù)測(cè)模型方案，即將NMPC控制器轉(zhuǎn)變?yōu)長(zhǎng)TV-MPC控制器后，控制器實(shí)時(shí)性大幅度提高，但在跟蹤曲率變化較快的參考路徑時(shí)，LTV-MPC控制器的精度較差。

(3)考慮到農(nóng)用機(jī)器人的工作場(chǎng)景，控制器除了要保證實(shí)時(shí)性，還需保證機(jī)器人的靈活性，即機(jī)器人跟蹤曲率變化較快的參考路徑的能力，因此，減少控制步數(shù)是較佳的實(shí)時(shí)性優(yōu)化方案。在實(shí)際工程應(yīng)用中，也可以將減少控制步數(shù)方案與降低控制頻率方案組合使用，進(jìn)一步保證NMPC路徑跟蹤控制器的實(shí)時(shí)性。