基于神經網絡的移動機器人軌跡預測控制

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

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

1 引言

隨著自動化技術的不斷發展，移動機器人技術逐步被應用在工業、農業、救援、交通等各個領域。移動機器人路徑規劃是集動態環境檢測、動態規劃與執行、行為決策與控制等數項功能于一體的綜合化智能系統,近年來已經成功地應用到工農業生產的各個領域，并在各個特定的工作空間下分別實現了有效的解決方案。本文中針對未知離散障礙物環境下移動機器人的環境感知、導航控制系統進一步設計、完善與測試。

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

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

傳統的移動機器人自主導航需提前獲取感知地圖信息，即在已知環境下規劃導航與控制。針對未知的離散障礙物環境，本文中創新性地構建了神經網絡控制器以實現移動機器人的基于視覺的主探索方案。并在感知環境后自主切換至導航功能，以實現路徑規劃與跟蹤。

綜上所述，本文章節安排及研究成果如下：第二部分進行系統的阿克曼運動學模型構建及整體工作流程設計；第三部分針對離散障礙物信息構建神經網絡控制器，控制機器人自主感知未知環境，并將感知環境信息進行預處理，從而約束機器人運動；第四部分根據預處理感知得到的環境信息，構建路徑規劃器，實現機器人的自主導航；第五部分進行仿真與實踐測試，驗證機器人系統整體設計的合理性及穩定性；第六部分進行全文的總結。

2 模型構建

以世界坐標系為參考坐標系，構建移動機器人的運動學模型，如圖1所示。

圖1 運動學模型示意圖Fig.1 Schematic diagram of kinematic model

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

為降低模型復雜度，對其進行條件假設如下：

1) 機器人的運動過程是剛體運動；

2) 機器人在運動過程中，前軸兩輪胎的偏向角大小相等，由舵機控制；

3) 機器人在進行轉向運動時，自身的轉彎半徑等同于道路的曲率半徑。

表1 運動學模型參數Table 1 Parameters of the kinematic model

2.1 運動學分析

對機器人底盤后軸進行分析，將其線速度分解至世界坐標系的兩軸方向上：

(1)

對機器人前軸進行分析，可得如下約束條件：

(2)

根據機器人底盤的航向角及軸距，可知前后軸軸心處坐標間的數學關系，對其求導得到如下約束：

(3)

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

=tan

(4)

對航向角求導可得機器人底盤的角速度，進而由式(1)和式(4)可得如下狀態空間表達式：

(5)

2.2 線性誤差模型

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

(6)

對在期望處進行一階泰勒展開如下：

(7)

(8)

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

(9)

2.3 模型離散化

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

(10)

式中，參變量為采樣時間。定義系統輸出方程如下：

(11)

(12)

(13)

2.4 系統設計

本文中設計的移動機器人工作流程如圖2所示，整體運作可分為探索、規劃2部分。其中探索部分采用神經網絡控制器避障行駛，同時采用激光雷達感知環境點云信息；規劃部分根據感知得到的點云地圖信息進行進一步的采用JPS全局規劃器和MPC局部規劃器規劃，控制機器人在期望全局路徑下自主導航行駛。

針對機器人行駛環境，做出如下假設：

1)機器人在初始運行時，周圍環境特征信息處于未知狀態;

2)機器人運行的環境中包含大量的離散的障礙物信息;

3)采用紅色、藍色錐桶模擬離散障礙物并規定機器人感知完成后自動進行導航。

圖2 移動機器人系統工作流程框圖Fig.2 Working flow diagram of mobile robot system

3 探索模塊設計

針對探索任務，設計思路如下：采用相機實時獲取環境特征并進行目標信息提取；根據提取到的有效信息設計神經網絡自主避障；將離散點云信息連續化處理，約束規劃效果。

3.1 離散障礙物特征提取

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

離散障礙物目標檢測算法的檢測過程為：

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

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

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

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

圖4 目標檢測示意圖Fig.4 Schematic diagram of target detection

3.2 神經網絡控制器設計

常見的控制算法包括PID控制、神經網絡控制等。傳統的視覺控制算法通常采用PID控制器實現，其設計原理如圖5所示。

