基于单目立体视觉的NAO机器人的目标定位与抓取

李鹤喜，张长亮

（五邑大学 计算机学院，广东 江门 529020）

基于单目立体视觉的NAO机器人的目标定位与抓取

李鹤喜，张长亮

（五邑大学 计算机学院，广东 江门 529020）

为了实现NAO机器人对空间目标的定位与抓取，提出了一种单目立体视觉的导航方法.首先利用机器人上部摄像机的平动获取两幅目标图像，等效于双目立体视觉，采用FFT频域互相关和累加求和快速算法，进行两幅目标图像的立体匹配，实现目标位置的快速检测；然后根据测得的目标高度，运用视野中心算法和三角测量原理计算出目标距离，进行目标的跟踪逼近，当机器人到达目标邻近区域时使用下部摄像机进行目标检测并进行姿态调整，直到距离误差落在目标中心许可的圆形区域内，进行目标抓取，实际测试表明采用这种视觉导航方法的NAO机器人抓取成功率在90%以上.

立体匹配；互相关；目标定位；目标抓取

近年来智能人形机器人的研究与应用逐渐兴起，特别是让人形机器人配上视觉已是一种必然趋势[1].NAO机器人是一款用于科研、竞赛、娱乐的双足类人形机器人，有25个自由度和36个传感器，其中头部有上下两个视觉传感器，用于视觉导航.传统上，对NAO机器人视觉导航的研究与应用主要是集中在地面捡球与踢球竞技上[2-5]，对于位于空间任何位置的目标（非地面目标）的定位、追踪、抓取等操作缺乏有效的方法，一些研究将空间目标贴上标记以便识别[6]，显然这种方法在实际应用中很不方便.本文针对目标空间定位与抓取问题提出了一种解决方法，主要思想是：利用NAO头部摄像机的横向平动形成立体视觉，采用FFT频域互相关与累加求和快速算法进行立体匹配，并通过立体视觉的测量原理计算得到目标特征点的空间高度，再根据高度利用视觉追踪算法快速逼近目标.考虑到NAO机器人行走时产生的偏转角度和距离误差，需要选择合适的控制策略来完成目标的追踪与抓取.NAO机器人头部有两个上下成一定角度的摄像头，当距离较远时，采用上部摄像头，应用视野中心法和透视三角原理实现远距离测距，当机器人走到目标附近时，采用下部摄像头，并根据目标距离图像中心的位置来调整NAO机器人到误差允许的范围，最后实现目标的抓取操作.

1 NAO机器人头部摄像机内参数的标定

在利用视觉定位前，需要对NAO机器人的摄像机进行标定，以获取摄像机的内参数.一般摄像机用于视觉测量需要3个坐标系：图像坐标系、摄像机坐标系和世界坐标系，由于运动的机器人考虑的是目标相对摄像机的位置关系，故这里只考虑图像坐标系(u, v)和摄像机坐标系(xc,yc,zc)的映射关系，可用下式表达：

移动靶标，使靶标与摄像机成45o左右的倾角，不同位置拍摄18幅图像.通过图像上的点与其靶标上的点的DLT直接线性变换求出式（1）中的摄像机的内参数矩阵A为：

图1 标定所用的平面靶标

2 NAO机器人的立体视觉匹配

设NAO机器人在位置1获取的图像为f1(u, v)，在垂直于目标深度方向平行移动距离B，到位置2获取图像为f2(u, v)，按顺序约束，采用区域匹配算法进行两幅图像同名点的匹配计算.设右图像f2(u, v)上目标的特征点为(u2, v2)，选择包括(u2, v2)的匹配区域为g2(u, v)，选择左图像f1(u, v)的搜索区域为g1(u, v)，包含了(u2, v2)对应的同名点为(u1, v1)，区域尽量达到水平方向的最大覆盖，如图2.

图2 左右图像的区域匹配

