劉海平，戰(zhàn)強(qiáng)

(北京航空航天大學(xué) a.中法工程師學(xué)院； b. 機(jī)械工程及自動(dòng)化學(xué)院, 北京100191)

0 引言

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

首先介紹ROS的基礎(chǔ)概念以及導(dǎo)航相關(guān)的功能包；然后按照實(shí)驗(yàn)的研究步驟分別介紹了定位方法、機(jī)器人運(yùn)動(dòng)模型、卡爾曼濾波和機(jī)器人控制策略；進(jìn)而論述實(shí)現(xiàn)定位的算法和相關(guān)函數(shù)；最后分析在Rovio移動(dòng)機(jī)器人平臺(tái)上所獲得的測試數(shù)據(jù)和結(jié)果，論證研究方法的有效性和算法優(yōu)化的思路。

1 機(jī)器人操作系統(tǒng)

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

1) 點(diǎn)對(duì)點(diǎn)的通訊方式：每個(gè)進(jìn)程以節(jié)點(diǎn)(Node)的形式存在，并且節(jié)點(diǎn)可運(yùn)行分布在網(wǎng)絡(luò)各端口的多臺(tái)計(jì)算機(jī)上。節(jié)點(diǎn)管理器負(fù)責(zé)存儲(chǔ)所有節(jié)點(diǎn)的主題和服務(wù)信息[2]，并在兩個(gè)節(jié)點(diǎn)間建立連接查詢表。點(diǎn)對(duì)點(diǎn)的通訊方式可以提高移動(dòng)機(jī)器人導(dǎo)航過程數(shù)據(jù)傳輸?shù)男省?/p>

2) 跨編程語言開發(fā)：軟件主要使用C++和Python，同時(shí)兼容Java、Lisp等編程語言。ROS在編譯時(shí)統(tǒng)一定義節(jié)點(diǎn)所傳入傳出的消息(Message)結(jié)構(gòu)，并采用中立的接口轉(zhuǎn)譯給不同的開發(fā)語言。節(jié)點(diǎn)將消息發(fā)送到指定的話題(Topic)從而完成發(fā)布。若一個(gè)節(jié)點(diǎn)對(duì)某數(shù)據(jù)感興趣，它只需訂閱相關(guān)的話題即可。另外，節(jié)點(diǎn)間也可以通過服務(wù)(Service)實(shí)現(xiàn)請(qǐng)求和調(diào)用。

定位與導(dǎo)航的算法實(shí)現(xiàn)過程中使用了ROS提供的開源庫函數(shù)：RVIZ可將機(jī)器人轉(zhuǎn)化為可視的三維模型；TF是坐標(biāo)轉(zhuǎn)換庫，提供系統(tǒng)中任一個(gè)點(diǎn)在坐標(biāo)系之間坐標(biāo)轉(zhuǎn)換的查詢和變換服務(wù)。

2 機(jī)器人導(dǎo)航技術(shù)和控制策略

移動(dòng)機(jī)器人可以自行組織運(yùn)行和自主規(guī)劃，其基礎(chǔ)在于定位精度和導(dǎo)航準(zhǔn)確性。通過導(dǎo)航定位可獲得機(jī)器人所在的位置、方向，再分析實(shí)驗(yàn)數(shù)據(jù)，建立運(yùn)動(dòng)模型，從而完成路徑的規(guī)劃和目標(biāo)跟蹤。

2.1 定位方法

在導(dǎo)航過程中，定位是實(shí)現(xiàn)自主能力的核心問題之一，其功能是獲得機(jī)器人相對(duì)于坐標(biāo)系的位置。機(jī)器人定位一般可劃分為相對(duì)定位和絕對(duì)定位。

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

絕對(duì)定位主要依靠環(huán)境標(biāo)識(shí)、路標(biāo)或匹配地圖進(jìn)行定位。絕對(duì)定位的精度高，但處理速度較慢，設(shè)備成本高。

2.2 機(jī)器人運(yùn)動(dòng)模型

機(jī)器人運(yùn)動(dòng)模型如圖1所示，將機(jī)器人的運(yùn)動(dòng)分解為平移與旋轉(zhuǎn)兩種方式。假設(shè)機(jī)器人在第i時(shí)刻的位姿為{x(i),y(i),θ(i)}，以此為原點(diǎn)建立局部坐標(biāo)系，以機(jī)器人的行進(jìn)方向作為x軸。假設(shè)在下一時(shí)刻i+1，機(jī)器人相對(duì)時(shí)刻i的位姿變化量為：{Δx(i),Δy(i),Δθ(i)}，則根據(jù)坐標(biāo)變換公式可知機(jī)器人在第i+1時(shí)刻的全局坐標(biāo)系下的位姿{(lán)x(i+1),y(i+1),θ(i+1)}為：

