基于神經(jīng)網(wǎng)絡(luò)的移動(dòng)機(jī)器人軌跡預(yù)測控制

江一鳴，吳 坷，李佳洛，孟范偉

(東北大學(xué)秦皇島分校， 河北 秦皇島 066004)

1 引言

隨著自動(dòng)化技術(shù)的不斷發(fā)展，移動(dòng)機(jī)器人技術(shù)逐步被應(yīng)用在工業(yè)、農(nóng)業(yè)、救援、交通等各個(gè)領(lǐng)域。移動(dòng)機(jī)器人路徑規(guī)劃是集動(dòng)態(tài)環(huán)境檢測、動(dòng)態(tài)規(guī)劃與執(zhí)行、行為決策與控制等數(shù)項(xiàng)功能于一體的綜合化智能系統(tǒng),近年來已經(jīng)成功地應(yīng)用到工農(nóng)業(yè)生產(chǎn)的各個(gè)領(lǐng)域，并在各個(gè)特定的工作空間下分別實(shí)現(xiàn)了有效的解決方案。本文中針對未知離散障礙物環(huán)境下移動(dòng)機(jī)器人的環(huán)境感知、導(dǎo)航控制系統(tǒng)進(jìn)一步設(shè)計(jì)、完善與測試。

移動(dòng)機(jī)器人系統(tǒng)主要包含環(huán)境感知、路徑規(guī)劃和運(yùn)動(dòng)控制3個(gè)過程。環(huán)境感知過程通常采用激光SLAM技術(shù)進(jìn)行，常用的感知算法包括GMapping算法、Hector算法等，然而對于給定的未知離散障礙物環(huán)境，此類算法構(gòu)建的感知地圖無法直接用于規(guī)劃得到滿足約束條件的軌跡。故本文中在GMapping構(gòu)建感知地圖的基礎(chǔ)上，進(jìn)一步設(shè)計(jì)感知數(shù)據(jù)處理方法，從而約束機(jī)器人的規(guī)劃。

路徑規(guī)劃過程包含全局規(guī)劃器和局部規(guī)劃器。全局規(guī)劃器基于感知地圖信息進(jìn)行規(guī)劃，由于移動(dòng)機(jī)器人的實(shí)際工作空間存在動(dòng)態(tài)變化，通常將全局路徑分割為多個(gè)局部過程，由局部規(guī)劃器基于動(dòng)態(tài)點(diǎn)云信息進(jìn)一步規(guī)劃。丁皓等基于A算法，構(gòu)建移動(dòng)機(jī)器人的全局路徑。然而其通常耗費(fèi)大量算力用于擴(kuò)展無用節(jié)點(diǎn)且存在所得軌跡不平滑等問題，本文中采用JPS算法代替A算法從而提高算法的效率。同時(shí)采用模型預(yù)測控制(model predictive control，MPC)用于局部規(guī)劃，預(yù)測控制用于移動(dòng)機(jī)器人的運(yùn)動(dòng)過程調(diào)節(jié)，能夠提升計(jì)算速度，加快移動(dòng)機(jī)器人軌跡跟蹤的速度，已經(jīng)應(yīng)用到移動(dòng)機(jī)器人控制的各個(gè)領(lǐng)域。

傳統(tǒng)的移動(dòng)機(jī)器人自主導(dǎo)航需提前獲取感知地圖信息，即在已知環(huán)境下規(guī)劃導(dǎo)航與控制。針對未知的離散障礙物環(huán)境，本文中創(chuàng)新性地構(gòu)建了神經(jīng)網(wǎng)絡(luò)控制器以實(shí)現(xiàn)移動(dòng)機(jī)器人的基于視覺的主探索方案。并在感知環(huán)境后自主切換至導(dǎo)航功能，以實(shí)現(xiàn)路徑規(guī)劃與跟蹤。

綜上所述，本文章節(jié)安排及研究成果如下：第二部分進(jìn)行系統(tǒng)的阿克曼運(yùn)動(dòng)學(xué)模型構(gòu)建及整體工作流程設(shè)計(jì)；第三部分針對離散障礙物信息構(gòu)建神經(jīng)網(wǎng)絡(luò)控制器，控制機(jī)器人自主感知未知環(huán)境，并將感知環(huán)境信息進(jìn)行預(yù)處理，從而約束機(jī)器人運(yùn)動(dòng)；第四部分根據(jù)預(yù)處理感知得到的環(huán)境信息，構(gòu)建路徑規(guī)劃器，實(shí)現(xiàn)機(jī)器人的自主導(dǎo)航；第五部分進(jìn)行仿真與實(shí)踐測試，驗(yàn)證機(jī)器人系統(tǒng)整體設(shè)計(jì)的合理性及穩(wěn)定性；第六部分進(jìn)行全文的總結(jié)。

