OpenCV NAO机器人辅助捡球丢球

目标：基于OpenCV库编写python程序，识别小球和垃圾桶，抓球并丢球。

1. 概述

NAO机器人身高58cm，体重5.4kg。主要硬件包括CPU、主板、扬声器、话筒、红外线、相机、超声波(声呐)、传感器、电机、语音合成器、陀螺仪等。实验室中NAO机器人的型号为NAO H25，其构造如下：

  NAO机器人的操作系统为Gentoo Linux，它支持Windows、Linux、Mac OS等操作系统的远程控制，可以在这些平台上编程控制NAO。头部CPU运行Linux内核，支持Aldebaran Robotics内核NAOqi，NAOqi提供了一组应用程序接口(API)用于操作机器人，如控制机器人运动、拍摄、声音识别、读传感器值等。使用C++、Python、.Net、Java、Matlab等语言可以调用这些API。
  本项目使用NAO机器人识别球并捡起，然后将其扔到指定位置。主要涉及图像的获取、滤波、目标物体定位和NAO机器人的运动控制。
  整个流程大致为：获取机器人头部摄像头的图片，对图片进行一系列处理后，再根据霍夫圆检测函数求出图片中球的坐标。这样循环求取球的坐标，先左右移动机器人使球位于图像的中间以防止球超出摄像头范围，再前后移动机器人使球位于图片的最下方以便于接下来切换到嘴部摄像头检测球。接着让机器人向前移动固定距离，然后切换到嘴部摄像头进行近距离检测，与头部摄像头检测类似，循环检测并调整球的坐标到预先设定的像素范围内，即可调用录制好的抓球动作将球捡起。最后机器人检测垃圾桶的轮廓，过滤无关轮廓并根据轮廓重心和轮廓面积移动到指定位置，然后将球扔入垃圾桶内即可。

需要解决的问题包括：
（1）OpenCV：图像指定颜色提取与二值化，噪声滤除、圆和垃圾桶的轮廓检测与定位
（1）NAO机器人：摄像头图像的获取、Animation模式下指定动作帧的录制、根据检测到的轮廓坐标进行运动控制

2. 相关技术

本项目主要用到了NAO机器人的运动控制和OpenCV图像处理等技术。

2.1 NAO机器人

（1）启动与连接

  按下NAO机器人胸口的按钮即可启动NAO。用户可以通过以太网或Wi-Fi两种方式连接计算机。第一次使用时先用以太网线连接NAO机器人，在机器人启动之后再按下其胸口的按钮，它会报出机器人的IP地址，接着在浏览器打开这个IP地址即可进入NAO机器人的设置界面。该界面可以设置机器人的扬声器音量，Wi-Fi连接、语言等功能。接着通过这个IP地址和固定端口号9559，即可使用C++、Python等编程语言通过TCP/IP协议与NAOqi进行连接，调用NAOqi的API即可实现对NAO机器人的操作。
  另外，NAO实际是一台计算能力足够强的计算机，它还提供了SSH(安全外壳协议)的远程登录(Telnet)和文件传输(FTP)服务。通过远程登录和文件传输服务，可以像是用一台装有Linux操作系统的计算机那样使用NAO。

（2）NAOqi
  在NAO上执行NAOqi是通过一个代理程序(Broker)完成的。启动机器人时，代理程序会自动加载/etc/naoqi/autoload.ini文件，这个文件中指定了需要加载NAOqi的哪些库，这些库文件位于/usr/lib/naoqi目录下。一个库包含一个或多个模块，每个模块定义了多种方法。例如NAO的运动功能都放在ALMotion模块中，让机器人完成移动、转头、张手等动作分别要调用ALMotion模块中的moveto()、setAngles()、OpenHand()等方法。
  使用NAOqi模块时，不需要像普通Python程序那样用import语句导入所有模块，模块通过Broker通告它所提供的方法。通过Broker，任何模块都可以找到所有已经通告的模块及方法。
  Broker主要由两个作用：直接服务，即查找模块和方法；网络访问，即从Broker进程外部调用模块方法。Broker既是一个可执行程序，也是一个服务器，可以对指定的IP和端口监听远程命令。通过IP和端口与调用NAOqi模块的程序，既可以在机器人上运行，也可以在远程计算机上直接运行。一般来说Broker是透明的，在大部分情况下，编程时可以不考虑Broker，调用本地模块的代码与调用远程模块的代码是一样的。

