李鶴喜，張長亮

（五邑大學(xué) 計算機學(xué)院，廣東 江門 529020）

基于單目立體視覺的NAO機器人的目標(biāo)定位與抓取

李鶴喜，張長亮

（五邑大學(xué) 計算機學(xué)院，廣東 江門 529020）

為了實現(xiàn)NAO機器人對空間目標(biāo)的定位與抓取，提出了一種單目立體視覺的導(dǎo)航方法.首先利用機器人上部攝像機的平動獲取兩幅目標(biāo)圖像，等效于雙目立體視覺，采用FFT頻域互相關(guān)和累加求和快速算法，進行兩幅目標(biāo)圖像的立體匹配，實現(xiàn)目標(biāo)位置的快速檢測；然后根據(jù)測得的目標(biāo)高度，運用視野中心算法和三角測量原理計算出目標(biāo)距離，進行目標(biāo)的跟蹤逼近，當(dāng)機器人到達目標(biāo)鄰近區(qū)域時使用下部攝像機進行目標(biāo)檢測并進行姿態(tài)調(diào)整，直到距離誤差落在目標(biāo)中心許可的圓形區(qū)域內(nèi)，進行目標(biāo)抓取，實際測試表明采用這種視覺導(dǎo)航方法的NAO機器人抓取成功率在90%以上.

立體匹配；互相關(guān)；目標(biāo)定位；目標(biāo)抓取

近年來智能人形機器人的研究與應(yīng)用逐漸興起，特別是讓人形機器人配上視覺已是一種必然趨勢[1].NAO機器人是一款用于科研、競賽、娛樂的雙足類人形機器人，有25個自由度和36個傳感器，其中頭部有上下兩個視覺傳感器，用于視覺導(dǎo)航.傳統(tǒng)上，對NAO機器人視覺導(dǎo)航的研究與應(yīng)用主要是集中在地面撿球與踢球競技上[2-5]，對于位于空間任何位置的目標(biāo)（非地面目標(biāo)）的定位、追蹤、抓取等操作缺乏有效的方法，一些研究將空間目標(biāo)貼上標(biāo)記以便識別[6]，顯然這種方法在實際應(yīng)用中很不方便.本文針對目標(biāo)空間定位與抓取問題提出了一種解決方法，主要思想是：利用NAO頭部攝像機的橫向平動形成立體視覺，采用FFT頻域互相關(guān)與累加求和快速算法進行立體匹配，并通過立體視覺的測量原理計算得到目標(biāo)特征點的空間高度，再根據(jù)高度利用視覺追蹤算法快速逼近目標(biāo).考慮到NAO機器人行走時產(chǎn)生的偏轉(zhuǎn)角度和距離誤差，需要選擇合適的控制策略來完成目標(biāo)的追蹤與抓取.NAO機器人頭部有兩個上下成一定角度的攝像頭，當(dāng)距離較遠(yuǎn)時，采用上部攝像頭，應(yīng)用視野中心法和透視三角原理實現(xiàn)遠(yuǎn)距離測距，當(dāng)機器人走到目標(biāo)附近時，采用下部攝像頭，并根據(jù)目標(biāo)距離圖像中心的位置來調(diào)整NAO機器人到誤差允許的范圍，最后實現(xiàn)目標(biāo)的抓取操作.

1 NAO機器人頭部攝像機內(nèi)參數(shù)的標(biāo)定

在利用視覺定位前，需要對NAO機器人的攝像機進行標(biāo)定，以獲取攝像機的內(nèi)參數(shù).一般攝像機用于視覺測量需要3個坐標(biāo)系：圖像坐標(biāo)系、攝像機坐標(biāo)系和世界坐標(biāo)系，由于運動的機器人考慮的是目標(biāo)相對攝像機的位置關(guān)系，故這里只考慮圖像坐標(biāo)系(u, v)和攝像機坐標(biāo)系(xc,yc,zc)的映射關(guān)系，可用下式表達：

移動靶標(biāo)，使靶標(biāo)與攝像機成45o左右的傾角，不同位置拍攝18幅圖像.通過圖像上的點與其靶標(biāo)上的點的DLT直接線性變換求出式（1）中的攝像機的內(nèi)參數(shù)矩陣A為：

圖1 標(biāo)定所用的平面靶標(biāo)

2 NAO機器人的立體視覺匹配