2 模型構(gòu)建

以世界坐標(biāo)系為參考坐標(biāo)系，構(gòu)建移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型，如圖1所示。

圖1 運(yùn)動(dòng)學(xué)模型示意圖Fig.1 Schematic diagram of kinematic model

機(jī)器人底盤前后兩軸中心相對世界坐標(biāo)分別為(,)和(,)。前輪偏向角為，機(jī)器人的航向角為，模型的其余參數(shù)如表1所示，其中采用下標(biāo)和分別表示后軸和前軸。

為降低模型復(fù)雜度，對其進(jìn)行條件假設(shè)如下：

1) 機(jī)器人的運(yùn)動(dòng)過程是剛體運(yùn)動(dòng)；

2) 機(jī)器人在運(yùn)動(dòng)過程中，前軸兩輪胎的偏向角大小相等，由舵機(jī)控制；

3) 機(jī)器人在進(jìn)行轉(zhuǎn)向運(yùn)動(dòng)時(shí)，自身的轉(zhuǎn)彎半徑等同于道路的曲率半徑。

表1 運(yùn)動(dòng)學(xué)模型參數(shù)Table 1 Parameters of the kinematic model

2.1 運(yùn)動(dòng)學(xué)分析

對機(jī)器人底盤后軸進(jìn)行分析，將其線速度分解至世界坐標(biāo)系的兩軸方向上：

(1)

對機(jī)器人前軸進(jìn)行分析，可得如下約束條件：

(2)

根據(jù)機(jī)器人底盤的航向角及軸距，可知前后軸軸心處坐標(biāo)間的數(shù)學(xué)關(guān)系，對其求導(dǎo)得到如下約束：

(3)

將式(1)與式(3)聯(lián)立，所得表達(dá)式代入式(2)并展開三角函數(shù)，可得機(jī)器人底盤的角速度表達(dá)式：

=tan

(4)

對航向角求導(dǎo)可得機(jī)器人底盤的角速度，進(jìn)而由式(1)和式(4)可得如下狀態(tài)空間表達(dá)式：

(5)

2.2 線性誤差模型

對式(5)進(jìn)行線性化，令表示期望狀態(tài)，則對于任意一個(gè)期望狀態(tài)都滿足如下條件：

(6)

對在期望處進(jìn)行一階泰勒展開如下：

(7)

(8)

系數(shù)矩陣由雅可比矩陣求得，記為、，則得到線性誤差模型如式(9)所示：

(9)

2.3 模型離散化

對式(9)近似離散化得到下式：

(10)

式中，參變量為采樣時(shí)間。定義系統(tǒng)輸出方程如下：

(11)

(12)

(13)

2.4 系統(tǒng)設(shè)計(jì)

本文中設(shè)計(jì)的移動(dòng)機(jī)器人工作流程如圖2所示，整體運(yùn)作可分為探索、規(guī)劃2部分。其中探索部分采用神經(jīng)網(wǎng)絡(luò)控制器避障行駛，同時(shí)采用激光雷達(dá)感知環(huán)境點(diǎn)云信息；規(guī)劃部分根據(jù)感知得到的點(diǎn)云地圖信息進(jìn)行進(jìn)一步的采用JPS全局規(guī)劃器和MPC局部規(guī)劃器規(guī)劃，控制機(jī)器人在期望全局路徑下自主導(dǎo)航行駛。

針對機(jī)器人行駛環(huán)境，做出如下假設(shè)：

1)機(jī)器人在初始運(yùn)行時(shí)，周圍環(huán)境特征信息處于未知狀態(tài);

2)機(jī)器人運(yùn)行的環(huán)境中包含大量的離散的障礙物信息;

3)采用紅色、藍(lán)色錐桶模擬離散障礙物并規(guī)定機(jī)器人感知完成后自動(dòng)進(jìn)行導(dǎo)航。

圖2 移動(dòng)機(jī)器人系統(tǒng)工作流程框圖Fig.2 Working flow diagram of mobile robot system