设搜索的目标图像g1(u, v)的像素尺寸为，匹配区域g2(u, v)的像素尺寸为，则g2(u, v)在图像g1(u, v)中第(I, J)点两者的相似性可以用函数互相关来表征，考虑归一化处理后，其离散形式的相关可以表达为

R(I, J)就是传统的NCC（Normal Cross Correlation）归一化互相关系数表达式，在空间域内计算R(I, J)是非常消耗时间的，难以满足机器人视觉的实时性要求，一种改进方法是将其变换到频率域利用FFT快速傅里叶变换提高计算效率，R(I, J)的分子部分正是两函数相关的定义，按相关运算与傅里叶变换的关系有

就是先把g1(u, v)和g2(u, v)分别进行FFT变换，得到和，在频率域计算与共轭的乘积，然后再进行逆FFT变换得到空间域的相关系数.R(I, J)归一化的分母部分无法转到频率域处理，只能在空间域里进行单独计算，为此令R(I, J)的分母为

则由公式（3）得到

由于式（6）的分母S(I, J)求和是随模板的移动动态变化的，在模板到达的每个位置(I, J)，都需要计算这个能量和，非常耗时.因此，能够快速计算这个能量和就可以加快归一化处理的速度，提高匹配效率.这里利用Viloa的积分求和思想进行快速递推[8]，下面分析式（6）分母S(I, J)的快速递推求和方法.

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

这里Y(1,J)为第一行上按模板覆盖下面J列的灰度平方和，由下式决定：

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

与行类似，对于任意一列J，可以得到

完成空间域的灰度快速求和后应用式（4）的频域FFT变换首先求得g1(u, v)和g2(u, v)未归一化处理的互相关，然后按式（6）除以S(I, J)便得到归一化的互相关系数R(I, J).与传统NCC匹配方法相比，本法计算效率高，一个匹配点的检测＜300 ms ，满足机器人视觉的实时性要求.一个典型的匹配点相关系数R(I, J)的计算实例如图4所示.取R(I, J)达到最大值的点作为与特征点(u2, v2)匹配的点(u1, v1)，即

图4 匹配点检测的相关系数R的分布图

3 目标红球的定位与抓取试验

这里以NAO机器人定位与抓取空间中任意高度的红球作为实例，选择红球的中心为识别特征点，试验按以下步骤进行：

1）目标图像的检测

进行红球目标定位，首先要在NAO机器人摄像头拍摄的整幅场景图像中检测出目标红球.一般在室内环境条件下，基于颜色的空间分割易受光照的影响，这里采用YUV颜色空间[9]，因为它受光照的影响小.目标的YUV颜色空间图像如图5，通过对YUV图像的颜色滤波处理，就能快速检测出红球.

图5 目标的YUV颜色空间的图像

2）目标特征点的确定

根据试验确定红色的YUV各通道的阈值，得到二值图像，先按canny算子进行边缘检测，再进行霍夫圆检测[10]，得到红球的圆心坐标，作为目标特征点，如图6所示.

图6 目标红球特征点的检测

3）利用立体视觉获取红球高度

取红球目标上的圆心特征点，利用NAO上部摄像机水平移动B距离，产生左右两幅图像，(u1,v1)和(u2, v2)为红球目标上采用立体匹配技术确定的一对匹配点，则根据式（1）得到两个方程组：

解之得到空间目标上特征点在摄像机坐标系下的坐标为(xc, yc, zc)，其中.yc就是目标红球到NAO头顶摄像机中心的高度h（即）.在已知h的情形下，就可以利用NAO机器人头部上下两个摄像机采用单目视觉方式，快速逼近目标.

4）逼近控制策略

下面讨论在在高度h确定后，NAO机器人的目标逼近控制策略.NAO机器人头部有上下两个摄像头，其夹角为固定的39.7°，如图7所示.在NAO机器人跟踪逼近时，离红球较远时采用上摄像头，较近时采用下摄像头.

图7 NAO机器人头部摄像机参数图

