基于單目攝像頭的人形機(jī)器人實(shí)時(shí)導(dǎo)航系統(tǒng)

王軍華，魏衡華（中國科學(xué)技術(shù)大學(xué)　自動(dòng)化系，安徽　合肥　230026）

基于單目攝像頭的人形機(jī)器人實(shí)時(shí)導(dǎo)航系統(tǒng)

王軍華，魏衡華
（中國科學(xué)技術(shù)大學(xué)自動(dòng)化系，安徽合肥230026）

提出一種用于室內(nèi)環(huán)境下人形機(jī)器人的實(shí)時(shí)導(dǎo)航系統(tǒng)。首先基于顏色特征，把單目圖像的像素點(diǎn)分類為地面和障礙物，并對(duì)各個(gè)障礙物的像素點(diǎn)進(jìn)行聚類，以選擇出它們最底部的像素點(diǎn)作為特征點(diǎn)。然后基于SVM（Support Vector Machine）算法，構(gòu)造攝像頭中實(shí)際點(diǎn)和像素點(diǎn)坐標(biāo)的映射關(guān)系，并用均值場理論輔助SVM的學(xué)習(xí)過程。根據(jù)映射關(guān)系和特征像素點(diǎn)，就可以估計(jì)出障礙物與機(jī)器人的距離。最后根據(jù)距離信息，在離散化的運(yùn)動(dòng)空間中，一步一步規(guī)劃出機(jī)器人的行走路徑。在人形機(jī)器人DARWIN上對(duì)提出的方法進(jìn)行了實(shí)現(xiàn)，實(shí)驗(yàn)結(jié)果顯示了該方法的有效性。

人形機(jī)器人；導(dǎo)航；顏色信息；支持向量機(jī)；路徑規(guī)劃

0　引言

近年來，研究人員對(duì)人形機(jī)器人開展了大量研究工作，并取得了巨大進(jìn)步。人形機(jī)器人的機(jī)械結(jié)構(gòu)使得它們更適合人類的生活環(huán)境，比如家里和辦公室等室內(nèi)環(huán)境。而人形機(jī)器人能自由行動(dòng)的關(guān)鍵在于其自主導(dǎo)航能力。人形機(jī)器人的自主導(dǎo)航涉及很多基礎(chǔ)研究領(lǐng)域，比如計(jì)算機(jī)視覺、人工智能、運(yùn)動(dòng)規(guī)劃等。

目前，已有一些針對(duì)不同硬件平臺(tái)的人形機(jī)器人的自主導(dǎo)航方法。陳衛(wèi)東等人［1］將障礙物和目標(biāo)的位置信息模糊化，建立模糊規(guī)則并對(duì)其求解以實(shí)現(xiàn)自主導(dǎo)航；CHESTNUTT J等人［2］為Honda ASIMO人形機(jī)器人設(shè)計(jì)了腳步規(guī)劃器，基于有限的狀態(tài)獨(dú)立行為，使用A*算法搜索最優(yōu)腳步序列；ZANNATHA J M等人［3］設(shè)計(jì)了人形足球機(jī)器人的視覺自定位系統(tǒng)，通過圖像處理算法及幾何分析，進(jìn)行3D單目重建，估計(jì)機(jī)器人在球場內(nèi)的位置和方向；WANG Y T等人［4］實(shí)現(xiàn)了人形機(jī)器人的定位和建圖，其使用SURF進(jìn)行圖像特征檢測(cè)和選擇，并基于擴(kuò)展的卡爾曼濾波繼續(xù)圖像深度計(jì)算和特征狀態(tài)估計(jì)；BALTES J等人［5］以視覺信息作為反饋，使用粒子濾波對(duì)機(jī)器人定位，據(jù)此規(guī)劃出機(jī)器人的行走路徑。

前人的工作中較多采用的是雙目視覺系統(tǒng)，或者配合聲納等設(shè)備，雖然這樣能使測(cè)量和控制更加精準(zhǔn)，但機(jī)器人的體積及質(zhì)量往往較大。由于單目攝像頭成本較低、結(jié)構(gòu)簡單，越來越多的機(jī)器人用其作為自主導(dǎo)航檢測(cè)系統(tǒng)的傳感器。本文針對(duì)配置一個(gè)攝像頭的人形機(jī)器人以及在室內(nèi)地面行走的任務(wù)，設(shè)計(jì)了一個(gè)實(shí)用的實(shí)時(shí)導(dǎo)航系統(tǒng)，此系統(tǒng)使機(jī)器人在實(shí)驗(yàn)環(huán)境中能夠從當(dāng)前位置到達(dá)預(yù)定的目的地。本文的工作為基于單目的人形機(jī)器人在更加復(fù)雜的環(huán)境中順利行走奠定了基礎(chǔ)。

