基于模型預(yù)測控制的移動機器人戶外導(dǎo)航方法

郭明陽， 劉 爽

（ 華東理工大學(xué)機械與動力工程學(xué)院，上海 200237）

戶外巡檢、戶外保潔、智能農(nóng)業(yè)等應(yīng)用的廣泛興起使得移動機器人的使用場景由室內(nèi)轉(zhuǎn)向戶外。導(dǎo)航技術(shù)是自主機器人研究的核心，目前廣泛應(yīng)用于移動機器人的室內(nèi)導(dǎo)航系統(tǒng)為機器人操作系統(tǒng)（Robot Operating System，ROS）開源框架：Gmapping和Navigation。Gmapping 是基于粒子濾波算法的激光同步定位與建圖（Simultaneous Localization and Mapping，SLAM）方案，能有效利用車輪里程計信息，根據(jù)激光設(shè)備的輸入和姿態(tài)數(shù)據(jù)建立一個基于網(wǎng)格的2D 地圖。此方法在構(gòu)建小場景地圖時所需的計算量小且精度較高，但是隨著場景增大所需的粒子增加，內(nèi)存和計算量都會隨之增加[1]；此外Gmapping沒有回環(huán)檢測，在回環(huán)閉合時會造成地圖錯位[2]。Navigation 是ROS 的二維導(dǎo)航功能包，導(dǎo)入地圖后根據(jù)機器人當(dāng)前位置和目標(biāo)位置，通過Dijkstra 算法進行全局路徑規(guī)劃，行進過程中使用動態(tài)窗口法計算機器人每個周期內(nèi)應(yīng)該行駛的線速度和角速度，但是動態(tài)窗口法在機器人穿越稠密障礙物時易出現(xiàn)從密集障礙物區(qū)域外繞行、軌跡不平滑的問題[3]。

目前在戶外導(dǎo)航方法的研究中，常用方法有使用傳感器捕獲道路信息、在道路上預(yù)先布置路徑線或者使用深度學(xué)習(xí)識別道路參數(shù)，然后再進行相應(yīng)的控制以實現(xiàn)自主導(dǎo)航。Asami 等[4]通過攝像頭采集機器人周圍道路圖像，然后使用Canny 算法提取路段信息用于導(dǎo)航。該方法易受光照強度影響，而且當(dāng)路面存在條狀噴涂線或裂痕時將直接影響路段信息提取效果。Zuo 等[5]使用攝像頭識別道路內(nèi)的引導(dǎo)線，并使用PID 控制器控制車輛運動以保持引導(dǎo)線圖像在視窗中間區(qū)域，實現(xiàn)實時跟蹤。與激光導(dǎo)航相比，該方法更具實用性和可靠性，但光強對系統(tǒng)有較大影響，需要預(yù)先在道路上設(shè)置引導(dǎo)線和標(biāo)識，靈活性欠佳。Shin 等[6]和Jin 等[7]均使用單個激光測距儀實時檢測路緣來提取可行駛區(qū)域。該方法能有效識別彎道和障礙物，但在寬闊平坦的道路上很難準(zhǔn)確識別路緣特征，進而影響導(dǎo)航效果。García 等[8]使用街道地圖和行車視頻對基于機器學(xué)習(xí)的路況識別模型進行訓(xùn)練，該模型可實時識別道路的幾何形狀和尺寸，由此估計機器人在直行道路和交叉路口所應(yīng)執(zhí)行的車速和轉(zhuǎn)向角。該方法具有一定的靈活性，但無法應(yīng)對道路上的突發(fā)情況，復(fù)雜環(huán)境下容易出錯?？梢姡瑧敉鈱?dǎo)航仍然面臨各種挑戰(zhàn)。