圖1 Rovio機(jī)器人運(yùn)動(dòng)模型

(1)

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

2.3 卡爾曼濾波

卡爾曼濾波(kalmanfilter,KF)通過遞歸的方法，迭代更新估計(jì)值來減小誤差[4]。KF的優(yōu)點(diǎn)在于計(jì)算過程只需要上一時(shí)刻的估計(jì)值和當(dāng)前時(shí)刻的觀測值，極大地減少運(yùn)算內(nèi)存的消耗。

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

圖2 卡爾曼濾波器的結(jié)構(gòu)示意圖

實(shí)驗(yàn)是在規(guī)劃路徑下的定位與導(dǎo)航，所以假設(shè)整個(gè)系統(tǒng)是線性的，其KF的狀態(tài)描述方程

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

(2)

A為狀態(tài)變換矩陣，B為數(shù)據(jù)輸入變換矩陣，ω為機(jī)器人運(yùn)動(dòng)過程的噪聲元素，滿足以下協(xié)方差矩陣Q(實(shí)驗(yàn)數(shù)據(jù)結(jié)果)：

ωk～N(0,Q)

(3)

測量方程

Z=H×X+vk

(4)

H為觀測矩陣，v為測量噪聲，滿足以下協(xié)方差矩陣R(實(shí)驗(yàn)數(shù)據(jù))：

vk～N(0,R)

(5)

(6)

(7)

然后，KF利用對(duì)當(dāng)前時(shí)刻系統(tǒng)狀態(tài)的觀測值來更新在上一階段的預(yù)測值，從而獲得一個(gè)平均誤差更小的估計(jì)值，測量更新過程如下：

(8)

(9)

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

(10)

2.4 控制分層

結(jié)合上述運(yùn)動(dòng)模型和Rovio機(jī)器人實(shí)驗(yàn)平臺(tái)，在機(jī)器人操作平臺(tái)中機(jī)器人的控制策略可劃分為以下3個(gè)層次：

1) 機(jī)器人的電機(jī)驅(qū)動(dòng)部分：Rovio控制器接收ROS發(fā)送的命令，轉(zhuǎn)化cmd_vel的速度為輪子的期望速度，對(duì)輪子分別進(jìn)行PID控速。等時(shí)間間隔地讀取電機(jī)碼盤值，轉(zhuǎn)化為萬向輪速度再上傳給Rovio控制器，并用航跡推演法計(jì)算Rovio當(dāng)前的速度。

2)ROS與Rovio的通信部分：ROS發(fā)出速度指令至Rovio平臺(tái)，Rovio將當(dāng)前實(shí)際速度值發(fā)布到Odom主題，以提供給其他節(jié)點(diǎn)訂閱。

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

3 ROS平臺(tái)上的定位算法實(shí)現(xiàn)

利用ROS分布式框架的特點(diǎn)，在Rovio機(jī)器人只設(shè)置慣性導(dǎo)航數(shù)據(jù)采集以及部分預(yù)處理的節(jié)點(diǎn)，在上位機(jī)完成定位、導(dǎo)航以及可視化的節(jié)點(diǎn)。上位機(jī)和Rovio之間各節(jié)點(diǎn)使用無線網(wǎng)絡(luò)進(jìn)行數(shù)據(jù)通信[6]。

3.1 導(dǎo)航規(guī)劃層算法

參考ROS功能包move_base中的局部規(guī)劃層和全局規(guī)劃層，程序中創(chuàng)建了Nav_to_goal和Nav_points2個(gè)節(jié)點(diǎn)。

Nav_to_goal初步實(shí)現(xiàn)將機(jī)器人導(dǎo)航到目標(biāo)點(diǎn)。算法假設(shè)當(dāng)前位置與目標(biāo)點(diǎn)之間沒有障礙物。將機(jī)器人的速度和轉(zhuǎn)向控制分為3個(gè)步驟：首先調(diào)整機(jī)器人位姿使之面向目標(biāo)點(diǎn)；然后驅(qū)動(dòng)機(jī)器人接近目標(biāo)點(diǎn)，用插值法平滑運(yùn)動(dòng)過程的速度；到達(dá)目標(biāo)之后旋轉(zhuǎn)機(jī)器人朝向以滿足下一步驟的準(zhǔn)備。在這個(gè)過程，節(jié)點(diǎn)使用tf庫的TransformStamped函數(shù)，將Rovio當(dāng)前的位置轉(zhuǎn)化為其在target_frame的坐標(biāo)。

Nav_points提供根據(jù)規(guī)劃的路徑完成Rovio平臺(tái)和ROS之間控制命令的傳輸。算法用geometry_msgs::PoseArray表示規(guī)劃的路徑軌跡，并進(jìn)行RVIZ可視化處理，在模擬的幾何地圖上實(shí)時(shí)顯示機(jī)器人的運(yùn)動(dòng)。

