目标：基于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的颜色如果是橙色则表示刚度为1，可通过触摸其手部和脚部让LED的颜色变为绿色，这就可以随意的调整机器人的不同关节，调整完后在软件中记录这个关键帧即可。
?两个关键帧之间的动作可以通过贝塞尔曲线来模拟生成一个平滑的动作曲线以保证机器人动作的正常执行。调整完动作后，可以生成对应的C++/Python代码。
三、OpenCV视觉处理与机器人移动
（1）NAO摄像头的捕捉
?NAO头部有两个相机，用于识别视野中的物体，其中前额相机主要用于拍摄远景图像，嘴部相机主要用于拍摄下方图像。相机的可选分辨率有6种，最高为1280x960px，这里设置为640x480px。
?获取图像：首先连接ALVideoDevice模块，再调用subscribe函数订阅图像，该函数可以设置图片的分辨率、获取图片的色彩空间等参数。最后调用getImageRemote即可从摄像头中获取一个图像数组，其中0号和1号元素为图像的宽、高，6号元素为图像数据。获取图像后将图像转为numpy格式并返回。
?与RGB相比，HSV色彩空间不会随光照强度的变化而发生剧烈变化，目标物体颜色值也不会出现较大的偏差，一定程度上减弱了光照条件对机器人视觉系统的影响，增强了机器人视觉系统的自适应能力。所以获取RGB色彩空间的图像之后，通常将其转化为HSV色彩空间进行检测。
def getImage(IP, PORT, flag):
camProxy = ALProxy("ALVideoDevice", IP, PORT)
resolution = 2 # 640x480
colorSpace = 11 # RGB
# 头部/嘴部摄像头
camProxy.setParam(18, flag)
videoClient = camProxy.subscribe("python_client", resolution, colorSpace, 5)
naoImage = camProxy.getImageRemote(videoClient)
camProxy.unsubscribe(videoClient)
imageWidth = naoImage[0]
imageHeight = naoImage[1]
array = naoImage[6]
im = Image.frombytes("RGB", (imageWidth, imageHeight), array)
ci = np.array(im) # 转化成numpy格式图像
r, g, b = cv2.split(ci) # opencv的图像为BGR因此需要转换一下
ci = cv2.merge([b, g, r])
return ci
（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(dilated),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)接着将右手从右往左移动以保证球能在手掌中。
将上述三个关键帧生成代码后在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 视频展示
NAO机器人识别球并抓取丢到垃圾桶
- 识别的慢是这个NAO机器人内部的伺服驱动模块坏了，调用它的前进函数就会走歪。我就把步幅放慢，慢慢识别，同时还有左右走歪调整的函数。换一个没坏的NAO机器人就不会有这个问题了。