設(shè)NAO機器人在位置1獲取的圖像為f1(u, v)，在垂直于目標(biāo)深度方向平行移動距離B，到位置2獲取圖像為f2(u, v)，按順序約束，采用區(qū)域匹配算法進行兩幅圖像同名點的匹配計算.設(shè)右圖像f2(u, v)上目標(biāo)的特征點為(u2, v2)，選擇包括(u2, v2)的匹配區(qū)域為g2(u, v)，選擇左圖像f1(u, v)的搜索區(qū)域為g1(u, v)，包含了(u2, v2)對應(yīng)的同名點為(u1, v1)，區(qū)域盡量達到水平方向的最大覆蓋，如圖2.

圖2 左右圖像的區(qū)域匹配

設(shè)搜索的目標(biāo)圖像g1(u, v)的像素尺寸為，匹配區(qū)域g2(u, v)的像素尺寸為，則g2(u, v)在圖像g1(u, v)中第(I, J)點兩者的相似性可以用函數(shù)互相關(guān)來表征，考慮歸一化處理后，其離散形式的相關(guān)可以表達為

R(I, J)就是傳統(tǒng)的NCC（Normal Cross Correlation）歸一化互相關(guān)系數(shù)表達式，在空間域內(nèi)計算R(I, J)是非常消耗時間的，難以滿足機器人視覺的實時性要求，一種改進方法是將其變換到頻率域利用FFT快速傅里葉變換提高計算效率，R(I, J)的分子部分正是兩函數(shù)相關(guān)的定義，按相關(guān)運算與傅里葉變換的關(guān)系有

就是先把g1(u, v)和g2(u, v)分別進行FFT變換，得到和，在頻率域計算與共軛的乘積，然后再進行逆FFT變換得到空間域的相關(guān)系數(shù).R(I, J)歸一化的分母部分無法轉(zhuǎn)到頻率域處理，只能在空間域里進行單獨計算，為此令R(I, J)的分母為

則由公式（3）得到

由于式（6）的分母S(I, J)求和是隨模板的移動動態(tài)變化的，在模板到達的每個位置(I, J)，都需要計算這個能量和，非常耗時.因此，能夠快速計算這個能量和就可以加快歸一化處理的速度，提高匹配效率.這里利用Viloa的積分求和思想進行快速遞推[8]，下面分析式（6）分母S(I, J)的快速遞推求和方法.

圖3 第一行灰度平方和S(1,J)的計算

這里Y(1,J)為第一行上按模板覆蓋下面J列的灰度平方和，由下式?jīng)Q定：

對于任意一行I，可以得到

與行類似，對于任意一列J，可以得到

完成空間域的灰度快速求和后應(yīng)用式（4）的頻域FFT變換首先求得g1(u, v)和g2(u, v)未歸一化處理的互相關(guān)，然后按式（6）除以S(I, J)便得到歸一化的互相關(guān)系數(shù)R(I, J).與傳統(tǒng)NCC匹配方法相比，本法計算效率高，一個匹配點的檢測＜300 ms ，滿足機器人視覺的實時性要求.一個典型的匹配點相關(guān)系數(shù)R(I, J)的計算實例如圖4所示.取R(I, J)達到最大值的點作為與特征點(u2, v2)匹配的點(u1, v1)，即

圖4 匹配點檢測的相關(guān)系數(shù)R的分布圖

3 目標(biāo)紅球的定位與抓取試驗

這里以NAO機器人定位與抓取空間中任意高度的紅球作為實例，選擇紅球的中心為識別特征點，試驗按以下步驟進行：

1）目標(biāo)圖像的檢測

進行紅球目標(biāo)定位，首先要在NAO機器人攝像頭拍攝的整幅場景圖像中檢測出目標(biāo)紅球.一般在室內(nèi)環(huán)境條件下，基于顏色的空間分割易受光照的影響，這里采用YUV顏色空間[9]，因為它受光照的影響小.目標(biāo)的YUV顏色空間圖像如圖5，通過對YUV圖像的顏色濾波處理，就能快速檢測出紅球.

圖5 目標(biāo)的YUV顏色空間的圖像

2）目標(biāo)特征點的確定

根據(jù)試驗確定紅色的YUV各通道的閾值，得到二值圖像，先按canny算子進行邊緣檢測，再進行霍夫圓檢測[10]，得到紅球的圓心坐標(biāo)，作為目標(biāo)特征點，如圖6所示.

圖6 目標(biāo)紅球特征點的檢測

3）利用立體視覺獲取紅球高度

取紅球目標(biāo)上的圓心特征點，利用NAO上部攝像機水平移動B距離，產(chǎn)生左右兩幅圖像，(u1,v1)和(u2, v2)為紅球目標(biāo)上采用立體匹配技術(shù)確定的一對匹配點，則根據(jù)式（1）得到兩個方程組：

