NAO足球/高尔夫比赛视觉系统设计(python初级版)

概述

  初级版的内容主要分为两部分,第一部分是如何利用NAO的视觉传感器,即上下摄像头,来获取图片及如何利用opencv显示获得的图片。第二部分是如何利用opencv里面的视觉算法从NAO获取的图片中找到所需目标,并返回需要的目标信息。

视觉系统框架设计

  首先要搭建好视觉系统的程序框架,python是一种面向对象的编程语言,而面向对象最重要的特征就是类的封装,所以我们可以将整个视觉系统分为若干类,每个类实现相应的功能。
data

ConfigureNAO类

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
from naoqi import ALProxy


class ConfigureNao(object):
def __init__(self, IP="169.254.67.213", PORT=9559):
self.IP = IP
self.PORT = PORT

self.cameraProxy = ALProxy("ALVideoDevice", self.IP, self.PORT)
self.landMarkProxy = ALProxy("ALLandMarkDetection", self.IP, self.PORT)

self.motionProxy = ALProxy("ALMotion", self.IP, self.PORT)
self.postureProxy = ALProxy("ALRobotPosture", self.IP, self.PORT)

self.tts = ALProxy("ALTextToSpeech", self.IP, self.PORT)
self.memoryProxy = ALProxy("ALMemory", self.IP, self.PORT)

  首先定义NAO机器人的IP端口号,并将此作为类的参数,其次声明一些常用的类,如视觉、运动、姿势和存储类等。只有实例化后的类才可以使用。

VisualBasis类

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
# coding: utf-8

from configureNao import ConfigureNao
from naoqi import ALProxy

import numpy as np
import almath
import math

import time
import sys
import os

import cv2
import cv2.cv as cv
import vision_definitions as vd


class VisualBasis(ConfigureNao):
def __init__(self, IP, PORT, cameraID=vd.kBottomCamera, resolution=vd.kVGA):
super(VisualBasis, self).__init__(IP, PORT)
self.cameraID = cameraID
self.resolution = resolution
self.colorSpace = vd.kBGRColorSpace
self.fps = 20

self.frameHeight = 320
self.frameWidth = 640
self.frameChannels = 0
self.frameArray = None

self.cameraPitchRange = 47.64 / 180 * np.pi
self.cameraYawRange = 60.97 / 180 * np.pi

  这里使用了naoqi系统的宏定义vision_definitions,为了书写方便,将其简写为vd,其中这里使用了摄像头ID的宏定义:vd.kBottomCamera(下摄像头)和vd.kTopCamera(上摄像头)、分辨率宏定义vd.kVGA和颜色空间宏定义vd.kBGRColorSpace。(具体含义见下文分析)。另外将图像本身(frameArray)及其高度、宽度、通道,摄像头的俯仰角作为类的属性,以便后续方法的调用。
  因为NAO读取球目标和黄杆目标分别使用的是上面和下面的摄像头,所以将摄像头ID作为类的参数以便后续调整。基类的第二个参数为分辨率,方便后续图像的清晰度调整。IP和PORT是为了继承父类所需的参数。

获取图片

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
def updateFrame(self):
if self.cameraProxy.getActiveCamera() != self.cameraID:
self.cameraProxy.setActiveCamera(self.cameraID)

videoClient = self.cameraProxy.subscribe("cxx", self.resolution, self.colorSpace, self.fps)
frame = self.cameraProxy.getImageRemote(videoClient)
self.cameraProxy.unsubscribe(videoClient)

self.frameWidth = frame[0]
self.frameHeight = frame[1]
self.frameChannels = frame[2]
self.frameArray = np.frombuffer(frame[6], dtype=np.uint8).reshape([frame[1], frame[0], frame[2]])
# frombuffer将data以流的形式读入转化成ndarray对象, 第一参数为stream,第二参数为返回值的数据类型
# frame[6]: binary array of size height * width * nblayers containing image data.
if self.frameArray is None:
print("no frame find")
return np.array([])

return self.frameArray

  让NAO读取一张图片大致可以分为3个步骤。1.激活摄像头。2.订阅。3.读取图像。
  调用setActiveCamera()函数即可激活摄像头,其次就是调用subscribe()函数订阅该摄像头,该函数的函数头为std::string ALVideoDeviceProxy::subscribe(const std::string& Name, const int& resolution, const int& colorSpace, const int& fps),一共需要4个参数。第1个为订阅者的名字,随便写一个字符串即可,第2个是分辨率,NAO最高支持1280*960的分辨率,但是越高的分辨率意味着越大的数据量,所以我们选择的是640*480的分辨率,其宏定义为vd.kVGA,第3个参数为颜色空间,这里选择的是一般的BGR空间,其宏定义为vd.kBGRColorSpace,最后一个为帧数,这里选择20即可。
  订阅完之后,就可以调用getImageRemote()函数来获得图像了。其参数为之前的订阅者,返回值为图像的容器,具体内容如下:
data
  其中0-5都是图像的基本信息,即宽、高、通道、颜色空间和时间戳。第6个索引值存放的内容就是我们需要的图像信息,使用numpy里面的reshape()函数将其简单的处理成我们需要的宽*高*通道类型的数组。
  最后使用unsubscribe()取消订阅以释放内存即可。

显示图片

1
2
3
4
5
6
7
8
def showFrame(self, frameArray, timeMs=1000, isSave=False):
if frameArray is None:
print("please get an image from Nao with the method updateFrame()")
else:
cv2.imshow("current frame", frameArray)
cv2.waitKey(timeMs)
if isSave is True:
cv2.imwrite("test.jpg", frameArray)

  opencv中常用的显示图片的函数为imshow(),该函数需要传入2个参数,即显示图像窗口的名称和图像内容。然后调用waitKey()函数延迟显示一段时间,单位为ms。最后提供了一个是否保存的参数接口,以便后续需要保存获得的图像。

FootBallDetect

  针对足球和红球,主要的流程为检测->筛选->定位。首先对NAO获得的图像进行检测,从而获得足球/红球或类似的目标,其次对其进行简单的判断筛选,以确保是我们需要的目标,最后进行定位,返回需要的目标信息。

初始化

1
2
3
4
5
6
7
8
class FootBallDetect(VisualBasis):
def __init__(self, IP, PORT, cameraID=vd.kBottomCamera, resolution=vd.kVGA):
super(FootBallDetect, self).__init__(IP, PORT, cameraID, resolution)
self.ballData = {"centerX": 0, "centerY": 0, "radius": 0}
self.ballPosition = [0, 0, 0]
self.minDist = 100 # int(self._frameHeight/30.0)
self.minRadius = 25
self.maxRadius = 80 # int(self._frameHeight/10.0)

  首先将足球的基本信息和位置信息作为类的属性,然后将霍夫圆检测算法(具体见下文分析)中的参数也作为类的属性。

检测

1
2
3
4
5
6
7
8
9
10
11
def findCirclesV0(self, img, minDist=100, minRadius=25, maxRadius=80):
grayImg = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
circles = cv2.HoughCircles(grayImg, cv.CV_HOUGH_GRADIENT, 1, minDist,
param1=100, param2=20, minRadius=minRadius, maxRadius=maxRadius)
if circles is None:
circles = []
print("no circle")
else:
circles = circles[0, ]

return circles

  初级版采用的检测球的算法是霍夫变换圆检测算法,该算法比较简单,而且opencv有现成的函数,直接调用即可。函数头为:

1
cv2.HoughCircles(image, method, dp, minDist, circles, param1, param2, minRadius, maxRadius)

  image:输入图像,必须为灰度图像,这里使用opencv中的cvtColor()函数将其从BGR转为GRAY即可。method为检测方法,常用的是霍夫梯度法HOUGH_GRADIENT(opencv2的写法为:cv2.cv.CV_HOUGH_GRADIENT)。dp为检测内侧圆心的累加器图像的分辨率于输入图像之比的倒数,如dp=1,累加器和输入图像具有相同的分辨率,如果dp=2,累计器便有输入图像一半那么大的宽度和高度(取1即可)。minDist表示两个圆之间圆心的最小距离。param1method设置的检测方法的对应的参数,对于霍夫梯度法,它表示传递给canny边缘检测算子的高阈值,而低阈值为高阈值的一半。param2method设置的检测方法的对应的参数,对于霍夫梯度法,它表示在检测阶段圆心的累加器阈值,它越小,就越可以检测到更多根本不存在的圆,而它越大的话,能通过检测的圆就更加接近完美的圆形了。minRadius表示圆半径的最小值,maxRadius表示圆半径的最大值。
  由上述分析可知,霍夫圆检测算法的参数十分重要,如果设置的不合理,很难达到预期的要求。

筛选

  将球检测出来后,下一步就是进行筛选并找到我们需要的球。初级版采用的策略是颜色概率法。如下图所示:
筛选圆
  检测出圆形后,外接一个正方形,其边长d2(k*r)r为圆的半径,k为比例。这样就可以通过各个颜色出现的概率判断是否是绿地毯上的红球或足球(黑白球)。
  opencv中有两种常用的颜色通道空间:HSV空间和BGR空间。实际测试下来发现HSV空间比较稳定。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
