基于ROS的室內機器人導航技術研究

劉海平，戰強

(北京航空航天大學 a.中法工程師學院； b. 機械工程及自動化學院, 北京100191)

0 引言

隨著機器人技術的快速發展，越來越多的智能家居走進人們的日常生活。但移動機器人在室內的定位與導航精度的問題仍然是推廣家庭服務機器人所需解決的關鍵點之一。不同機器人平臺因所采用傳感器、處理器等硬件的多樣性，需依靠開發者重復開發控制系統。與此同時，導航系統和相關軟件卻不可重復利用。其次，現代機器人系統開發的規模和復雜程度與日俱增，在這樣的大環境下，WillowGarage公司于2010年公布了開源的機器人操作系統ROS(robotoperatingsystem)。

首先介紹ROS的基礎概念以及導航相關的功能包；然后按照實驗的研究步驟分別介紹了定位方法、機器人運動模型、卡爾曼濾波和機器人控制策略；進而論述實現定位的算法和相關函數；最后分析在Rovio移動機器人平臺上所獲得的測試數據和結果，論證研究方法的有效性和算法優化的思路。

1 機器人操作系統

機器人操作系統(ROS)是一套用于機器人設計與控制的操作系統。ROS不僅提供了豐富的操作功能，如底層設備控制、進程間的消息傳遞，還整合了主流的庫函數，如OpenGL、OpenCV，并且擁有很多傳感器的驅動程序，包括Kinect、激光測距儀等[1]。該系統可概括為以下2個特點：

1) 點對點的通訊方式：每個進程以節點(Node)的形式存在，并且節點可運行分布在網絡各端口的多臺計算機上。節點管理器負責存儲所有節點的主題和服務信息[2]，并在兩個節點間建立連接查詢表。點對點的通訊方式可以提高移動機器人導航過程數據傳輸的效率。

2) 跨編程語言開發：軟件主要使用C++和Python，同時兼容Java、Lisp等編程語言。ROS在編譯時統一定義節點所傳入傳出的消息(Message)結構，并采用中立的接口轉譯給不同的開發語言。節點將消息發送到指定的話題(Topic)從而完成發布。若一個節點對某數據感興趣，它只需訂閱相關的話題即可。另外，節點間也可以通過服務(Service)實現請求和調用。

定位與導航的算法實現過程中使用了ROS提供的開源庫函數：RVIZ可將機器人轉化為可視的三維模型；TF是坐標轉換庫，提供系統中任一個點在坐標系之間坐標轉換的查詢和變換服務。

2 機器人導航技術和控制策略

移動機器人可以自行組織運行和自主規劃，其基礎在于定位精度和導航準確性。通過導航定位可獲得機器人所在的位置、方向，再分析實驗數據，建立運動模型，從而完成路徑的規劃和目標跟蹤。

2.1 定位方法

在導航過程中，定位是實現自主能力的核心問題之一，其功能是獲得機器人相對于坐標系的位置。機器人定位一般可劃分為相對定位和絕對定位。

相對定位主要依靠內部傳感器，包括里程計(odometry)、陀螺儀等。通過測量相對于機器人初始位姿的平面距離和轉動方向，從而估計機器人位置。該方法計算量小[3]，但有累加特性。

絕對定位主要依靠環境標識、路標或匹配地圖進行定位。絕對定位的精度高，但處理速度較慢，設備成本高。

2.2 機器人運動模型

機器人運動模型如圖1所示，將機器人的運動分解為平移與旋轉兩種方式。假設機器人在第i時刻的位姿為{x(i),y(i),θ(i)}，以此為原點建立局部坐標系，以機器人的行進方向作為x軸。假設在下一時刻i+1，機器人相對時刻i的位姿變化量為：{Δx(i),Δy(i),Δθ(i)}，則根據坐標變換公式可知機器人在第i+1時刻的全局坐標系下的位姿{x(i+1),y(i+1),θ(i+1)}為：

圖1 Rovio機器人運動模型

(1)

其中：{Δx(i),Δy(i),Δθ(i)}來自里程計的觀測值，{Ωx(i),Ωy(i),Ωθ(i)}是傳感器噪聲。

2.3 卡爾曼濾波

卡爾曼濾波(kalmanfilter,KF)通過遞歸的方法，迭代更新估計值來減小誤差[4]。KF的優點在于計算過程只需要上一時刻的估計值和當前時刻的觀測值，極大地減少運算內存的消耗。

