基于非線性模型預測控制的移動機器人實時路徑跟蹤

白國星 劉 麗 孟 宇 劉思言 劉 立 羅維東

(1.北京科技大學機械工程學院， 北京 100083; 2.吉林省農業機械研究院， 長春 130021)

0 引言

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

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

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

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

1 NMPC控制器

在基于MPC的路徑跟蹤控制中，運動學模型的應用十分廣泛[6,7-9,12-14,18-21]。在這種模型中，通常假設系統符合非完整約束，車體無側向滑動。由于農用機器人速度較低，在轉向時受離心加速度等側向加速度的影響較小，所以可以通過機器人的運動學模型推導控制器的預測模型。

農用機器人的運動學模型十分成熟，其一般形式為

(1)

式中x、y——機器人轉動中心的橫、縱坐標，m

θ——機器人航向角，rad

ω——機器人航向角速度，rad/s

v——機器人縱向速度，m/s

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

(2)

其中

(3)

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

(4)

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

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

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

(5)

假設預測步數為Np，控制步數為Nc，則在預測時域內機器人的位姿狀態為

(6)

設路徑規劃系統給出的參考路徑點序列為Xref(t+i|t)(i=1,2,…,Np)。則優化目標函數可設計為

(7)

式中Q——誤差懲罰項權重矩陣

R——控制量增量懲罰項權重矩陣

其中控制量增量懲罰項的作用是保證控制器較為平穩地控制機器人跟蹤參考路徑，避免機器人在短時間內過于頻繁地加減速和轉向。

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

(8)

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

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

(9)

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

2 實時性優化方案

由于NMPC控制器每經過一個控制周期輸出一組控制變量，所以求解式(8)的時間如果大于控制周期長度，則控制器的實時性就無法得到保證。因此較為容易得到的實時性優化方案為減少求解式(8)的計算復雜度。

由式(6)可知，機器人未來的狀態，可以通過當前的狀態和未來的控制輸入預測獲得，而對于每一步預測，理論上的最大搜索次數由約束條件決定。

(10)

式中ns——單步預測中的理論最大搜索次數

p——搜索步長

對于Nc步之后的預測，因為其控制變量與第Nc步相同，所以理論上NMPC控制器的預測時域內最大搜索次數為

(11)

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

(1)方案1：減少控制步數方案。設置控制步數Nc1為1，并對與控制步數有關聯的預測步數Np1進行相應調節，其他參數與NMPC控制器一致。

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

(12)

式中Tc——控制時域

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

3 仿真驗證

本文提出的NMPC路徑跟蹤控制器實時性優化方案通過Matlab/Simulink中的Desktop Real-Time平臺進行仿真驗證，硬件中處理器為Intel?CoreTMi5-8500，內存為8 GB。仿真中各控制器的參數如表1所示，這些參數是通過參數調整獲得的各控制器性能較佳時的取值。

表1 機器人及NMPC控制器的參數Tab.1 Parameters of robot and NMPC controller

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

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

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

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

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

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

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

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

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

圖5 仿真解算時間Fig.5 Computation time of simulation

4 實驗驗證

采用圖6所示的國興智能履帶式機器人作為實驗平臺。實驗系統的主控制器為研華ARK-3500型工控機，其處理器型號為Intel Core i5-3610ME，頻率為2.7 GHz。執行控制器采用的是樣機自帶的線控驅動系統，用于實現行駛速度和航向角速度的控制。定位系統采用的是較為可靠的多傳感器聯合定位系統Inertial Labs INS-D，在RTK(Real time kinematic)模式下，該系統的坐標誤差為0.01 m以內，航向誤差為0.03°以內。圖7所示為導航系統的回環檢測結果。回環檢測即人工操作機器人，使其移動一定距離后再返回原點，然后將其初始定位結果與最終定位結果之間的偏差作為定位是否可靠的判斷標準。如圖7所示，移動距離較短時，定位系統的累積誤差較小，基本可以忽略不計。

圖6 農用機器人模型樣機Fig.6 Model prototype of agricultural robots

圖7 定位系統回環檢測結果Fig.7 Loop detection of positioning system

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

圖8 實驗行駛軌跡Fig.8 Trajectory of experiment

由圖8可知，3種經過實時性優化的MPC控制器均可以控制機器人跟蹤參考路徑。圖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顯示了每一控制周期內控制器的解算時間，方案1、方案2和LTV-MPC的最大解算時間分別為0.047 0、0.187 0、0.010 0 s。

整體而言，實驗結果與仿真結果具有一致的規律，采用減少控制步數或者降低控制頻率的方案，可以實現對NMPC控制器的實時性優化。從控制精度來看，減少控制步數的效果相對較好。

圖9 實驗橫向誤差Fig.9 Displacement error of experiment

圖10 實驗航向誤差Fig.10 Heading error of experiment

圖11 實驗解算時間Fig.11 Computation time of experiment

5 結論

(1)針對基于NMPC的機器人路徑跟蹤控制的實時性問題，基于模型預測控制算法提出了兩種實時性優化方案，即減少控制步數方案和降低控制頻率方案。對經過實時性優化的NMPC控制器與未經優化的控制器以及經過預測模型線性化的控制器進行了對比。

(2)仿真與實驗結果表明，未經優化的NMPC控制器在實時性方面表現不佳，仿真中NMPC控制器在控制周期內的最大解算時間大于控制周期，無法滿足實時性需求；采用減少控制步數方案對NMPC控制器進行優化后，仿真和實驗中的最大解算時間均小于控制周期，能夠滿足實時性需求，路徑跟蹤的控制精度較高；采用降低控制頻率方案對NMPC控制器進行優化后，仿真和實驗中的最大解算時間均小于控制周期，能夠滿足實時性需求，但控制系統的誤差稍大于采用減少控制步數方案時的誤差；采用線性化預測模型方案，即將NMPC控制器轉變為LTV-MPC控制器后，控制器實時性大幅度提高，但在跟蹤曲率變化較快的參考路徑時，LTV-MPC控制器的精度較差。

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