3.2 Rovio的檢測與定位

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

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

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

3) 節(jié)點(diǎn)sampler的作用為采集數(shù)據(jù)樣本，通過機(jī)器學(xué)習(xí)得到(pan,tilt)和(x,y)之間的變換關(guān)系。算法引入支持向量機(jī)(supportvectormachines,SVM)，并構(gòu)造最優(yōu)化超平面從而獲得x和y的預(yù)估方程。

x=pred1(pan,tilt)

(11)

y=pred2(pan,tilt)

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

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

(12)

對(duì)于一個(gè)基于RBF核函數(shù)的SVM，其劃分效果是由參數(shù)(C,γ)決定， C為懲罰因子，γ為核參數(shù)。實(shí)驗(yàn)中用k折交叉驗(yàn)證法來評(píng)估SVM優(yōu)化結(jié)果的推廣能力。

3.3 多目標(biāo)的聚類與跟蹤

當(dāng)攝像頭傳遞對(duì)應(yīng)位置的點(diǎn)云信息時(shí)候，多目標(biāo)的點(diǎn)云信息會(huì)形成有限個(gè)點(diǎn)云集合。聚類分析的目的是：經(jīng)過圖像檢測的結(jié)果為點(diǎn)云圖，從中提取一定數(shù)目標(biāo)點(diǎn)來表示機(jī)器人所處位置。

為了結(jié)構(gòu)化點(diǎn)云數(shù)據(jù)，選取聚類數(shù)目以及獲得時(shí)間不變性的標(biāo)注信息，算法在增長型神經(jīng)元(growingneuralgas，GNG)模型基礎(chǔ)上修改聚類數(shù)目判斷的標(biāo)準(zhǔn)，從而避免了增長型神經(jīng)網(wǎng)絡(luò)出現(xiàn)網(wǎng)絡(luò)過度冗余的情況，最終獲得結(jié)構(gòu)更為簡單、泛化能力強(qiáng)、學(xué)習(xí)速度快、性能穩(wěn)定的聚類算法。增長性神經(jīng)網(wǎng)絡(luò)可以根據(jù)信息處理的需要，實(shí)時(shí)改變神經(jīng)網(wǎng)絡(luò)的拓?fù)浣Y(jié)構(gòu)與神經(jīng)元個(gè)數(shù)[7]。在GNG過程中，新增marker點(diǎn)引入的誤差為：

(13)

(14)

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

?ω

(15)

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

4 實(shí)驗(yàn)結(jié)果

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

4.1 卡爾曼濾波

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

圖3 KF的預(yù)測值與真實(shí)值的對(duì)比圖

4.2 SVM構(gòu)造最優(yōu)化超平面

實(shí)驗(yàn)通過采集樣本來訓(xùn)練SVM以構(gòu)造最優(yōu)化超平面從而獲得x和y的預(yù)測方程，如圖4、圖5所示。

圖4 x坐標(biāo)的預(yù)估方程

圖5 y坐標(biāo)的預(yù)估方程

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

4.3 多目標(biāo)的聚類

利用優(yōu)化后的GNG算法對(duì)3個(gè)Rovio機(jī)器人進(jìn)行檢測和跟蹤。相比原GNG算法，在ROS平臺(tái)上雖然仍不能達(dá)到實(shí)時(shí)快速跟蹤，但聚類速度有著明顯的提升。實(shí)驗(yàn)結(jié)果認(rèn)可了GNG算法通過對(duì)判斷條件的改善可加快聚類速度的思路，如圖6所示。

圖6 優(yōu)化后GNG算法的聚類效果

5 結(jié)語

文中基于機(jī)器人操作系統(tǒng)對(duì)室內(nèi)機(jī)器人的定位和導(dǎo)航技術(shù)進(jìn)行研究和實(shí)現(xiàn)。利用ROS提供的開源功能包，在Rovio平臺(tái)實(shí)現(xiàn)定位導(dǎo)航算法。該算法避免傳統(tǒng)方法所需要的大量計(jì)算，提高了定位和導(dǎo)航效率。實(shí)驗(yàn)結(jié)果證明了利用SVM研究攝像機(jī)定標(biāo)問題的可行性。優(yōu)化判斷標(biāo)準(zhǔn)的GNG算法對(duì)數(shù)據(jù)點(diǎn)云聚類，在保證聚類效果的前提下，取得了速度上的提升。最后卡爾曼濾波的應(yīng)用也得到了實(shí)驗(yàn)結(jié)果積極的反饋，下一步的工作可以研究融合多數(shù)據(jù)來源提高定位精度。