實驗用一個添加了噪聲干擾的馬爾可夫鏈來表示卡爾曼濾波的動態過程[5](圖2)。采用Rovio位姿作為狀態向量X=(x y θ)，控制向量U=(v Ω)是Rovio的線速度和角速度，Z=(xdyd)是Rovio的坐標觀測值。

圖2 卡爾曼濾波器的結構示意圖

實驗是在規劃路徑下的定位與導航，所以假設整個系統是線性的，其KF的狀態描述方程

X=A×X+B×U+ωk

(2)

A為狀態變換矩陣，B為數據輸入變換矩陣，ω為機器人運動過程的噪聲元素，滿足以下協方差矩陣Q(實驗數據結果)：

ωk～N(0,Q)

(3)

測量方程

Z=H×X+vk

(4)

H為觀測矩陣，v為測量噪聲，滿足以下協方差矩陣R(實驗數據)：

vk～N(0,R)

(5)

(6)

(7)

然后，KF利用對當前時刻系統狀態的觀測值來更新在上一階段的預測值，從而獲得一個平均誤差更小的估計值，測量更新過程如下：

(8)

(9)

PK=(I-KkHk)Rk|k-1

(10)

2.4 控制分層

結合上述運動模型和Rovio機器人實驗平臺，在機器人操作平臺中機器人的控制策略可劃分為以下3個層次：

1) 機器人的電機驅動部分：Rovio控制器接收ROS發送的命令，轉化cmd_vel的速度為輪子的期望速度，對輪子分別進行PID控速。等時間間隔地讀取電機碼盤值，轉化為萬向輪速度再上傳給Rovio控制器，并用航跡推演法計算Rovio當前的速度。

2)ROS與Rovio的通信部分：ROS發出速度指令至Rovio平臺，Rovio將當前實際速度值發布到Odom主題，以提供給其他節點訂閱。

3) 機器人的定位：決策層，具有不同的地圖建模和定位導航策略。這一部分調用ROS相應的程序包，如move_base，AMCL，map_server等。

3 ROS平臺上的定位算法實現

利用ROS分布式框架的特點，在Rovio機器人只設置慣性導航數據采集以及部分預處理的節點，在上位機完成定位、導航以及可視化的節點。上位機和Rovio之間各節點使用無線網絡進行數據通信[6]。

3.1 導航規劃層算法

參考ROS功能包move_base中的局部規劃層和全局規劃層，程序中創建了Nav_to_goal和Nav_points2個節點。

Nav_to_goal初步實現將機器人導航到目標點。算法假設當前位置與目標點之間沒有障礙物。將機器人的速度和轉向控制分為3個步驟：首先調整機器人位姿使之面向目標點；然后驅動機器人接近目標點，用插值法平滑運動過程的速度；到達目標之后旋轉機器人朝向以滿足下一步驟的準備。在這個過程，節點使用tf庫的TransformStamped函數，將Rovio當前的位置轉化為其在target_frame的坐標。

Nav_points提供根據規劃的路徑完成Rovio平臺和ROS之間控制命令的傳輸。算法用geometry_msgs::PoseArray表示規劃的路徑軌跡，并進行RVIZ可視化處理，在模擬的幾何地圖上實時顯示機器人的運動。

3.2 Rovio的檢測與定位

實驗室利用AxisPTZ相機，通過圖像檢測Rovio的像素重心，然后由坐標變換函數轉化為攝像頭姿態(pan,tilt)。通過支持向量機算法間接實現(pan,tilt)和固定坐標系(x,y)的轉換。具體定位步驟如下：

1) 節點detect的作用為檢測Rovio在圖像中的位置：利用OpenCv將圖片采集到ROS中，并根據顏色特征選擇HSV值范圍。然后算法對檢測結果進行濾噪處理，計算檢測所得到的閉環區域重心，作為目標的圖像坐標。然后通過pantiltzoom方程結合攝像頭當前的參數信息，將Rovio的像素坐標轉化為(pan,tilt)坐標，也就是將目標置于攝像頭圖像中心(x/2, y/2)時，其對應攝像頭的橫縱轉角。最后將當前攝像頭檢測到的所有機器人的坐標轉化為(pan,tilt)數組，保存并發布到話題PanTilts。

