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

(武漢大學衛(wèi)星導(dǎo)航定位技術(shù)研究中心，湖北 武漢 430079)

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

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

1 室內(nèi)測圖運動軌跡控制算法

1.1 機器人室內(nèi)測圖與定位模型

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

1.1.1 運動模型

為準確描述移動機器人的運動，需要將實際的運動軌跡參數(shù)化，利用數(shù)據(jù)來表示機器人在環(huán)境中的位置和姿態(tài)信息，并進行調(diào)控，如圖1所示。

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

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

(1)

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

(2)

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

1.1.2 SLAM模塊

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

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

圖2 SLAM框架

圖3 SLAM真實測試環(huán)境

圖4 SLAM地圖創(chuàng)建結(jié)果

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

(3)

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

1.2 PID軌跡修正控制算法

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

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

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

圖5 反饋控制結(jié)構(gòu)

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

1.2.1 方位角修正原理

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

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

(4)

1.2.2 距離修正原理

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

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

2 軌跡修正算法測試

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

2.1 直線軌跡測試

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

圖6 輕微修正參數(shù)

圖7 較佳修正參數(shù)

對比分析可以看到，修正參數(shù)經(jīng)過調(diào)整后，對機器人運動軌跡的漂移有很好的限制作用，優(yōu)化后的修正參數(shù)能夠?qū)C器人運動軌跡限制到厘米級別。

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

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

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

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

2.2 回環(huán)路線測試

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

圖10 回環(huán)測試軌跡模型

試驗結(jié)果分析可知，在正常的室內(nèi)環(huán)境下，進行軌跡修正和不進行軌跡修正時，機器人按照預(yù)設(shè)路線的運動軌跡，可以看出軌跡修正算法的可行性。

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

圖11 機器人無人狀態(tài)下室內(nèi)回環(huán)測試

3 結(jié) 語

本文針對機器人運動軌跡偏差設(shè)計了一套軌跡修正算法，定性和定量地分析了機器人軌跡修正的數(shù)學模型和修正參數(shù)。通過大量的試驗數(shù)據(jù)構(gòu)建了機器人軌跡修正控制的經(jīng)驗?zāi)Ｐ?，得到了可靠的軌跡修正控制參數(shù)。分析軌跡修正后的多個試驗結(jié)果可知，對移動機器人室內(nèi)測圖的預(yù)設(shè)軌跡進行PID軌跡控制算法修正后，機器人室內(nèi)運動過程中能夠?qū)崟r調(diào)整自己的運動狀態(tài)，不需要附加人為的控制命令約束。另外，進行軌跡修正算法控制的移動機器人能夠按照預(yù)設(shè)路線進行準確移動，具有很高的穩(wěn)定性，在室內(nèi)運動過程中能夠達到預(yù)期的精度要求，經(jīng)過大量試驗得到的軌跡修正控制模型效果明顯，驗證了本套機器人運動軌跡偏差修正方案的可行性和可靠性。

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

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

[3] 董剛.智能小車運動控制系統(tǒng)研制[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ǎo)航系統(tǒng)研究[D].武漢：華中科技大學,2014.

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

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

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

[11] 周華.多傳感器融合技術(shù)在移動機器人定位中的應(yīng)用研究[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] 王璐，蔡自興．未知環(huán)境中移動機器人并發(fā)建圖與定位(CML)的研究進展[J]．機器人，2004，26(4)：380-384．