2.2 Choregraphe

  Choregraphe是NAO提供的编程环境，用该软件可以创建应用于NAO机器人的行为模块，并可以将其上传至所连接的机器人进行测试。Choregraphe采用的是图形化编程，创建复杂性为模块不需要用户编写任何一条代码，Choregraphe也提供用户自定义功能，允许使用Python语言编写自定义模块。在本次捡球程序中主要使用了该软件的时间轴指令盒和Animation模式来录制用户想让机器人完成的动作。

2.3 OpenCV

  OpenCV是一个跨平台的计算机视觉库，它是由英特尔公司发起并参与开发，以BSD许可证授权发行，可以在商业和研究领域中免费使用。OpenCV可用于开发实时的图像处理、计算机视觉以及模式识别程序。OpenCV可用于解决如下领域的问题：增强现实、人脸识别、手势识别、人机交互、动作识别、运动跟踪、物体识别、图像分割、机器人。
  OpenCV使用C++语言编写，它的主要接口也是C++语言，但是依然保留了大量的C语言接口。该库也有大量的Python,Java and MATLAB的接口，本项目中主要在Python中使用OpenCV接口。

3. 总体设计与详细设计

系统主要分为三大模块，分别为：姿势控制、运动控制、OpenCV视觉处理。

3.1 系统模块划分

一、姿势控制

由于需要根据摄像头的图像识别球，而机器人的不同姿势决定了机器人的视觉范围，所以在不同情况下需要指定机器人保持不同的姿势。

二、运动控制

机器人需要抓球与扔球，而整个动作序列通过填写不同帧时刻各个关节的角度值则不仅难以达到理想的效果，而且会花费大量的精力。所以这里通过Choregraphe的Animation模式对动作的关键帧进行录制。

三、OpenCV视觉处理与机器人移动

完成抓球和丢球最核心的工作就是对摄像头获取的图像进行处理并控制机器人的移动，图像处理主要通过OpenCV提供的API对图像进行去噪、模糊、颜色提取、轮廓检测，而机器人的移动主要是通过获取的轮廓的像素信息控制机器人移动到指定位置。

3.2 主要功能模块

一、姿势控制
如下图所示，NAO机器人使用旋转集合横滚(roll)、俯仰(pitch)和偏转(yaw)表示运动姿态，分别对应绕X、Y、Z轴方向上的旋转。在描述关节的运动范围时，沿旋转轴顺时针方向转动为负，逆时针转动为正。

NAO机器人的每个关节名称由部件名称+姿态名称组成，如HeadYaw表示机器人头部左右旋转的关节。经过测试，本项目中初始化动作为ShoulderPitch为80度，ShoulderRoll为20度，HipPitch为-25度，KneePitch为40度，AnklePitch为-20度，其它关节均为0度。将上述关节和角度写入一个list中，然后调用ALMotion中的angleInterpolationWithSpeed函数调整机器人的姿势。

二、运动控制
  打开Choregraphe-右键创建指令盒-时间轴，双击创建的时间轴即可编辑机器人的动作，可以在不同帧时刻存储NAO身体某个关节或全部关节的角度信息来完成动作的录制。而NAO机器人在正常模式下所有关节的刚度为1，不能移动其关节。而在Animation模式下，机器人的眼睛和脚部LED的颜色如果是橙色则表示刚度为0，此时可通过触摸其手部和脚部让LED的颜色变为绿色，这就可以随意的调整机器人的不同关节，调整完后在软件中记录这个关键帧即可。
  两个关键帧之间的动作可以通过贝塞尔曲线来模拟生成一个平滑的动作曲线以保证机器人动作的正常执行。调整完动作后，可以生成对应的C++/Python代码。

三、OpenCV视觉处理与机器人移动

（1）NAO摄像头的捕捉

  NAO头部有两个相机，用于识别视野中的物体，其中前额相机主要用于拍摄远景图像，嘴部相机主要用于拍摄下方图像。相机的可选分辨率有6种，最高为1280x960px，这里设置为640x480px。
  获取图像：首先连接ALVideoDevice模块，再调用subscribe函数订阅图像，该函数可以设置图片的分辨率、获取图片的色彩空间等参数。最后调用getImageRemote即可从摄像头中获取一个图像数组，其中0号和1号元素为图像的宽、高，6号元素为图像数据。获取图像后将图像转为numpy格式并返回。
  与RGB相比，HSV色彩空间不会随光照强度的变化而发生剧烈变化，目标物体颜色值也不会出现较大的偏差，一定程度上减弱了光照条件对机器人视觉系统的影响，增强了机器人视觉系统的自适应能力。所以获取RGB色彩空间的图像之后，通常将其转化为HSV色彩空间进行检测。