機器人導(dǎo)航的關(guān)鍵技術(shù)包括環(huán)境感知、運動決策、軌跡規(guī)劃、軌跡跟蹤、人機交互等。在這些關(guān)鍵技術(shù)中，機器人的軌跡規(guī)劃和軌跡跟蹤是較為重要的兩項，在戶外導(dǎo)航的安全性和魯棒性方面也發(fā)揮著重要作用。目前主要的軌跡跟蹤控制方法包括各種改進的PID 控制、反步控制、滑?？刂啤⒛Ｐ皖A(yù)測控制等。已有大量文獻表明PID 控制在機器人控制方面取得了成功，自適應(yīng)巡航控制[9]（Adaptive Cruise Control，ACC）和 車 道 保 持 控 制[10]（Lane Keeping Control，LKC）可以使用PID 控制器實現(xiàn)良好的性能。然而PID 控制器的參數(shù)較難確定，對于復(fù)雜擾動的表現(xiàn)稍顯不足。反步控制法要求被控對象具有嚴(yán)格反饋形式或者能夠轉(zhuǎn)化為嚴(yán)格反饋形式，在設(shè)計過程中還存在子系統(tǒng)Lyapunov 函數(shù)不易設(shè)定的問題[11]?；？刂圃谠O(shè)計的過程中滑模函數(shù)及其參數(shù)難以確定，在系統(tǒng)到達階段還容易受到外部擾動而出現(xiàn)抖振問題[12]。模型預(yù)測控制（Model Predictive Control，MPC）是基于預(yù)測模型的控制算法，利用已知的參考軌跡和當(dāng)前時刻的狀態(tài)信息，通過內(nèi)部模型預(yù)測系統(tǒng)在未來時刻的輸出狀態(tài)，根據(jù)給定的二次型性能指標(biāo)函數(shù)和約束條件滾動地求解最優(yōu)控制序列。此外，模型預(yù)測控制算法在處理輸入和狀態(tài)等多條件約束方面有其獨特的優(yōu)勢，非常適用于移動機器人軌跡跟蹤控制[13-14]。

在關(guān)于移動機器人控制器的文獻中，都認(rèn)為所跟蹤的參考軌跡是已知的，這里的軌跡可以是全局軌跡也可以是局部軌跡。對于戶外機器人，環(huán)境交通流量動態(tài)多變，不可控因素多，所跟蹤的應(yīng)是行駛期間實時規(guī)劃和優(yōu)化的局部避障軌跡。當(dāng)前在局部規(guī)劃領(lǐng)域已經(jīng)有了大量的成功經(jīng)驗[15-17]?；舅枷胧窃谄瘘c和終點之間建立局部地圖，然后通過啟發(fā)式搜索算法獲得一系列相鄰網(wǎng)格到達目標(biāo)點。Dijkstra 算法及A*算法是最常見的路徑搜索算法，其中A*算法除了簡單高效外，還具有可擴展性強的優(yōu)點，許多團隊使用A*及其改進算法在實際應(yīng)用中取得了很好的效果[18-19]。

針對戶外導(dǎo)航對實時性、安全性和靈活性的需求，本文設(shè)計了一種“感知建圖-A*規(guī)劃-模型預(yù)測控制（MPC）”移動機器人戶外導(dǎo)航方法，有效提高了軌跡規(guī)劃的穩(wěn)定性和軌跡跟蹤的精度，與傳統(tǒng)方法相比效率更高、具有更強的環(huán)境適應(yīng)力和穩(wěn)定性。

1 軌跡規(guī)劃

軌跡規(guī)劃部分涉及局部地圖構(gòu)建、規(guī)劃終點的計算和基于路段走向改進A*算法這3 部分，最終為模型預(yù)測控制器提供具有避障作用的局部軌跡。

1.1 地圖構(gòu)建

柵格化是目前最常用的環(huán)境建模方法之一，柵格地圖容易構(gòu)建、表示和保存，對于短路徑的規(guī)劃也十分方便，因此局部地圖采用八向柵格圖。LiDAR理論最大探測半徑為200 m，此處建圖半徑為10 m，柵格單元大小為10 cm × 10 cm。基于實時更新策略，將非地面點云數(shù)據(jù)投影到機器人坐標(biāo)系（機器人當(dāng)前位置為原點O，前方為X軸，左側(cè)為Y軸，上方為Z軸）XOY平面，然后對所形成的柵格邊界做1 m的膨脹處理，以補償機器人外形尺寸對避障的影響。最終所構(gòu)建的局部柵格圖如圖1 所示。

圖1 局部柵格圖Fig. 1 Local grid map

1.2 規(guī)劃終點

規(guī)劃涉及到的坐標(biāo)系除機器人坐標(biāo)系外還有大地坐標(biāo)系（機器人初始位置為原點O，東方為X軸，北方為Y軸，上方為Z軸）和里程計坐標(biāo)系（機器人初始位置為原點O，前方為X軸，左側(cè)為Y軸，上方為Z軸），柵格地圖建立在機器人坐標(biāo)系下，而航跡點處于大地坐標(biāo)系下，因此需要將航跡點轉(zhuǎn)換至機器人坐標(biāo)系。