3 探索模塊設(shè)計(jì)

針對探索任務(wù)，設(shè)計(jì)思路如下：采用相機(jī)實(shí)時(shí)獲取環(huán)境特征并進(jìn)行目標(biāo)信息提??；根據(jù)提取到的有效信息設(shè)計(jì)神經(jīng)網(wǎng)絡(luò)自主避障；將離散點(diǎn)云信息連續(xù)化處理，約束規(guī)劃效果。

3.1 離散障礙物特征提取

使用OpenCV庫對離散障礙物特征提取，提取圖像中符合條件的紅色錐桶、藍(lán)色錐桶的位置及像素面積特征。

離散障礙物目標(biāo)檢測算法的檢測過程為：

1:如果相機(jī)節(jié)點(diǎn)VideoCapture開啟:2:捕捉圖像幀ImgInput3:對ImgInput進(jìn)行高斯模糊4:將ImgInput由RGB通道轉(zhuǎn)為LAB通道5:對ImgInput進(jìn)行開運(yùn)算得到ImgOpen6:獲取邊界信息bndscv2.findcontours(ImgOpen)7:初始化bndsRed表和bndsBlue表8:遍歷bnds中所有item:9:如果bnds[i]滿足藍(lán)色閾值,且面積area>1 500 Pix:10:bndsBlue.append(bnds[i])11:如果bnds[i]滿足紅色閾值,且面積area>1 500Pix:12:bndsRed.append(bnds[i])13:計(jì)算紅色和藍(lán)色錐桶的中心坐標(biāo)xRed和xBlue

對采集得到的環(huán)境特征圖像進(jìn)行高斯模糊并進(jìn)行閾值分割，使得分割效果更為平滑。由于相機(jī)鏡頭邊緣處圖像與中心區(qū)域亮度差明顯，故進(jìn)行分區(qū)域閾值分割及開運(yùn)算，有效提升邊緣信息的提取效果，如圖3所示。

圖3 分區(qū)域閾值分割示意圖Fig.3 Schematic diagram of threshold segmentation in sub regions

對處理后的圖像進(jìn)行邊界提取，并二次判定所得最大連通域是否為近似三角。二次判定通過的連通域，以面積最大錐桶為起點(diǎn)，從右向左(從左向右)對紅色(藍(lán)色)錐桶連線，如圖4所示。

圖4 目標(biāo)檢測示意圖Fig.4 Schematic diagram of target detection

3.2 神經(jīng)網(wǎng)絡(luò)控制器設(shè)計(jì)

常見的控制算法包括PID控制、神經(jīng)網(wǎng)絡(luò)控制等。傳統(tǒng)的視覺控制算法通常采用PID控制器實(shí)現(xiàn)，其設(shè)計(jì)原理如圖5所示。

圖5 PID控制器設(shè)計(jì)原理圖Fig.5 Design schematic diagram of PID controller

采用網(wǎng)格法調(diào)試PID控制器參數(shù)，設(shè)置方向間隔為1,方向間隔為0.05。多次試驗(yàn)并觀察響應(yīng)曲線，最終選擇最優(yōu)的PD參數(shù)為(5,0.2)。

基于“深層網(wǎng)絡(luò)結(jié)構(gòu)+單層計(jì)算復(fù)雜度低”的思路，本文中設(shè)計(jì)了“5層網(wǎng)絡(luò)+限幅模塊”的網(wǎng)絡(luò)結(jié)構(gòu)，用以平衡深層網(wǎng)絡(luò)準(zhǔn)確率高以及淺層網(wǎng)絡(luò)計(jì)算速度快之間的矛盾。選擇ReLU作為激活函數(shù)，相比其他激活函數(shù)，進(jìn)一步降低了計(jì)算復(fù)雜度，且訓(xùn)練速度快，便于控制算法實(shí)時(shí)更新迭代。如圖6所示為所構(gòu)建的網(wǎng)絡(luò)結(jié)構(gòu)。

圖6 神經(jīng)網(wǎng)絡(luò)控制器結(jié)構(gòu)示意圖Fig.6 Structure diagram of neural network controller

通過多次實(shí)驗(yàn)，在推理速度和模型精度之間平衡，最終選擇五層全ReLU的網(wǎng)絡(luò)結(jié)構(gòu)。神經(jīng)網(wǎng)絡(luò)參數(shù)如表2所示。