1　系統(tǒng)概述

此導(dǎo)航系統(tǒng)分為3部分，分別為障礙物的檢測(cè)、障礙物的定位和機(jī)器人路徑規(guī)劃。

檢測(cè)障礙物時(shí)，基于顏色特征，把單目圖像中的所有像素點(diǎn)分類為地面和障礙物。對(duì)屬于障礙物的像素點(diǎn)進(jìn)行聚類，以區(qū)分開不同的障礙物。選擇每個(gè)障礙物最底部的像素點(diǎn)作為此障礙物的特征點(diǎn)，用于估計(jì)此障礙物和機(jī)器人之間的相對(duì)位置。

定位障礙物時(shí)，先用SVM（Support Vector Machine）算法構(gòu)造攝像頭中實(shí)際點(diǎn)和像素點(diǎn)坐標(biāo)的映射關(guān)系，并用均值場理論來輔助SVM的學(xué)習(xí)過程。基于此映射關(guān)系，還原出特征像素點(diǎn)的3D信息，就可以估計(jì)出障礙物與機(jī)器人的距離。

根據(jù)機(jī)器人的行走特點(diǎn)，離散化機(jī)器人的運(yùn)動(dòng)空間，然后根據(jù)距離信息，一步一步規(guī)劃出機(jī)器人的行走路徑。

2　系統(tǒng)設(shè)計(jì)描述

2.1障礙物檢測(cè)

機(jī)器人在自主運(yùn)動(dòng)的過程中，需要避開地面上的障礙物。可對(duì)機(jī)器人行走的室內(nèi)地面環(huán)境進(jìn)行一些合理假設(shè)：地面平坦且顏色一致；障礙物都在地面上沒有懸空；障礙物的顏色與地面的顏色不同；障礙物的形狀比較簡單且有規(guī)則。

本文選擇顏色特征作為識(shí)別障礙物的主要線索。使用機(jī)器人的攝像頭對(duì)機(jī)器人行走的地面進(jìn)行多次隨機(jī)采樣，以獲取較完備的地面參考信息。將攝像頭輸出的RGB顏色空間的圖像轉(zhuǎn)換到HIS顏色空間，計(jì)算出所有地面采樣圖像的平均色調(diào)和強(qiáng)度直方圖，并用簡單均值濾波器進(jìn)行濾波處理。

機(jī)器人行走時(shí)，用5×5高斯濾波器對(duì)輸入圖像進(jìn)行濾波，以降低圖像中的噪聲。將過濾后的圖像轉(zhuǎn)換到HIS顏色空間。

將圖像中的每個(gè)像素的色調(diào)、強(qiáng)度與參考數(shù)據(jù)相比較，如果滿足以下兩個(gè)條件之一，則該像素被判定屬于障礙物：（1）該像素色調(diào)值在直方圖中的頻數(shù)低于某個(gè)閾值；（2）該像素強(qiáng)度值在直方圖中的頻數(shù)低于某個(gè)閾值。

基于前文所描述的方法，對(duì)圖1（a）中地面上的圓柱體進(jìn)行識(shí)別，識(shí)別結(jié)果如圖1（b）為所示，此實(shí)驗(yàn)驗(yàn)證了障礙物檢測(cè)方法的有效性。

圖1　障礙物識(shí)別結(jié)果示例

2.2障礙物定位

識(shí)別出視野內(nèi)地面上的障礙物后，還需要弄清障礙物和機(jī)器人的相對(duì)位置，才能進(jìn)一步確定機(jī)器人的行走方向。根據(jù)前文的假設(shè)，由于障礙物都比較簡單且有規(guī)則，可以選擇障礙物最底部的點(diǎn)作為障礙物的特征點(diǎn)，用于度量障礙物和機(jī)器人的距離。

對(duì)單攝像頭采集的2D圖像，無法直接得到其中的像素點(diǎn)對(duì)應(yīng)實(shí)際點(diǎn)的真實(shí)坐標(biāo)，需要建立它們之間的映射關(guān)系。本文利用SVM［6］回歸分析，求得攝像頭模型參數(shù)。均值場理論［7］用于輔助SVM算法的學(xué)習(xí)過程。

本文采用針孔透視投影模型，假設(shè)真實(shí)點(diǎn)M（X，Y，Z）對(duì)應(yīng)圖像像素點(diǎn)m（x，y），用式（1）表示這種投影關(guān)系：

式（1）的圖形化表示如圖2所示，可以看出，3個(gè)輸出分支之間存在耦合，因此無法對(duì)其單獨(dú)優(yōu)化。

圖2　齊次變換的圖形化表示