（2）球检测

待抓取的球主要通过霍夫圆变换进行检测，具体步骤如下：
1、从机器人的相机中获取图像，并将其转化为HSV色彩空间的图像。
2、接着根据HSV颜色范围表提取指定颜色范围，以绿球为例，其HSV的范围为[35,43,46]至[77,255,255]，调用inRange函数提取出绿色色彩空间。但实际测试效果并不好，会将周围环境中无关的内容提取出来，而实验中使用的绿球较鲜艳，经测试将HSV的下限中的S(饱和度)调高至128即可较好的提取出绿色色彩空间。
3、对图像进行高斯模糊以消除图片中的噪声，内核取7x7的矩阵。
4、进行一次开操作，即先腐蚀后膨胀以进一步消除噪声。
5、观察此时的图像，除了被检测的球，还有很多位置有断断续续的白色。所以先取反然后进行三次闭操作(先膨胀再腐蚀)，内核取15x15的矩阵。
6、此时得到的二值图较为理想，图像处理过程结果如下图所示：

7、调用HoughCircles进行霍夫圆检测，具体参数如下：

cv2.HoughCircles(np.uint8(picture),cv2.HOUGH_GRADIENT,1,100,param1=100,param2=7,minRadius=2,maxRadius=60)

其中

• 第一个参数为输入图像
• 第二个参数检测方法选择霍夫梯度法
• 第三个参数为累加面分辨率大小，这里设置为1表示分辨率与原图像相同
• 第四个参数为param1，即Canny边缘检测的高阈值
• 第五个参数为param2，其值越小检测圆的限制就越低
• 第六个参数为检测的最小圆半径
• 第七个参数为检测的最大圆半径

8、如果滤波效果好的话，能检测出图中所有圆圆心坐标和半径，这里假设仅有一个绿球，所以最终判断如果检测出来的结果中只有一个圆的话，返回该圆的圆心坐标。

以上即为检测球圆心的步骤，具体调用时，连续获取10次摄像头图像并检测球圆心，排序后，如果中间元素为(0,0)表示没有检测到球，接着将球坐标返回。

（3）抓球

由于NAO机器人蹲下抓取地上的球时姿势保持太久容易发热导致机器人需要散热才能继续使用，由于这需要大量时间和经验，而实训时间有限，所以这里将球放置在高21cm的平台上供机器人抓取。

首先在Choregraphe中打开Animation模式，然后记录3个关键帧：
（1）屈膝蹲下到手容易抓球的高度
（2）将手掌打开以防止手触碰到平台部分
（3）将机器人的右手移动到球上方的一点的高度
（4）接着将右手从右往左移动以保证球能在手掌中。
（5）站起
将上述5个关键帧生成代码后在Python中测试，开始上述动作之前需要调用ALMotion中的openHand函数打开机器人的手掌，上述动作完成后再调用closeHand抓起球。

（4）球定位

  将机器人放置到固定位置，然后测试上述的动作能顺利的将球抓起。记录此时机器人与球的相对位置：前脚离平台9cm、瓶盖圆心与右腿膝盖的右侧平行。接着在这个位置调用之前的球检测函数，在机器人使用嘴部摄像头且髋部HipPitch和膝部KneePitch为-25和+40时，球的坐标大概在(523.5,426.5)，由于此时机器人离球很近，所以这里允许X、Y坐标都有25像素的偏差，经测试在这个偏差范围内可以将球抓起。
  上述为嘴部摄像头的参数确定，而刚开始由于机器人离球有一定距离，需要用前额摄像头拍摄球。所以刚开始将机器人保持在固定动作，然后用前额摄像头检测球，根据检测出的球坐标对(x,y)进行调整。首先希望球位于到摄像头的最底端，这里实时检测球的位置，让机器人向前移动，将y增大到430以上。接着希望球能位于摄像头的最中心，所以实时检测球将其x坐标调整到290~350之间。
  接着让机器人前进，直到能从嘴部摄像头中检测到绿球。根据前面的测试，将球坐标稳定于(523.5,426.5)，且允许有25px的偏差即可。所以与前额摄像头的步骤一样，先调整y坐标再调整x坐标。
  这里要注意的是，由于ALMotion模块中机器人前进或后退的函数执行过程中会的受一些客观条件的影响，机器人往往会往左或往右偏离一定的角度。所以在机器人左右或前后移动后，判断其x或y坐标的偏差，如调整x坐标时，y坐标的偏差超过一定阈值则认为机器人的移动出现了偏离，所以向左或向右旋转一定的角度进行补偿。
  最后调用之前录制好的抓球动作进行抓球即可。