圖5 PID控制器設計原理圖Fig.5 Design schematic diagram of PID controller

采用網格法調試PID控制器參數，設置方向間隔為1,方向間隔為0.05。多次試驗并觀察響應曲線，最終選擇最優的PD參數為(5,0.2)。

基于“深層網絡結構+單層計算復雜度低”的思路，本文中設計了“5層網絡+限幅模塊”的網絡結構，用以平衡深層網絡準確率高以及淺層網絡計算速度快之間的矛盾。選擇ReLU作為激活函數，相比其他激活函數，進一步降低了計算復雜度，且訓練速度快，便于控制算法實時更新迭代。如圖6所示為所構建的網絡結構。

圖6 神經網絡控制器結構示意圖Fig.6 Structure diagram of neural network controller

通過多次實驗，在推理速度和模型精度之間平衡，最終選擇五層全ReLU的網絡結構。神經網絡參數如表2所示。

表2 神經網絡參數Table 2 Parameters of the neural network

綜上，機器人將依據感知得到的環境信息構建神經網絡控制器，輸出對應的期望控制量，控制車輛實現自主感知環境工作。

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

將人工采集的未知障礙物環境數據集進行測試，比較PID控制算法、神經網絡控制算法和測試集的誤差，如圖7所示。在實際采集的數據中，PID控制器的RMSE(均方根誤差)為15.01，神經網絡控制器的RMSE為9.47，可得神經網絡控制效果優于PID控制器。

圖7 PID控制器同神經網絡控制器的誤差曲線Fig.7 Error curve of PID controller and neural network controller

3.3 環境信息感知與處理

在神經網絡控制器控制機器人自主感知環境的同時，采用2D激光雷達基于GMapping算法，實現對環境點云地圖的構建。如圖8所示為構建的初始感知地圖。

傳統的圖搜索路徑規劃算法普遍用于連續障礙物環境，而對于離散障礙物環境則需要對地圖進行處理。單純提高障礙物膨脹無法達到良好的效果，故進行預處理作業。將離散的障礙物進行連線從而擬合為連續障礙物環境，確保路徑規劃的準確性，實現邏輯如圖9所示。

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

圖9 感知數據預處理的實現邏輯框圖Fig.9 Implementation logic block diagram of perceptual data preprocessing

在采集環境信息的同時，記錄機器人里程計的位姿，從而獲取行進的路徑并區分內外離散障礙物，使其連接為2個閉環。圖10所示為預處理后的感知地圖。

圖10 預處理后感知地圖Fig.10 Perceptual map after preprocessing

4 規劃模塊設計

環境感知得到地圖后，根據給定的目標點與起點，機器人將自主完成規劃任務。本文中設計采用JPS算法構建全局路徑并使用MPC構建局部路徑跟蹤期望軌跡。

4.1 JPS全局規劃器

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

()=()+()

(14)

式中:()表示從起點至節點的最小代價；()表示從節點至終點的預估代價，此處采用曼哈頓距離計算。

JPS的具體算法流程為：

1:初始化：創建用于放置節點的Open表和Closed表；

2:輸入值：起點節點Start、目標節點Goal；

3:Start放置于Open表中，設置Start代價值為0；

4:遍歷開始；

5:如果Open表非空；

6:彈出Open表中代價值()最低的節點；

7:將節點加入Closed表，視為已探索節點；

8:如果節點是目標節點Goal；

9:從節點起追蹤Parent節點，直至Start；

10:返回路徑，算法結束；

11:遍歷節點所有不在Closed表中的鄰居節點；

12:設置節點為節點的Parent節點；

13:計算節點的代價值()；

14:將節點加入Open表中；

15:遍歷結束。

算法的實現邏輯同A算法類似，其主要不同點在于對鄰居節點的定義，如圖11所示。A算法在探索時以八聯通方式進行，探索當前節點周圍的八個節點；而JPS算法則采用跳躍規則橫向、縱向、對角探索節點。

圖11 鄰居節點定義示意圖Fig.11 Schematic diagram of neighbor node definition

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