使用線性核的SVM回歸規(guī)則表示輸出為：

為克服回歸輸出之間存在的耦合問題，在優(yōu)化支持向量回歸機(jī)時(shí)，可以使用梯度下降法對(duì)伸縮因子s進(jìn)行優(yōu)化。

用放置在地面上的黑白相間的方格棋盤來構(gòu)造訓(xùn)練數(shù)據(jù)集和測(cè)試數(shù)據(jù)集，以機(jī)器人在地面投影的中心為原點(diǎn)構(gòu)造真實(shí)世界坐標(biāo)系，如圖3所示。選擇方塊的角作為特征點(diǎn)，用其中60個(gè)點(diǎn)的數(shù)據(jù)作為訓(xùn)練數(shù)據(jù)，另外20個(gè)點(diǎn)的數(shù)據(jù)作為測(cè)試數(shù)據(jù)。可以通過角點(diǎn)檢測(cè)計(jì)算出所有特征點(diǎn)的像素坐標(biāo)。對(duì)被選入訓(xùn)練集合的特征點(diǎn)，它們?cè)谧鴺?biāo)系O-xyz中的3D坐標(biāo)為輸入，它們所對(duì)應(yīng)的像素點(diǎn)在圖像平面上的像素坐標(biāo)為輸出。

圖3　訓(xùn)練場景示意圖

給定伸縮因子初始值后，就可以開始訓(xùn)練映射函數(shù)。用平均場方法優(yōu)化圖3中的回歸分支，用梯度下降法更新伸縮因子，重復(fù)這兩個(gè)過程直到式（4）達(dá)到最小。

訓(xùn)練結(jié)束后，投影矩陣的估計(jì)值為：

測(cè)試時(shí)，輸入測(cè)試集中特征點(diǎn)的像素坐標(biāo)，得到對(duì)應(yīng)真實(shí)坐標(biāo)的估計(jì)值，3D重建的準(zhǔn)確度用均方根誤差來衡量：

2.3路徑規(guī)劃

機(jī)器人的運(yùn)動(dòng)機(jī)制比較復(fù)雜，使得其路徑規(guī)劃也很困難，一種簡單的處理辦法就是離散化機(jī)器人的運(yùn)動(dòng)空間。針對(duì)機(jī)器人的機(jī)械結(jié)構(gòu)及步態(tài)特點(diǎn)，本文為人形機(jī)器人設(shè)計(jì)了一種樹形路徑規(guī)劃算法。對(duì)機(jī)器人的下一個(gè)位置，設(shè)計(jì)了5種不同的選擇，如圖4所示。

圖4　運(yùn)動(dòng)空間離散化

對(duì)每個(gè)位置，由式（7）計(jì)算出一個(gè)值，作為選擇下一個(gè)位置的依據(jù)：

其中，j∈｛1，2，…，5｝，dmax為距離的最大值，本文中取dmax=300cm。

計(jì)算出每個(gè)位置對(duì)應(yīng)的fj后，再根據(jù)式（8）確定下一個(gè)位置的最優(yōu)選擇：

在圖4中，此位置為m3。當(dāng)機(jī)器人到達(dá)下一位置后，不斷重復(fù)以上路徑規(guī)劃過程直到機(jī)器人走到目的地。

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

本文以韓國ROBOTIS公司制造的小型人形機(jī)器人DARWIN為驗(yàn)證平臺(tái)，對(duì)所提出的導(dǎo)航方法進(jìn)行了實(shí)現(xiàn)。實(shí)驗(yàn)場景如圖5（a）所示，地面上的紅色小球?yàn)槟繕?biāo)地點(diǎn)，行進(jìn)的路徑上有4個(gè)障礙物。

圖5（a）、（b）分別為機(jī)器人在起點(diǎn)處的場景圖和視野圖，機(jī)器人看到預(yù)設(shè)的4個(gè)障礙和兩個(gè)目標(biāo)小球；圖5（c）、（d）分別為機(jī)器人到達(dá)第一個(gè)目標(biāo)地點(diǎn)處的場景圖和視野圖，機(jī)器人避開障礙物并準(zhǔn)確到達(dá)目的地，此時(shí)視野中還有兩個(gè)預(yù)設(shè)的障礙物以及一個(gè)目標(biāo)小球；圖5（e）、（f）為機(jī)器人到達(dá)第二個(gè)目標(biāo)地點(diǎn)處的場景圖和視野圖，機(jī)器人依然成功避開障礙物并準(zhǔn)確到達(dá)目的地，此時(shí)視野中無目標(biāo)，機(jī)器人停止行走。

圖5　機(jī)器人自主導(dǎo)航的快照