此處定義航跡點由大地坐標(biāo)系轉(zhuǎn)換到機器人坐標(biāo)系的轉(zhuǎn)換關(guān)系為T，它由姿態(tài)變換角度p和平移量q組成，如式（1）所示。

其中：x、y、z分別表示大地坐標(biāo)系相對機器人坐標(biāo)系在X軸、Y軸、Z軸的平移量； θ、 φ、 φ分別表示大地坐標(biāo)系相對于機器人坐標(biāo)系繞X軸、Y軸、Z軸的旋轉(zhuǎn)角，這3 個角度統(tǒng)稱為歐拉角。

其中：R表示由歐拉角組成的旋轉(zhuǎn)矩陣，這里忽略Z方向平移量，并認(rèn)為 θ 、 φ 均為0，則

航跡點在機器人XOY坐標(biāo)系下表示為

如圖2 所示，連接G與原點Orobot的線段，與以探測范圍為半徑的圓交于M，則Orobot、M分別為每次局部路徑規(guī)劃的起點和終點。當(dāng)G處于雷達探測范圍以內(nèi)時，則取G為規(guī)劃的終點。

圖2 局部規(guī)劃起點和終點Fig. 2 Starting point and ending point of local planning

1.3 改進A＊算法

A*算法原估價函數(shù)如下：

A*啟發(fā)函數(shù)如圖3 所示，f(s) 為節(jié)點Snow的總啟發(fā)式代價值，g(s) 代表由規(guī)劃起點Sstart至當(dāng)前檢索點Snow的連通代價值，h(s) 代表由當(dāng)前檢索點Snow至規(guī)劃終點Sgoal的啟發(fā)式代價值。

圖3 A*啟發(fā)函數(shù)Fig. 3 A* heuristic function

A*算法總是選取具有最小總啟發(fā)式代價f(s) 的待擴展節(jié)點，不斷向終點方向搜索，直至到達終點，基于A*的改進算法一直是路徑規(guī)劃領(lǐng)域的研究熱點。此處使用路段走向?qū)*搜索方向進行約束，使得路徑結(jié)果盡量與路段平行[20]，以減少機器人不必要的轉(zhuǎn)向，保證運行的穩(wěn)定性。路段方向和搜索方向如圖4 所示，Droad為由航跡點計算得出的路段方向向量，Dsearch為由子節(jié)點與父節(jié)點計算得出的方向向量， α 為兩者夾角。

圖 4 路段方向與搜索方向Fig. 4 Road direction and search direction

約束后的啟發(fā)式代價值為

其中：w為夾角約束項權(quán)重。

A*搜索結(jié)果為一柵格序列，此處從中選取6 個路徑點，然后結(jié)合機器人當(dāng)前瞬時速度對路徑點分段擬合[21]，使得整體軌跡平滑流暢，便于機器人跟蹤。

2 模型預(yù)測控制器

軌跡跟蹤控制器是機器人導(dǎo)航控制的一項核心技術(shù)，任務(wù)是在得到期望軌跡后，控制機器人跟蹤這條軌跡從當(dāng)前位姿點到達目標(biāo)位姿點，并盡可能減小運行過程中機器人與期望軌跡之間的誤差。模型預(yù)測控制可以有效處理各種約束問題，其迭代步驟最終保證最小的誤差和最佳的性能，在實現(xiàn)控制指標(biāo)最優(yōu)的同時具有一定的魯棒性。模型預(yù)測控制的基本原理描述如圖5 所示。

圖5 模型預(yù)測控制原理圖Fig. 5 Model predictive control diagram

模型預(yù)測控制器算法的實現(xiàn)由構(gòu)建預(yù)測模型、構(gòu)建目標(biāo)函數(shù)、滾動優(yōu)化和反饋矯正[22]這4 部分組成。

2.1 預(yù)測模型

對于差速移動機器人，機器人的狀態(tài)量可由X=[x,y,δ]T表示，其中(x,y)表示機器人在地圖中的位置， δ 表示機器人的航向角，如圖6 所示。

圖6 機器人位姿Fig. 6 Robot pose

移動機器人的運動學(xué)模型可表示為[23]

其中：U=[v,w]T表示機器人的控制量；v代表線速度；w代表角速度。在時刻t=kT0（其中T0為預(yù)測步長，k∈{0,1,2,3,···} ）離散化后為

將式(8)、(9)抽象化為

記預(yù)測步數(shù)為Np，控制時域為Nc，則在預(yù)測時域內(nèi)機器人的狀態(tài)預(yù)測模型可表示為