2) 節點Tracker的作用為控制攝像頭實時跟蹤Rovio。該節點可讀取話題PanTilts的信息并發布一系列控制攝像頭移動的指令。Tracker提供3種模式，跟蹤模式：攝像頭實時跟蹤機器人Rovio的移動，當監控圖像中檢測到多個機器人時，計算攝像頭當前橫縱角坐標與(pan,tilt)的最小差值；自動搜尋模式：控制攝像頭按一定角度范圍和增減規律遍歷；掃描跟蹤模式，攝像頭先按照搜尋模式遍歷，當檢測到Rovio時切換為跟蹤模式。

3) 節點sampler的作用為采集數據樣本，通過機器學習得到(pan,tilt)和(x,y)之間的變換關系。算法引入支持向量機(supportvectormachines,SVM)，并構造最優化超平面從而獲得x和y的預估方程。

x=pred1(pan,tilt)

(11)

y=pred2(pan,tilt)

通過觀察節點Sampler所得到的數據集，其中數據的分布并非線性。SVM通過一個映射函數，將低維的輸入空間Rn映射到高維的特征空間H。實現過程中，本文使用了徑向基核函數(radialbasisfunction)，其中最常用的RBF為高斯核函數:

K(x,y)=exp(-y‖x-y‖2)

(12)

對于一個基于RBF核函數的SVM，其劃分效果是由參數(C,γ)決定， C為懲罰因子，γ為核參數。實驗中用k折交叉驗證法來評估SVM優化結果的推廣能力。

3.3 多目標的聚類與跟蹤

當攝像頭傳遞對應位置的點云信息時候，多目標的點云信息會形成有限個點云集合。聚類分析的目的是：經過圖像檢測的結果為點云圖，從中提取一定數目標點來表示機器人所處位置。

為了結構化點云數據，選取聚類數目以及獲得時間不變性的標注信息，算法在增長型神經元(growingneuralgas，GNG)模型基礎上修改聚類數目判斷的標準，從而避免了增長型神經網絡出現網絡過度冗余的情況，最終獲得結構更為簡單、泛化能力強、學習速度快、性能穩定的聚類算法。增長性神經網絡可以根據信息處理的需要，實時改變神經網絡的拓撲結構與神經元個數[7]。在GNG過程中，新增marker點引入的誤差為：

(13)

(14)

由式(13)和式(14)可以推出

?ω

(15)

將T的選擇添加到GNG算法流程中的判斷增長終止與否的條件，從而優化了GNG算法，提高聚類分析的速度。

4 實驗結果

Rovio機器人是美國WowWee公司發布的一款wifi遙控的移動攝像機器人，配備有640×480的webcam攝像頭以及聲音采集和擴散設備，并具有完整的控制軟件和開發工具。

4.1 卡爾曼濾波

實驗測試過程中以視覺定位的初步聚類結果作為Rovio位姿的觀測值，通過KF多次迭代后使位姿相比于處理前的數據更加精確，誤差更小，如圖3所示。從圖3可以觀察出Rovio的轉角預測與真實值之間的差值由初始的劇烈波動到趨于穩定，證明了卡爾曼濾波對隱藏變量的預估作用。

圖3 KF的預測值與真實值的對比圖

4.2 SVM構造最優化超平面

實驗通過采集樣本來訓練SVM以構造最優化超平面從而獲得x和y的預測方程，如圖4、圖5所示。

圖4 x坐標的預估方程

圖5 y坐標的預估方程

在該實驗中，算法使用RBF核函數，其中C=120，gamma=0.02。用10組真實數據交叉驗證得到經驗誤差為x=0.012, y=0.009；真實誤差值為ΔE= 1.21。

4.3 多目標的聚類

利用優化后的GNG算法對3個Rovio機器人進行檢測和跟蹤。相比原GNG算法，在ROS平臺上雖然仍不能達到實時快速跟蹤，但聚類速度有著明顯的提升。實驗結果認可了GNG算法通過對判斷條件的改善可加快聚類速度的思路，如圖6所示。

圖6 優化后GNG算法的聚類效果

5 結語

文中基于機器人操作系統對室內機器人的定位和導航技術進行研究和實現。利用ROS提供的開源功能包，在Rovio平臺實現定位導航算法。該算法避免傳統方法所需要的大量計算，提高了定位和導航效率。實驗結果證明了利用SVM研究攝像機定標問題的可行性。優化判斷標準的GNG算法對數據點云聚類，在保證聚類效果的前提下，取得了速度上的提升。最后卡爾曼濾波的應用也得到了實驗結果積極的反饋，下一步的工作可以研究融合多數據來源提高定位精度。