def selectCircleV0(self, circles):
img = self.frameArray.copy()
HSV = cv2.cvtColor(self.frameArray, cv2.COLOR_BGR2HSV)
circleSelected = []
BWRatiomin = 1.0
k = 1.5

for circle in circles:
centerX, centerY, radius = circle[0], circle[1], circle[2]
initX, initY = int(centerX - int(k * radius)), int(centerY - int(k * radius))
endX, endY = int(centerX + int(k * radius)), int(centerY + int(k * radius))
if initX < 0 or initY < 0 or endX > img.shape[1] or endY > img.shape[0] or radius < 1:
continue

rectBallArea = HSV[initY:endY, initX:endX, :]
HFlat, SFlat, VFlat = rectBallArea[:, :, 0].flatten(), rectBallArea[:, :, 1].flatten(), rectBallArea[:, :, 2].flatten()
size = HFlat.shape[0]
onesArray = np.ones((size,))

# 参考HSV颜色分布表
GScoreHL = np.uint8(35 * onesArray <= HFlat)
GScoreHR = np.uint8(HFlat <= 77 * onesArray)
GScoreSL = np.uint8(43 * onesArray <= SFlat)
GScoreVL = np.uint8(46 * onesArray <= VFlat)
GScore = float(np.sum(GScoreHL * GScoreHR * GScoreSL * GScoreVL))

WScoreSR = np.uint8(SFlat <= 35 * onesArray)
WScoreVL = np.uint8(221 * onesArray <= VFlat)
WScore = float(np.sum(WScoreSR * WScoreVL))

BScoreVR = np.uint8(VFlat <= 46 * onesArray)
BScore = float(np.sum(BScoreVR))

GRatio, BRatio, WRatio = GScore * 1.0 / size, BScore * 1.0 / size, WScore * 1.0 / size
WhiteBlackRatio = BScore * 1.0 / size + WScore * 1.0 / size
if WhiteBlackRatio > 0.1 and np.abs(WhiteBlackRatio - 0.34) < BWRatiomin and GRatio > 0.1:
BWRatiomin = np.abs(WhiteBlackRatio - 0.34)
circleSelected = circle
circleSelected = np.array(circleSelected)
return circleSelected

  首先将图像空间转换为HSV空间,然后对于检测出来的圆利用for循环依次遍历筛选。对于每个圆,首先根据比例k和圆心、半径求出矩形的左上角和右下角,对于超出图像边界的矩形框不予讨论,其次将HSV空间根据通道索引将其分解成3个部分,然后将每个部分根据HSV空间颜色分布表求出每个部分的颜色得分,全部设置为1,然后求和,算出每个颜色的总得分,然后除以总数,即可得到概率,最后根据概率即可判断是否是我们需要的目标。
data

  可以利用opencvcircle()函数将结果显示出来:

1
2
3
4
5
6
def drawCircle(self, img, circle):  
x, y, r = int(circle[0]), int(circle[1]), int(circle[2])
cv2.circle(img, (x, y), r, (0, 0, 255), 2)
cv2.imshow("result", img)
cv2.waitKey(0)
cv2.destroyAllWindows()

  下面进行一个简单的测试,将之前的函数综合调用一下,并画出检测的结果。

1
2
3
4
5
6
7
8
9
10
11
if __name__ == "__main__":
footBallDet = FootBallDetect("192.168.1.101", 9559)
footBallDet.postureProxy.goToPosture("StandInit", 0.5)
img = footBallDet.updateFrame()
circles = footBallDet.findCirclesV0(img)

if circles == []:
print("no ball")
else:
circleSelected = footBallDet.selectCircleV0(circles)
footBallDet.drawCircle(img, circleSelected)

足球检测结果
  结果显示在图中可以大致框出足球范围。

定位

摄像头视角
  灰色矩形区域是摄像头拍摄的图像区域,其中图像原点位于矩形的左上角,坐标轴方向如图所示。蓝点表示摄像头,红点表示球的位置。摄像头的pitch(张角)和yaw(仰角)的正方向如图所示。红线表示摄像头的张角范围(左右视角范围),紫线表示仰角范围(上下视角范围)。球的位置坐标为(x,y)
  如果球正好位于图像的中心位置(320,240),那么摄像头-球-机器人双脚中心正好可以构成一个平面的直角三角形,如下图所示:
摄像头视角
  摄像头离地面的

摄像头参数

  注:初级版只是为了NAO足球比赛的,完整的NAO比赛视觉系统设计参见高级版的博客。

谢谢老板!