機(jī)器人順利完成設(shè)定的避障導(dǎo)航任務(wù)，驗(yàn)證了本文所述方法的有效性。

4　結(jié)論

本文為小型人形機(jī)器人提出一種實(shí)用的實(shí)時(shí)導(dǎo)航系統(tǒng)。此系統(tǒng)包括3部分：（1）基于顏色特征的障礙物檢測(cè)系統(tǒng)；（2）基于SVM的障礙物定位系統(tǒng)；（3）基于運(yùn)動(dòng)空間離散化的樹形路徑規(guī)劃系統(tǒng)。障礙物檢測(cè)系統(tǒng)通過區(qū)別障礙物和地面的顏色特征，能夠比較準(zhǔn)確地識(shí)別出在給定地面上的有別于地面顏色的障礙物，并選擇出用于計(jì)算障礙物和機(jī)器人距離的特征點(diǎn)。經(jīng)過訓(xùn)練的障礙物定位系統(tǒng)對(duì)測(cè)試數(shù)據(jù)表現(xiàn)良好，能夠比較精確地根據(jù)圖像中的像素坐標(biāo)還原出對(duì)應(yīng)實(shí)際點(diǎn)的真實(shí)坐標(biāo)。樹形路徑規(guī)劃方法將機(jī)器人的運(yùn)動(dòng)空間離散化為樹形結(jié)構(gòu)，能根據(jù)當(dāng)前障礙物和機(jī)器人的相對(duì)位置關(guān)系，從當(dāng)前位置節(jié)點(diǎn)的子節(jié)點(diǎn)中選擇下一個(gè)行進(jìn)位置，使機(jī)器人一步一步抵達(dá)目標(biāo)位置。在DARWIN機(jī)器人上實(shí)現(xiàn)了整個(gè)系統(tǒng)，并通過實(shí)驗(yàn)驗(yàn)證，機(jī)器人順利避開障礙物到達(dá)設(shè)定的目標(biāo)位置，顯示了本文方法的有效性。

［1］陳衛(wèi)東，朱奇光.基于模糊算法的移動(dòng)機(jī)器人路徑規(guī)劃［J］.電子學(xué)報(bào)，2011，39（4）：971-974，980.

［2］CHESTNUTT J，LAU M，CHEUNG G，et al.Footstep planning for the Honda ASIMO humanoid［C］.Proceedings of the 2005 IEEE International Conference on Robotics and Automation，ICRA 2005，IEEE，2005：629-634.

［3］ZANNATHAJM I，CISNEROSLIMóN R，GóMEZ SáNCHEZ A D，et al.Monocular visual self-localization for humanoid soccer robots［C］.2011 21st International Conference on Electrical Communications and Computers，.IEEE，2011：100-107.

［4］WANG Y T，HUNG D Y，CHENG S H.Monocular SLAM for a small-size humanoid robot［J］.Tamkang Journal of Science and Engineering，2011，14（2）：123-129.

［5］BALTES J，CHENG C T，LAU M C.Real-time navigation fora humanoid robotusing particle filter［J］.Applied Mechanics and Materials，2013，284：1914-1918.

［6］VAPNIK V.The nature of statistical learning theory［M］.New York：Springer，1995.

［7］OPPER M，WINTHER O.Gaussian processes for classification：mean field algorithms［J］.Neural Computation，2000，12（11）：2655-2684.

Study on real-time navigation for humanoid robot with monocular

Wang Junhua，Wei Henghua
（Department of Automation，University of Science and Technology of China，Hefei 230026，China）

This paper presents a real-time navigation system.Based on color information，pixels from monocular image are classified into ground and obstacles，and pixels from the same obstacle are clustered to choose the bottom ones as features.Then SVM（Support Vector Machine）is used to establish a mapping between the real-world coordinates of points and their corresponding image pixels，and Field Mean theory is used to assist the learning procedure.Distances between obstacles and robot can be estimated according to the mapping and features.At last，the path is planned step by step in the discretized motion space with the distances information.The navigation method is implemented on humanoid robot DARWIN and the results of the experiment show its effectiveness.

humanoid robots；navigation；color information；SVM；path planning

TP391

A

1674-7720（2015）16-0049-03

王軍華，魏衡華.基于單目攝像頭的人形機(jī)器人實(shí)時(shí)導(dǎo)航系統(tǒng)［J］.微型機(jī)與應(yīng)用，2015，34（16）：49-51，54.

2015-03-20）

王軍華（1988-），男，碩士研究生，主要研究方向：控制理論與控制工程。

魏衡華（1955-），男，碩士，副教授，碩士生導(dǎo)師，主要研究方向：智能控制，伺服控制，運(yùn)動(dòng)控制。