表2 神經(jīng)網(wǎng)絡(luò)參數(shù)Table 2 Parameters of the neural network

綜上，機(jī)器人將依據(jù)感知得到的環(huán)境信息構(gòu)建神經(jīng)網(wǎng)絡(luò)控制器，輸出對應(yīng)的期望控制量，控制車輛實(shí)現(xiàn)自主感知環(huán)境工作。

神經(jīng)網(wǎng)絡(luò)控制器的輸入量為上述提取到的十二維度(紅色、藍(lán)色2種顏色面積最大的3個(gè)錐桶的中心坐標(biāo)信息和像素面積信息)的圖像特征信息。由于所得圖像存在缺失特征的可能性，先對數(shù)據(jù)做預(yù)處理后，再將其輸入神經(jīng)網(wǎng)絡(luò)進(jìn)行控制，空缺特征填補(bǔ)情況為：紅色的中心坐標(biāo)為0，藍(lán)色的中心坐標(biāo)為1 280；面積均為0。

將人工采集的未知障礙物環(huán)境數(shù)據(jù)集進(jìn)行測試，比較PID控制算法、神經(jīng)網(wǎng)絡(luò)控制算法和測試集的誤差，如圖7所示。在實(shí)際采集的數(shù)據(jù)中，PID控制器的RMSE(均方根誤差)為15.01，神經(jīng)網(wǎng)絡(luò)控制器的RMSE為9.47，可得神經(jīng)網(wǎng)絡(luò)控制效果優(yōu)于PID控制器。

圖7 PID控制器同神經(jīng)網(wǎng)絡(luò)控制器的誤差曲線Fig.7 Error curve of PID controller and neural network controller

3.3 環(huán)境信息感知與處理

在神經(jīng)網(wǎng)絡(luò)控制器控制機(jī)器人自主感知環(huán)境的同時(shí)，采用2D激光雷達(dá)基于GMapping算法，實(shí)現(xiàn)對環(huán)境點(diǎn)云地圖的構(gòu)建。如圖8所示為構(gòu)建的初始感知地圖。

傳統(tǒng)的圖搜索路徑規(guī)劃算法普遍用于連續(xù)障礙物環(huán)境，而對于離散障礙物環(huán)境則需要對地圖進(jìn)行處理。單純提高障礙物膨脹無法達(dá)到良好的效果，故進(jìn)行預(yù)處理作業(yè)。將離散的障礙物進(jìn)行連線從而擬合為連續(xù)障礙物環(huán)境，確保路徑規(guī)劃的準(zhǔn)確性，實(shí)現(xiàn)邏輯如圖9所示。

圖8 初始感知地圖Fig.8 Original perception map

圖9 感知數(shù)據(jù)預(yù)處理的實(shí)現(xiàn)邏輯框圖Fig.9 Implementation logic block diagram of perceptual data preprocessing

在采集環(huán)境信息的同時(shí)，記錄機(jī)器人里程計(jì)的位姿，從而獲取行進(jìn)的路徑并區(qū)分內(nèi)外離散障礙物，使其連接為2個(gè)閉環(huán)。圖10所示為預(yù)處理后的感知地圖。

圖10 預(yù)處理后感知地圖Fig.10 Perceptual map after preprocessing

4 規(guī)劃模塊設(shè)計(jì)

環(huán)境感知得到地圖后，根據(jù)給定的目標(biāo)點(diǎn)與起點(diǎn)，機(jī)器人將自主完成規(guī)劃任務(wù)。本文中設(shè)計(jì)采用JPS算法構(gòu)建全局路徑并使用MPC構(gòu)建局部路徑跟蹤期望軌跡。

4.1 JPS全局規(guī)劃器

JPS(jump point search)算法是一種基于圖搜索的啟發(fā)式全局路徑規(guī)劃算法，相比于A算法，該算法通過裁剪與跳躍，降低搜索的范圍。其代價(jià)函數(shù)()如下：

()=()+()

(14)

式中:()表示從起點(diǎn)至節(jié)點(diǎn)的最小代價(jià)；()表示從節(jié)點(diǎn)至終點(diǎn)的預(yù)估代價(jià)，此處采用曼哈頓距離計(jì)算。

JPS的具體算法流程為：

1:初始化：創(chuàng)建用于放置節(jié)點(diǎn)的Open表和Closed表；

