機器人室內運動軌跡修正控制算法

彭益堂，朱敦堯，唐 健，毛大智

(武漢大學衛星導航定位技術研究中心，湖北 武漢 430079)

移動機器人兼具環境信息感知、動態決策與智能規劃、行為精確控制與執行、自主導航與定位等復雜功能于一體[1]，是當前研究的熱點，在軍事、工業、農業、服務等行業具有巨大的應用潛力。目前，多數移動機器人在室內復雜環境中運動時，僅依賴里程計和聲吶等傳感器來確定自身位置，機器人的實際運動軌跡和預設軌跡會出現較大偏差，嚴重影響了機器人的深層次應用開發。本文針對室內機器人運動軌跡不精確問題提出了一種解決方案。

對機器人運動軌跡偏差的限制可從硬件和軟件兩方面考慮。從硬件角度看，可以采用高精度傳感器和高可靠性的運動控制系統來提升機器人導航的精確性；從軟件角度看，可以改進機器人導航定位算法，得到精確的位姿參數反饋給運動控制系統。本文主要從軟件算法的角度修正機器人運動軌跡偏差：首先，將LiDAR傳感器信息轉化為正確的運動控制命令，實時地反饋到運動控制端以修正機器人的運動軌跡偏差；其次，通過大量試驗優化算法中的軌跡修正模型參數，進一步提升機器人室內運動軌跡的精確度。

1 室內測圖運動軌跡控制算法

1.1 機器人室內測圖與定位模型

本文在機器人控制平臺上安裝LiDAR傳感器設備，利用開源的Hector SLAM算法包進行調試，搭建SLAM功能模塊，實現移動機器人室內未知環境下的同時定位與地圖創建，以便對機器人坐標定位信息進行采集。

1.1.1 運動模型

為準確描述移動機器人的運動，需要將實際的運動軌跡參數化，利用數據來表示機器人在環境中的位置和姿態信息，并進行調控，如圖1所示。

圖1 機器人運動模型[2]

設k時刻機器人運動狀態為Xk=(xk,yk,θk)，則基于控制命令的運動模型可表示為

(1)

式中，V為速度；W為角速度；U為噪聲項，表示輪子運動中的機械誤差或環境噪聲；同時有機器人速度

(2)

式中，輪子半徑為r；輪距為2l；輪子速度為φ。

1.1.2 SLAM模塊

SLAM功能模塊能在未知環境中獲取機器人的實時位姿信息并創建環境地圖，具體處理過程如圖2所示。首先，傳感器對環境信息進行掃描，并對觀測量進行信息處理，對采集到的點云和圖像數據進行加工；其次，需要進行特征匹配，在濾波器算法中進行優化，進而呈現出移動機器人在局部環境中的運動軌跡和創建的地圖信息；最后，通過SLAM功能模塊輸出的機器人運動軌跡和地圖信息，可以通過軌跡修正算法來調整機器人運動控制狀態。

具體環境中，將LiDAR安置在機器人面板上對走廊地圖進行掃描時，可以看到SLAM地圖創建和真實環境的對比情況，如圖3、圖4所示。

圖2 SLAM框架

圖3 SLAM真實測試環境

圖4 SLAM地圖創建結果

激光測距儀掃描環境信息的過程中，觀測量是環境路標和機器人之間的距離和方位角，設路標點為i，傳感器位置為s點，這時系統的觀測模型為

(3)

由于傳感器的位置與機器人的位置之間可相互轉換，借此可得到觀測值Z和機器人位置之間的函數關系，即傳感器觀測模型。在觀測過程中，觀測噪聲服從高斯分布，也可以把上述觀測量轉換到極坐標系中。

1.2 PID軌跡修正控制算法

PID控制(proportion,integral,differential)是比例-積分-微分控制器的簡稱，分為P(比例單元)、I(積分單元)和D(微分單元)3部分，主要適用于線性反饋控制系統。在機器人軌跡控制實際應用中，控制器將傳感器收集到的地圖和定位數據與預期參考值進行比較，然后把差值傳送到輸入控制端，用于求解所期望的輸出數據，新的輸入能夠保持系統參數向標準值的方向轉變。

PID回路包括3部分：傳感器測量信息、控制器決策信息、輸出設備單元。在回路中利用需求結果和測量結果的差值來計算系統的糾正值，再根據糾正值，做出相應決策，以消除誤差。在控制回路中，消除糾正值的含義是指清除當前的誤差、平均過去的誤差或改變未來的誤差。

如圖5所示，安置在機器人面板上的激光測距儀信息就是PID控制結構[3]中的反饋信息源，利用信息源做出決策信息是運動控制的關鍵部分，本文在軌跡修正決策中采取了PI閉環控制算法，利用激光雷達對機器人的定位輸出對機器人的運動狀態進行實時的反饋式調控。

圖5 反饋控制結構

在機器人的軌跡修正過程中主要采用PI控制算法，具體內容分為以下兩部分：

1.2.1 方位角修正原理

(1) 轉向設置：當機器人偏移預設直線軌跡時，機器人的偏移方向通過激光雷達SLAM輸出坐標信息可判斷。

(2) 轉動角度：轉動幅度的大小不是任意值，根據機器人的硬件特性和多次試驗測試模擬出最佳的轉向參數，對應不同的坐標閾值分別設定不同的轉向角度，才能保證修正軌跡的精確性。設機器人橫軸上偏離的距離為d，單位為m；運動速度為v,單位為m/s；調整的角度為θ,單位為度。經過試驗測試總結，有如下調整關系