2.2 目標(biāo)函數(shù)

設(shè)機器人參考軌跡為

在當(dāng)前時刻t，以T0為間隔依次在參考軌跡上取Np個 參 考 點X=f(k+i*T0)(i=1,2,3,···,Np) ，記為Xref(k+i|k)(i=1,2,3,···,Np) 。同時考慮預(yù)測值與參考值的跟蹤誤差、控制量和控制增量，為了使三者都盡可能小，目標(biāo)函數(shù)可表示為

式中：W1、W2、W3分別為跟蹤誤差懲罰權(quán)重、控制量懲罰權(quán)重、控制量增量懲罰權(quán)重。其中跟蹤誤差懲罰權(quán)重的作用是盡可能減少跟蹤誤差；控制量懲罰權(quán)重的作用是保證控制策略更經(jīng)濟；控制量增量懲罰權(quán)重的作用是保證控制器較為平穩(wěn)地控制機器人跟蹤參考軌跡，避免頻繁地加減速和轉(zhuǎn)向。

2.3 滾動優(yōu)化

在依據(jù)實際情況對機器人狀態(tài)、控制量及控制量增量設(shè)定上下限后，可以將多約束下的模型預(yù)測控制問題描述為[24]

為了得到k時刻控制時域Nc內(nèi)的最優(yōu)控制序列Uk，采用二次規(guī)劃方法進行求解，即將帶約束的二次最優(yōu)問題轉(zhuǎn)化為二次規(guī)劃問題進行數(shù)值求解，在每一個控制周期滾動求解后可得到控制序列

2.4 反饋矯正

預(yù)測控制中，采用預(yù)測模型對系統(tǒng)輸出進行估計只是一種思想方式，控制量作用于移動機器人后，由于實際系統(tǒng)存在時變、非線性以及干擾等一些不確定的要素，使得基于固定模型的預(yù)測與實際不可能完全一致，此時就需要采用反饋來對模型進行校正，即將實際輸出值和預(yù)測值做差，再利用此誤差來修正模型的預(yù)測值，這樣才能更好地克服系統(tǒng)的干擾及不確定性。

定義控制時域內(nèi)誤差修正項為ξ(U(i|k))(i=1,2,3,···,Nc)，則修正后的預(yù)測模型可表示為

每個控制周期內(nèi)重復(fù)滾動優(yōu)化和反饋矯正部分，并將求解得到的控制序列中第一個元素U(k|k) 作用于控制對象，即可控制機器人沿局部軌跡行進，實現(xiàn)軌跡跟蹤功能。

3 實驗與結(jié)果分析

為了驗證該方法的可行性，在仿真和戶外環(huán)境下分別使用本導(dǎo)航方法和ROS 開源的Gmapping &Navigation 導(dǎo)航方法進行了對比實驗。

Gazebo 是一款3D 動態(tài)模擬器，能夠準(zhǔn)確有效地模擬機器人的物理特性。Rvi 是ROS 針對系統(tǒng)的可視化需求，為用戶提供的一款三維可視化工具，以幫助開發(fā)者實現(xiàn)所有可監(jiān)測信息的圖形化顯示。在Gazebo 環(huán)境100 m×50 m 的范圍內(nèi)隨機生成100 個邊長在2～4 m 的塊狀障礙物，路段起點GPS 坐標(biāo)（121.431 389，31.15），終點GPS 坐標(biāo)（121.432 471，31.15），起點與終點距離100 m，最終布局如圖7 所示。

圖7 仿真環(huán)境Fig. 7 Simulation environment

本導(dǎo)航方法設(shè)定終點后所生成的期望軌跡和實際行駛軌跡如圖8 所示，路段起點至終點生成的軌跡平滑流暢，可有效躲避障礙物，最大跟蹤誤差出現(xiàn)在軌跡跟蹤啟動階段和障礙物附近，平直路段和曲率半徑較大的路段，跟蹤誤差極小。MPC 控制器參數(shù)設(shè)置如表1 所示。

圖8 改進A*規(guī)劃和MPC 跟蹤效果Fig. 8 Improved A* planning and MPC tracking results

表1 MPC 控制器相關(guān)參數(shù)Table 1 Related parameters of MPC controller

同樣環(huán)境布局下，在Gmapping 所建立的地圖中使用Navigation 功能包進行導(dǎo)航，Dijkstra 規(guī)劃及動態(tài)窗口法跟蹤路徑如圖9 所示。生成的期望路徑可有效躲避障礙物，最大跟蹤誤差出現(xiàn)在障礙物附近的轉(zhuǎn)角處，平直路段跟蹤誤差較小。