（5）垃圾桶定位

  这里希望将抓取到的球扔到红色垃圾桶中，同样通过机器人的摄像头获取图像，然后进行均值偏移滤波和中值滤波，再将图像转到HSV空间，提取红色的HSV范围。接着将结果进行二值化，再调用OpenCV中的findContour函数检测轮廓，这里根据垃圾桶与机器人的距离等参数测试，可得到垃圾桶在一定距离内的面积范围，根据这个范围选取出一个轮廓，再通过轮廓的结果可以计算出轮廓的重心坐标。
  将上述检测垃圾桶重心的功能封装成一个函数，其返回值为轮廓的重心坐标。这里将垃圾桶放置在机器人的左侧。在机器人抓起球后，首先后退25cm左右，这样做是防止机器人旋转时被平台挡住。然后将机器人旋转60°，保证垃圾桶在机器人摄像头范围内。后续步骤与检测球的步骤类似，首先调整机器人的Y坐标至340左右，接着调整X坐标在[300,335]范围内，最后机器人前进一定的距离即可走到垃圾桶的面前。

（6）丢球

  同样通过Choregraphe的Animation模式来录制动作，只需记录机器人手部的动作，共记录两帧：张开右臂和移动右臂到正中间。由于垃圾桶的直径相较于球来说大得多，所以允许的偏差范围也更大，成功率也更高。
  生成Python代码，执行上述动作无误后，调用ALMotion模块中的openHand将球放入垃圾桶中。

4 遇到的问题

一、机器人左右移动时，出现角度偏差导致抓球失败
  由于捡的是平台上的球，所以有一点角度偏差都可能导致机器人的手举起来的时候被平台挡住、球离开机器人的摄像头范围、调整Y坐标到了预期值后，调整X坐标时Y坐标又偏离太多等情况。
  最终发现，是NAO机器人自身伺服控制器的问题，让其前进自己会走歪，换一个NAO机器人完全没有走歪的问题，但实训过程中没有发现，故当作一个BUG来调整。
  这里简单地判断机器人移动的方向和另一坐标出现的偏差值，如果超过设定阈值即旋转一定的角度。如果已经调整完Y坐标后，调整X坐标的时导致球偏离到摄像头外部，则移动固定距离尽量抵消这个误差。实际可以通过大量的实验得出一定的规律，闭环反馈控制机器人的角度和位置。
  同时抓球的动作也很重要，一个好的抓球动作能允许机器人抓球有更大的误差。这里先将机器人的手放置于球的右侧，然后从右往左移动。如果球往右偏了一点，在手的移动过程中也会把球一起移动以确保球在手掌中。
二、OpenCV识别问题
（1）图像预处理
  从摄像头获取的图片会有一些噪声，所以需要先进行模糊处理，而模糊处理的矩阵内核大小需要根据球与机器人的距离得出一个范围，尽量将其取大以滤除无关像素。
  HSV色彩范围提取：有人总结出了不同颜色的HSV色彩空间范围，但毕竟这是一个大的范围，与我们待检测的物体颜色还有一定的偏差，会提取出很多无关的像素。所以需要根据实际情况调整色调(H)、饱和度(S)、明度(V)等参数。
  提取颜色范围时，物体周围很多断断续续的像素也会被提取出来，需要对结果进行膨胀与腐蚀等操作，将无关的像素滤除。
（2）圆检测
  调用HoughCircles函数,重点在于param1和param2参数的设置，同样需要不断地测试出一个较优的参数。实际检测中，有时拍到的图像明明没有球，也会将周围环境的部分像素检测成球。所以需要连续检测10次并排序，如果有5个都没有检测到，则输出(0,0)。