(4)

1.2.2 距離修正原理

(1) 方向設置：一般情況下機器人在行進過程中不能到達指定位置，即是積分式的修正控制模式。由機器人累計的距離增量和預設距離的差值可判斷機器人修正后的運動方向。

(2) 修正距離：即對機器人SLAM功能模塊輸出的距離增量和預設距離的差值進行補償。

2 軌跡修正算法測試

機器人典型的運動軌跡分為直線運動和曲線運動，曲線運動可以看作直線運動的疊加，因此在機器人運動學的研究領域中主要考察機器人直線運動和回環運動的軌跡準確性，本文針對這兩種典型的運動軌跡設計了相應的試驗，對軌跡修正算法的效果進行分析比較。

2.1 直線軌跡測試

(1) 直線行走測試:改變軌跡修正算法中的方位角調整參數，對比同等條件下，機器人直線運動10 m的修正效果，如圖6和圖7所示。

圖6 輕微修正參數

圖7 較佳修正參數

對比分析可以看到，修正參數經過調整后，對機器人運動軌跡的漂移有很好的限制作用，優化后的修正參數能夠將機器人運動軌跡限制到厘米級別。

(2) 控制移動機器人在直線上來回行走5 m距離，比較修正前后的軌跡，結果如圖8和圖9所示。

圖8 修正前后直線運動軌跡對比

圖9 修正前后運動軌跡放大對比

圖9是圖8中機器人運動軌跡終點處放大部分。分析圖9可知，進行修正后的機器人運動軌跡偏差能夠有效收斂，未經修正的機器人運動軌跡一旦發生漂移就會不斷累加。

2.2 回環路線測試

為了檢測算法修正的穩定性，設計了典型的閉環檢測[4]試驗：控制移動機器人在室內沿墻行走，軌跡為長方形，本次試驗設置在走廊環境，長方形軌跡的長4 m，寬為0.9 m，具體模型如圖10所示。

圖10 回環測試軌跡模型

試驗結果分析可知，在正常的室內環境下，進行軌跡修正和不進行軌跡修正時，機器人按照預設路線的運動軌跡，可以看出軌跡修正算法的可行性。

如圖11所示，試驗表明修正后的Y軸平均誤差在5 cm，未修正Y軸平均誤差28 cm。顯然，機器人運動軌跡偏差經過反饋控制修正后，可得到有效控制，僅依靠里程計定位的機器人運動軌跡誤差在漂移后會不斷累加，通過LiDAR傳感器信息的約束能夠收斂到厘米級別，也驗證了軌跡修正算法的可行性和可靠性。

圖11 機器人無人狀態下室內回環測試

3 結 語

本文針對機器人運動軌跡偏差設計了一套軌跡修正算法，定性和定量地分析了機器人軌跡修正的數學模型和修正參數。通過大量的試驗數據構建了機器人軌跡修正控制的經驗模型，得到了可靠的軌跡修正控制參數。分析軌跡修正后的多個試驗結果可知，對移動機器人室內測圖的預設軌跡進行PID軌跡控制算法修正后，機器人室內運動過程中能夠實時調整自己的運動狀態，不需要附加人為的控制命令約束。另外，進行軌跡修正算法控制的移動機器人能夠按照預設路線進行準確移動，具有很高的穩定性，在室內運動過程中能夠達到預期的精度要求，經過大量試驗得到的軌跡修正控制模型效果明顯，驗證了本套機器人運動軌跡偏差修正方案的可行性和可靠性。

[1] 李群明,熊蓉,褚健.室內自主移動機器人定位方法研究綜述[J].機器人,2003,25(6):560-567,573.

[2] 王鴻鵬.復雜環境下輪式自主移動機器人定位與運動控制研究[D].天津：南開大學,2009.

[3] 董剛.智能小車運動控制系統研制[D].西安：西安科技大學,2009.

[4] 季秀才,鄭志強,張輝.SLAM問題中機器人定位誤差分析與控制[J].自動化學報,2008(3):323-330.

[5] CAO Zhengcai,ZHAO Yingtao,WU Qidi.Adaptive Trajectory Tracking Control for a Nonholonomic Mobile Robot[J].Chinese Journal of Mechanical Engineering，2011(4):546-552.

[6] NILSSON N J.A Mobile Automation an Application of Artificial Intelligence Techniques[C]∥International Joint Conference on Artificial Intelligence.Las Vegas:Morgan Kaufmann Publishers，1969：509-520.

[7] 徐曙.基于SLAM的移動機器人導航系統研究[D].武漢：華中科技大學,2014.

[8] 殷波.移動機器人同時定位與地圖創建方法研究[D].青島：中國海洋大學,2006.

[9] 徐則中.移動機器人的同時定位和地圖構建[D].杭州：浙江大學,2004.

[10] 宋春林.基于激光測距儀的移動機器人SLAM研究與實現[D].哈爾濱：哈爾濱工業大學,2014.

[11] 周華.多傳感器融合技術在移動機器人定位中的應用研究[D].武漢：武漢理工大學,2009.

[12] DISSANAYAKE G，NEWMAN P，CLARK S，et al.A Solution to the Simultaneous Localization and Map Building (SLAM) Problem [J].IEEE Transactions on Robotics and automation,2001，17(3)：229-241.

[13] 王璐，蔡自興．未知環境中移動機器人并發建圖與定位(CML)的研究進展[J]．機器人，2004，26(4)：380-384．