圖9 Gmapping & Navigation 導(dǎo)航效果Fig. 9 Gmapping & Navigation results

改進的A*算法和Dijkstra 算法路徑規(guī)劃結(jié)果及耗時參數(shù)見表2。MPC 和動態(tài)窗口法實際跟蹤誤差如圖10 所示。由于Dijkstra 是基于廣度優(yōu)先策略的搜索方法，會有大量冗余的搜索操作，而A*的啟發(fā)式搜索策略極大提高了搜索效率，使搜索耗時更短。本文在A*基礎(chǔ)上加入的路段走向引導(dǎo)信息減少了路徑的轉(zhuǎn)向次數(shù)，而對路徑的長度沒有較大影響，這更有利于機器人的戶外行駛。由圖10 可見，跟蹤過程中，動態(tài)窗口法穩(wěn)態(tài)誤差在1.8 cm 左右，最大跟蹤誤差為10.3 cm；MPC 法穩(wěn)態(tài)誤差在0.5 cm 左右，拐角處誤差較大，最大值為6.6 cm，跟蹤精度優(yōu)于動態(tài)窗口法。動態(tài)窗口法在復(fù)雜路況下（圖9 中2、3、4 區(qū)域）為了保證避障效果而速度較慢，整體任務(wù)用時281 s，耗時較長；而MPC 算法的高精度軌跡跟蹤特性使得機器人在復(fù)雜路況下不必過多損失行駛速度也能保證避障的有效性，任務(wù)用時243 s，耗時更短。

表2 規(guī)劃及耗時參數(shù)Table 2 Planning and time consuming parameters

圖10 跟蹤誤差Fig. 10 Tracking error

最后，在室外進行了對比實驗。如圖11 所示，首先拾取序號為0～6 的7 個GPS 航跡點，導(dǎo)航任務(wù)可表示為：0-1-2-3-4-5-6-1-0，機器人運行至航跡點后駐停，旋轉(zhuǎn)至相應(yīng)角度后繼續(xù)行進。圖示4 處位置設(shè)置了障礙物，以測試避障效果。

圖11 GPS 航跡點及障礙物Fig. 11 GPS waypoints and obstacles

ROS 開源方案需要預(yù)先建立導(dǎo)航環(huán)境地圖，然后依次發(fā)送任務(wù)航跡點以實現(xiàn)導(dǎo)航任務(wù)，其中建圖耗時20'，導(dǎo)航耗時9'24''，整體耗時29'24''。本文導(dǎo)航方法將航跡點轉(zhuǎn)化至局部柵格圖后以5 s 為周期不斷計算局部避障軌跡，使用模型預(yù)測控制器跟蹤軌跡以到達指定的航跡點，由于無需預(yù)先建立地圖，任務(wù)整體耗時6'43''，效率明顯優(yōu)于ROS 開源方案。機器人戶外實際行駛軌跡如圖12所示。

圖12 戶外導(dǎo)航軌跡Fig. 12 Outdoor navigation trajectory

回放MPC 軌跡跟蹤數(shù)據(jù)包，復(fù)現(xiàn)機器人行駛至4 號障礙物時避障場景如圖13 所示。避障過程中機器人與障礙物及路緣距離在1 m 以上，保證了導(dǎo)航的安全。

4 結(jié)束語

本文設(shè)計的“感知建圖-A*規(guī)劃-MPC”移動機器人戶外導(dǎo)航方法無需預(yù)先建立全局地圖，依靠激光雷達實時建立局部地圖并規(guī)劃避障路徑，節(jié)省了內(nèi)存空間，實時性強，大大提高了使用的靈活性。針對戶外導(dǎo)航提出的基于路段走向改進的A*算法提升了搜索的穩(wěn)定性，在保證避障的同時減少了機器人行駛過程中的轉(zhuǎn)向次數(shù)；所設(shè)計的差速模型預(yù)測軌跡跟蹤控制器適用于所有差速移動平臺，跟蹤精度高，軌跡跟蹤耗時更短。戶外實驗表明，機器人在收到GPS 航跡點后，能夠自主避障依次到達航跡點，行駛過程中與路緣和障礙物距離在1 m 以上，可安全有效地完成導(dǎo)航任務(wù)，解決了戶外導(dǎo)航過程中的建圖、規(guī)劃、避障和控制等問題，滿足戶外大場景導(dǎo)航的任務(wù)需求。