（3）垃圾桶检测
  首先调用OpenCV的pyrMeanShiftFiltering函数，使图像在色彩层面进行平滑滤波，它可以中和色彩分布相近的颜色，平滑色彩细节，侵蚀掉面积较小的颜色区域。该函数需要指定两个参数：漂移物理空间半径大小和漂移色彩空间半径大小，这里分别取10和35。接着将图像转到HSV空间，然后提取出红色色彩范围并二值化，最后调用findContours函数获得二值图像中的所有轮廓，提取其中面积最大的轮廓，求其重心并返回。
 实际测试过程中，对于不同环境中亮度偏差较大的情况，滤波、提取颜色范围、圆检测等参数都需要做出适当的调整。
三、未实现的功能
调整机器人的关节动作和OpenCV遇到的问题都需要大量的时间不断测试和调整参数，而本次实训时间有限，再加上第一次接触NAO机器人，所以还有很多功能没有完成。
（1）抓地上的球
  抓取地上的球，由于球各面都是对称的，所以如果出现前述的机器人左右移动产生不确定的角度偏移时，一般不会像在平台上会有小几率定位球失败的情况。最开始也想抓取地上的球，但是当时经验不足，机器人在岔开腿蹲下时腿部的伺服电机会发热，研究蹲下站起的动作需要考虑重心和速度等问题，否则机器人很容易摔倒，而如果一直保持某个动作不动的话，机器人会因过热而需要休息一段时间。
  让机器人蹲下抓取地上的球需要时间和经验，刚开始做的时候急于求成后来就改为平台捡球，而在实训快结束时，自然而然地积累了一定的经验：提前考虑好每个帧的动作，并及时让机器人处于StandInit状态以防止其电机过热。经过测试，我已成功录制了一套机器人蹲下抓球并站起的动作。但整个动作并不是最优的，有些地方速度太快，站得也不是很稳，如果连续测试多次，当有部分关节的电机因过热而刚度无法达到最大值1时，可能会出现摔倒的情况。这是完成实训后，又想着之前没成功，可能是做了这么久也积累了很多经验，做完之后再研究捡地上的球很快就把动作调出来了，但也懒得继续深入测试一个最优的捡球动作，毕竟实训结束了。

NAO机器人抓地上的球

（2）避障功能
NAO的胸口两侧各有一对超声波传感器，一个用于发送超声波，一个负责接收超声波。测距范围为0.25~2.55m，精度为0.01m。传感器所在的高度固定，如果障碍物没有那么高的话，可以让其髋部俯仰一定角度，配合OpenCV并通过数学公式和补偿函数求出机器人与障碍物间的距离。如果仅实现特定情况，即障碍形状、高度、位置等固定的情况下的避障，难度并不大，似乎有些刻意。如果要实现全方位的避障，该传感器的检测范围和精度并不足以满足需求。比如说脚底很低的障碍，不可能每走一步，就将机器人头向下环顾，然后将声呐对准下方每个方位检测，十分刻意。

5 实习体会

  刚开始接触NAO时一切都是未知的，在学了一周理论后，开始录制机器人捡球的动作。由于蹲下时机器人容易发热导致无法正常工作，这十分需要时间和经验，两周过去虽学了很多理论知识，但实际还没有太多进展。后来则捡放在平台上的球，先把OpenCV写好，又遇到了图像处理、机器人移动偏差等问题，最后在课上和课余时间不断测试与完善代码才完成了这次实训任务。
  实际捡球过程中对光线有一定的要求，如远距离检测球时，如光线不足，则需要扩大HSV颜色范围，这样可能会降低识别的准确性；检测垃圾桶时，若背光，则重心会偏上，机器人扔球时就可能走过头。如果想实现一个抗干扰能力强的程序，则还需要更多的实验。
  NAO机器人还有丰富的传感器和音频等资源，虽然本次实训没有怎么用到，但也为以后更深入地研究NAO机器人打下了基础。

6 视频展示

NAO机器人识别球并抓取丢到垃圾桶

识别的慢是这个NAO机器人内部的伺服驱动模块坏了，调用它的前进函数就会走歪。我就把步幅放慢，慢慢识别，同时还有左右走歪调整的函数。换一个没坏的NAO机器人就不会有这个问题了。