2:輸入值：起點(diǎn)節(jié)點(diǎn)Start、目標(biāo)節(jié)點(diǎn)Goal；

3:Start放置于Open表中，設(shè)置Start代價(jià)值為0；

4:遍歷開始；

5:如果Open表非空；

6:彈出Open表中代價(jià)值()最低的節(jié)點(diǎn)；

7:將節(jié)點(diǎn)加入Closed表，視為已探索節(jié)點(diǎn)；

8:如果節(jié)點(diǎn)是目標(biāo)節(jié)點(diǎn)Goal；

9:從節(jié)點(diǎn)起追蹤Parent節(jié)點(diǎn)，直至Start；

10:返回路徑，算法結(jié)束；

11:遍歷節(jié)點(diǎn)所有不在Closed表中的鄰居節(jié)點(diǎn)；

12:設(shè)置節(jié)點(diǎn)為節(jié)點(diǎn)的Parent節(jié)點(diǎn)；

13:計(jì)算節(jié)點(diǎn)的代價(jià)值()；

14:將節(jié)點(diǎn)加入Open表中；

15:遍歷結(jié)束。

算法的實(shí)現(xiàn)邏輯同A算法類似，其主要不同點(diǎn)在于對鄰居節(jié)點(diǎn)的定義，如圖11所示。A算法在探索時(shí)以八聯(lián)通方式進(jìn)行，探索當(dāng)前節(jié)點(diǎn)周圍的八個(gè)節(jié)點(diǎn)；而JPS算法則采用跳躍規(guī)則橫向、縱向、對角探索節(jié)點(diǎn)。

圖11 鄰居節(jié)點(diǎn)定義示意圖Fig.11 Schematic diagram of neighbor node definition

通過仿真相同環(huán)境，對比A算法和JPS算法的效率以及所得路徑的長度如圖12所示。分析可知，前者計(jì)算耗時(shí)相對后者更長。

圖12 全局路徑規(guī)劃器規(guī)劃圖Fig.12 Global path planner plan

為將機(jī)器人移動(dòng)的方向約束為指定方向而非反向規(guī)劃，此處設(shè)計(jì)增加多個(gè)局部目標(biāo)點(diǎn)，迫使機(jī)器人抵達(dá)后再次規(guī)劃后續(xù)目標(biāo)點(diǎn)。如圖13所示，其中左下角箭頭處為機(jī)器人當(dāng)前位姿所在，紅線為兩點(diǎn)間由JPS規(guī)劃得到的全局路徑。

圖13 局部目標(biāo)點(diǎn)示意圖Fig.13 Schematic diagram of local target points

4.2 MPC局部規(guī)劃器

針對JPS規(guī)劃得到的軌跡，進(jìn)一步設(shè)計(jì)MPC局部規(guī)劃器用于跟蹤期望軌跡，如圖14為MPC局部規(guī)劃器的規(guī)劃過程框圖。

圖14 MPC規(guī)劃器規(guī)劃過程框圖Fig.14 MPC planner planning process block diagram

設(shè)預(yù)測時(shí)域?yàn)?span id="j5i0abt0b" class="subscript">，控制時(shí)域?yàn)?span id="j5i0abt0b" class="subscript">(≤)，對式多次迭代得到最終的預(yù)測模型：

()=()+Δ()

(15)

min=ΔΔ+ΔΔ

(16)

由預(yù)測模型[式(15)]可知，若已知當(dāng)前時(shí)刻機(jī)器人的狀態(tài)量和控制時(shí)域內(nèi)的控制增量，即可預(yù)測得到預(yù)測時(shí)域的系統(tǒng)輸出。

進(jìn)一步采用平方誤差模型設(shè)計(jì)目標(biāo)函數(shù)[式(16)]用于迭代得到局部最優(yōu)解，其中矩陣參量和為權(quán)重矩陣。對其做如下處理進(jìn)行簡化：

(17)

最后，考慮系統(tǒng)的約束條件。對于系統(tǒng)控制增量應(yīng)滿足如下不等式：

Δ≤Δ≤Δ

(18)

由此得到預(yù)測時(shí)域的控制增量Δ，用于規(guī)劃軌跡跟蹤所需的控制量。

5 仿真與實(shí)踐