解之得到空間目標(biāo)上特征點在攝像機坐標(biāo)系下的坐標(biāo)為(xc, yc, zc)，其中.yc就是目標(biāo)紅球到NAO頭頂攝像機中心的高度h（即）.在已知h的情形下，就可以利用NAO機器人頭部上下兩個攝像機采用單目視覺方式，快速逼近目標(biāo).

4）逼近控制策略

下面討論在在高度h確定后，NAO機器人的目標(biāo)逼近控制策略.NAO機器人頭部有上下兩個攝像頭，其夾角為固定的39.7°，如圖7所示.在NAO機器人跟蹤逼近時，離紅球較遠(yuǎn)時采用上攝像頭，較近時采用下攝像頭.

圖7 NAO機器人頭部攝像機參數(shù)圖

圖8 單目測距原理圖

在距離較遠(yuǎn)時，采用視野中心法來測量NAO機器人與紅球之間的距離.首先，因機器人的視角有限，只能看到有限的視野空間.所以必須通過上下左右擺動攝像頭來尋找紅球，當(dāng)視野中檢測到紅球的時候，通過單目成像原理，如圖8所示，計算出紅球的偏轉(zhuǎn)角度θ.

調(diào)整NAO機器人的頭部，讓攝像機的光軸坐標(biāo)（通過攝像機標(biāo)定出來的）(u0, v0)對準(zhǔn)紅球的球心坐標(biāo)，調(diào)整時設(shè)置圖像橫軸和縱軸像素誤差之和為20個像素，因為考慮到機器人每次的最小偏轉(zhuǎn)角度，所以誤差之和不宜過小，否則將無法確定紅球的最終偏轉(zhuǎn)角度.方向調(diào)節(jié)時由NAO機器人自身傳感器獲得機器人頭部的左右偏轉(zhuǎn)角度為和上下偏轉(zhuǎn)角度為，讓機器人身體偏轉(zhuǎn)角度，頭部偏轉(zhuǎn)角度，可使NAO機器人整個身體對準(zhǔn)紅球.為了讓NAO機器人走到紅球前，須測出兩者之間的距離.這里利用前面立體視覺測得的目標(biāo)高度和頭部偏轉(zhuǎn)角來計算，測距原理如圖9.O為紅球距離地面的高度，H為NAO機器人攝像機到地面的高度，h（即yc）為NAO機器人攝像機光軸中心距離紅球的高度，為NAO機器人頭部攝像機上下的偏轉(zhuǎn)角度，則NAO機器人與紅球的距離d的計算公式為：.在距離較近時，采用下面的攝像頭來定位紅球，在高度h確定的情況下，NAO機器人準(zhǔn)備抓取紅球前，調(diào)整NAO的頭部上下左右旋轉(zhuǎn)的角度都為0°，并保持不變，拍下目標(biāo)圖像，測得紅球中心點坐標(biāo)為(uc, vc)，如圖10所示.

圖9 測距原理圖

最后通過設(shè)定誤差半徑的方式確定(uc, vc)到指定坐標(biāo)(u2, v2)的誤差范圍，如圖11，r為誤差半徑，通過讓NAO機器人不斷走動來使紅球的中心坐標(biāo)達到允許的誤差范圍.即

經(jīng)試驗驗證，r取小于50個像素的時候，可以抓取目標(biāo)紅球.

圖10 抓取前目標(biāo)圖片及檢測出的中心

圖11 目標(biāo)中心落入的誤差圓

5）抓球動作的設(shè)計

確定目標(biāo)紅球在誤差圓內(nèi)，通過預(yù)先編程設(shè)計的機器人視覺伺服程序，完成手臂及手指的動作控制，通過控制NAO長臂的伸出擺動，短臂的回轉(zhuǎn)下落、手指的抓握等系列動作，完成紅球的抓取操作.圖12所示為NAO機器人到達目標(biāo)區(qū)后，目標(biāo)中心在誤差圓內(nèi)的抓取動作截圖.

圖12 NAO機器人抓球動作圖

6）目標(biāo)抓取試驗結(jié)果

試驗的目標(biāo)紅球采用兩種高度265 mm和290 mm，距離設(shè)定為兩個區(qū)段：1000～2 000 mm和2 0003000 mm，然后利用前面討論的技術(shù)由NAO機器人完全自主地在兩種高度和不同距離范圍內(nèi)，分別進行紅球的定位與抓取試驗50次，試驗結(jié)果列于表1，其抓取的成功率可達90%以上，一般在NAO機器人手臂可達的范圍，NAO機器人可以完成任意位置的紅球目標(biāo)的抓取.