圖12 全局路徑規劃器規劃圖Fig.12 Global path planner plan

為將機器人移動的方向約束為指定方向而非反向規劃，此處設計增加多個局部目標點，迫使機器人抵達后再次規劃后續目標點。如圖13所示，其中左下角箭頭處為機器人當前位姿所在，紅線為兩點間由JPS規劃得到的全局路徑。

圖13 局部目標點示意圖Fig.13 Schematic diagram of local target points

4.2 MPC局部規劃器

針對JPS規劃得到的軌跡，進一步設計MPC局部規劃器用于跟蹤期望軌跡，如圖14為MPC局部規劃器的規劃過程框圖。

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

設預測時域為，控制時域為(≤)，對式多次迭代得到最終的預測模型：

()=()+Δ()

(15)

min=ΔΔ+ΔΔ

(16)

由預測模型[式(15)]可知，若已知當前時刻機器人的狀態量和控制時域內的控制增量，即可預測得到預測時域的系統輸出。

進一步采用平方誤差模型設計目標函數[式(16)]用于迭代得到局部最優解，其中矩陣參量和為權重矩陣。對其做如下處理進行簡化：

(17)

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

Δ≤Δ≤Δ

(18)

由此得到預測時域的控制增量Δ，用于規劃軌跡跟蹤所需的控制量。

5 仿真與實踐

針對上述設計系統，使用Matlab的CarSim模塊進行仿真測試，隨后設計實際機器人系統在室外環境下實驗測試設計方案。

5.1 系統仿真測試

基于CarSim模塊，構建如圖15所示仿真模型。

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

此處模擬真實離散障礙物環境，構建仿真測試。設置機器人期望移動線速度為2 m/s；約束舵機轉向角度小于80°；機器人初始移動時間=0；采樣間隔時間=01 s，如圖16所示。

同時，根據期望軌跡計算機器人在移動時抵達期望軌跡的理論角度并記錄實際控制實現的角度，如圖17所示。0～20 s內，RMSE=7.05。在20～28.3 s，由于23 s時的延遲效應，RMSE=45.261。總體期望值與實際值擬合程度高，系統穩定。

綜上所述，本文中所設計的移動機器人系統穩定性強，能適應于離散障礙物環境下的未知環境探索與導航。

圖16 仿真測試結果圖Fig.16 Simulation test result diagram

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

5.2 實際環境測試

本文中進一步搭建實體移動機器人系統，如圖18所示，并基于ROS框架實現對機器人的軟件系統搭建。

圖18 機器人實物圖Fig.18 Pictures of robot objects

綜合考慮各個傳感器的特性，對其置放位置進行規劃。其中，相機安裝至車輛前端用于采集未知環境的圖像信息；IMU和激光雷達安裝至車輛中部位置，用于檢測車輛姿態信息及采集周圍環境的點云信息；處理器及控制器安裝至車輛后端位置，用于處理環境信息、決策車輛行駛狀態及控制車輛運動；編碼器計米輪安裝至車輛后側用于采集車輛的里程信息。

關鍵傳感器型號如表3所示。

表3 關鍵傳感器型號Table 3 Keysensor models

啟動程序后，機器人將自動依據視覺信息和神經網絡控制器進行自主探索環境，獲取到圖8所示的初始感知地圖后對其進行預處理，進而得到圖10所示的預處理后感知地圖。此后，采用JPS全局規劃器規劃全局路徑，并由MPC局部規劃器進行軌跡跟蹤測試，如圖19所示。實際運行效果表明，系統設計合理且運行穩定。

圖19 實際跟蹤測試結果圖Fig.19 Actual test result picture

6 結論

1) 使用Simulink和CarSim模塊構建車輛模型進行仿真，驗證控制系統的可實施性。

2) 設計并使用目標檢測和神經網絡控制算法，實現車輛自主建圖和路徑規劃。

3) 根據獲得軌跡信息，設計MPC軌跡跟蹤器，在已知環境中有效地跟蹤車輛的參考軌跡。該算法通過仿真測試和實際使用可有效實現車輛的穩定規劃，軌跡跟蹤效果顯著。