針對上述設(shè)計(jì)系統(tǒng)，使用Matlab的CarSim模塊進(jìn)行仿真測試，隨后設(shè)計(jì)實(shí)際機(jī)器人系統(tǒng)在室外環(huán)境下實(shí)驗(yàn)測試設(shè)計(jì)方案。

5.1 系統(tǒng)仿真測試

基于CarSim模塊，構(gòu)建如圖15所示仿真模型。

圖15 仿真模型框圖Fig.15 Structure block diagram of simulation model

此處模擬真實(shí)離散障礙物環(huán)境，構(gòu)建仿真測試。設(shè)置機(jī)器人期望移動(dòng)線速度為2 m/s；約束舵機(jī)轉(zhuǎn)向角度小于80°；機(jī)器人初始移動(dòng)時(shí)間=0；采樣間隔時(shí)間=01 s，如圖16所示。

同時(shí)，根據(jù)期望軌跡計(jì)算機(jī)器人在移動(dòng)時(shí)抵達(dá)期望軌跡的理論角度并記錄實(shí)際控制實(shí)現(xiàn)的角度，如圖17所示。0～20 s內(nèi)，RMSE=7.05。在20～28.3 s，由于23 s時(shí)的延遲效應(yīng)，RMSE=45.261?？傮w期望值與實(shí)際值擬合程度高，系統(tǒng)穩(wěn)定。

綜上所述，本文中所設(shè)計(jì)的移動(dòng)機(jī)器人系統(tǒng)穩(wěn)定性強(qiáng)，能適應(yīng)于離散障礙物環(huán)境下的未知環(huán)境探索與導(dǎo)航。

圖16 仿真測試結(jié)果圖Fig.16 Simulation test result diagram

圖17 機(jī)器人角度控制效果曲線Fig.17 Robot angle control effect curve

5.2 實(shí)際環(huán)境測試

本文中進(jìn)一步搭建實(shí)體移動(dòng)機(jī)器人系統(tǒng)，如圖18所示，并基于ROS框架實(shí)現(xiàn)對機(jī)器人的軟件系統(tǒng)搭建。

圖18 機(jī)器人實(shí)物圖Fig.18 Pictures of robot objects

綜合考慮各個(gè)傳感器的特性，對其置放位置進(jìn)行規(guī)劃。其中，相機(jī)安裝至車輛前端用于采集未知環(huán)境的圖像信息；IMU和激光雷達(dá)安裝至車輛中部位置，用于檢測車輛姿態(tài)信息及采集周圍環(huán)境的點(diǎn)云信息；處理器及控制器安裝至車輛后端位置，用于處理環(huán)境信息、決策車輛行駛狀態(tài)及控制車輛運(yùn)動(dòng)；編碼器計(jì)米輪安裝至車輛后側(cè)用于采集車輛的里程信息。

關(guān)鍵傳感器型號如表3所示。

表3 關(guān)鍵傳感器型號Table 3 Keysensor models

啟動(dòng)程序后，機(jī)器人將自動(dòng)依據(jù)視覺信息和神經(jīng)網(wǎng)絡(luò)控制器進(jìn)行自主探索環(huán)境，獲取到圖8所示的初始感知地圖后對其進(jìn)行預(yù)處理，進(jìn)而得到圖10所示的預(yù)處理后感知地圖。此后，采用JPS全局規(guī)劃器規(guī)劃全局路徑，并由MPC局部規(guī)劃器進(jìn)行軌跡跟蹤測試，如圖19所示。實(shí)際運(yùn)行效果表明，系統(tǒng)設(shè)計(jì)合理且運(yùn)行穩(wěn)定。

圖19 實(shí)際跟蹤測試結(jié)果圖Fig.19 Actual test result picture

6 結(jié)論

1) 使用Simulink和CarSim模塊構(gòu)建車輛模型進(jìn)行仿真，驗(yàn)證控制系統(tǒng)的可實(shí)施性。

2) 設(shè)計(jì)并使用目標(biāo)檢測和神經(jīng)網(wǎng)絡(luò)控制算法，實(shí)現(xiàn)車輛自主建圖和路徑規(guī)劃。

3) 根據(jù)獲得軌跡信息，設(shè)計(jì)MPC軌跡跟蹤器，在已知環(huán)境中有效地跟蹤車輛的參考軌跡。該算法通過仿真測試和實(shí)際使用可有效實(shí)現(xiàn)車輛的穩(wěn)定規(guī)劃，軌跡跟蹤效果顯著。