表1 兩種高度不同距離的紅球定位與抓取的試驗結(jié)果

4 結(jié)論

通過NAO機器人頭部攝像機的橫向平動實現(xiàn)的單目立體視覺，可以檢測目標(biāo)特征點在空間的任何位置，其中的立體匹配采用了FFT頻域變換和快速積分求和算法計算互相關(guān)系數(shù)，和傳統(tǒng)的區(qū)域NCC方法比較，更能有效地提高立體匹配的效率，滿足機器人視覺的實時性要求.在機器人逼近目標(biāo)策略上，使用遠(yuǎn)近兩種視覺控制策略，定位準(zhǔn)確性較高，對紅球目標(biāo)試驗結(jié)果表明，目標(biāo)抓取的成功率可以達到90%以上.

[1] CLAUDIO G, SPINDLER F, CHAUMETTE F.Vision-based manipulation with the humanoid robot Romeo [C]//IEEE-RAS 16th International Conference on Humanoid Robotics, Cancun, Mexico: IEEE, 2016: 286-293.

[2] TAMURA K, NOZAKI T, KAWAMURA A.Visual servo system for ball dribbling by using bipedal robot“Nao”[C]//41st Annual Conference of the IEEE Industrial Electronics Society, [S.l.]: IEEE, 2015: 3461-3466.

[3] MIRANDA S, REIS L P, SOUSA A.Impact of color spectrum reduction in object detection on the RoboCup Standard Platform League[C]//8th Iberian Conference on Information Systems and Technologies (CISTI).[S.l.]: IEEE, 2013:1-6.

[4] ZHAO Shuying, WANG Xiaotao, SU Han, et al.Target recognition and localization of international standard platform humanoid robot [C]//24th Chinese Control and Decision Conference (CCDC).Taiyuan: [s.n.], 2012: 1930-1934.

[5] GUAN Jingwei, MENG Q H.Study on distance measurement for NAO humanoid robot [C]//IEEE International Conference on Robotics and Biomimetics (ROBIO), [S.l.]: IEEE, 2012: 283-286.

[6] 袁麗，田國會，李國棟.NAO機器人的視覺伺服物品抓取操作[J].山東大學(xué)學(xué)報（工學(xué)版），2014, 44(3): 58-62.

[7] ZHANG Zhengyou.A flexible new technique for camera calibration [J].IEEE Trans Pattern analysis and machine intelligence, 2000, 22(11): 1330-1334.

[8] VILOA P, JONES M.Robust real-time object detection [J].International Journal of Computer Vision, 2004, 57(2): 137–154.

[9] 王昕，孫瑩瑩，杜旭.基于YUV顏色空間的圖像去霧算法[J].吉林大學(xué)學(xué)報（信息科學(xué)版），2015, 33(6): 658-661.

[10] 高煜妤，王春芳.基于歐氏距離圖的隨機Hough變換橢圓檢測方法[J].現(xiàn)代電子技術(shù)，2016, 39(21): 61-64.

[責(zé)任編輯：韋 韜]

Object Locating and Grasping of NAO Robot Based on Monocular Stereovision

LI He-xi, ZHANG Chang-liang
(School of Computer Science, Wuyi university, Jiangmen 529020, China)

A monocular stereovision-based navigation technique is presented in order to implement the object locating and grasping of NAO robot.First, two images of an object are acquired using an upper camera on robot head through translational motion of NAO robot, which is equivalent to a binocular vision, and FFT-based cross correlation in frequency domain and fast integral sum arithmetic are used for the stereo-matching of two images to achieve rapid measurement of object position, then according to the measured object height NAO robot approaches the object using the vision-field-center method and triangular surveying, when the robot arrive in the adjacent area of object, the lower camera on NAO head is applied to determine the object position and robot posture is adjusted until the distance error fell in a tolerant circular area surrounding the object center, finally the NAO robot will grasp the object.The test results show that the grasping success rate of NAO robot is over 90% using proposed vision navigation.

stereo matching; cross correlation; object locating; object grasping

TP242.6

A

1006-7302（2017）02-0020-07

2017-02-08

廣東省自然科學(xué)基金資助項目（2016A030313003）

李鶴喜（1961—），男，遼寧昌圖人，副教授，博士，主要研究方向為機器視覺與智能機器人.