图8 单目测距原理图

在距离较远时，采用视野中心法来测量NAO机器人与红球之间的距离.首先，因机器人的视角有限，只能看到有限的视野空间.所以必须通过上下左右摆动摄像头来寻找红球，当视野中检测到红球的时候，通过单目成像原理，如图8所示，计算出红球的偏转角度θ.

调整NAO机器人的头部，让摄像机的光轴坐标（通过摄像机标定出来的）(u0, v0)对准红球的球心坐标，调整时设置图像横轴和纵轴像素误差之和为20个像素，因为考虑到机器人每次的最小偏转角度，所以误差之和不宜过小，否则将无法确定红球的最终偏转角度.方向调节时由NAO机器人自身传感器获得机器人头部的左右偏转角度为和上下偏转角度为，让机器人身体偏转角度，头部偏转角度，可使NAO机器人整个身体对准红球.为了让NAO机器人走到红球前，须测出两者之间的距离.这里利用前面立体视觉测得的目标高度和头部偏转角来计算，测距原理如图9.O为红球距离地面的高度，H为NAO机器人摄像机到地面的高度，h（即yc）为NAO机器人摄像机光轴中心距离红球的高度，为NAO机器人头部摄像机上下的偏转角度，则NAO机器人与红球的距离d的计算公式为：.在距离较近时，采用下面的摄像头来定位红球，在高度h确定的情况下，NAO机器人准备抓取红球前，调整NAO的头部上下左右旋转的角度都为0°，并保持不变，拍下目标图像，测得红球中心点坐标为(uc, vc)，如图10所示.

图9 测距原理图

最后通过设定误差半径的方式确定(uc, vc)到指定坐标(u2, v2)的误差范围，如图11，r为误差半径，通过让NAO机器人不断走动来使红球的中心坐标达到允许的误差范围.即

经试验验证，r取小于50个像素的时候，可以抓取目标红球.

图10 抓取前目标图片及检测出的中心

图11 目标中心落入的误差圆

5）抓球动作的设计

确定目标红球在误差圆内，通过预先编程设计的机器人视觉伺服程序，完成手臂及手指的动作控制，通过控制NAO长臂的伸出摆动，短臂的回转下落、手指的抓握等系列动作，完成红球的抓取操作.图12所示为NAO机器人到达目标区后，目标中心在误差圆内的抓取动作截图.

图12 NAO机器人抓球动作图

6）目标抓取试验结果

试验的目标红球采用两种高度265 mm和290 mm，距离设定为两个区段：1000～2 000 mm和2 0003000 mm，然后利用前面讨论的技术由NAO机器人完全自主地在两种高度和不同距离范围内，分别进行红球的定位与抓取试验50次，试验结果列于表1，其抓取的成功率可达90%以上，一般在NAO机器人手臂可达的范围，NAO机器人可以完成任意位置的红球目标的抓取.

表1 两种高度不同距离的红球定位与抓取的试验结果

4 结论

通过NAO机器人头部摄像机的横向平动实现的单目立体视觉，可以检测目标特征点在空间的任何位置，其中的立体匹配采用了FFT频域变换和快速积分求和算法计算互相关系数，和传统的区域NCC方法比较，更能有效地提高立体匹配的效率，满足机器人视觉的实时性要求.在机器人逼近目标策略上，使用远近两种视觉控制策略，定位准确性较高，对红球目标试验结果表明，目标抓取的成功率可以达到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].山东大学学报（工学版），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].吉林大学学报（信息科学版），2015, 33(6): 658-661.

[10] 高煜妤，王春芳.基于欧氏距离图的随机Hough变换椭圆检测方法[J].现代电子技术，2016, 39(21): 61-64.

[责任编辑：韦 韬]

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

广东省自然科学基金资助项目（2016A030313003）

李鹤喜（1961—），男，辽宁昌图人，副教授，博士，主要研究方向为机器视觉与智能机器人.