基于树莓派3的网球场球童机器人

fantc · 发表于 2018-1-11 23:29:39

本帖最后由 ky123 于 2018-1-31 14:02 编辑

一、网球场球童机器人
1. 原理介绍
在二手路威Rovio机器人的基础上进行改造。在保留其完整的机械结构时，加装了一些而外的部件（后面会详细介绍）。核心框架使用“树莓派+Arduino”的方案。树莓派负责：摄像头的图像处理（定位运算、球场识别、网球识别）、远程图像共享、通过蓝牙交换数据、发送控制指令给下位机。Arduino作为下位机负责：运动传感器数据的计算、电机控制、LED控制等。

2. 系统版本
树莓派系统：Raspbian4.9
开发平台：Python2.7

蓝牙配置

$ sudo apt-get install Python-dev
$ sudo apt-get install libbluetooth-dev
$ sudo pip install pybluez

复制代码

可以使用hciconfig和bluetoothctl查看管理蓝牙，查看本机地址

Opencv2.4.9

$ sudo apt-get update
$ sudo apt-get upgrade
$ sudo apt-get install libopencv-dev
$ sudo apt-get install python-opencv

复制代码

这个不是完整的opencv开发包，不过已经足够使用了。

3. 应用源代码
项目涉及的所有源代码可在帖子末的附件中下载。
3.1. 树莓派和Arduino通信
用USB串口和Arduino进行通信
启动方式如下，剩余代码参见附件FROBOT_PiCamera中的fArduino。

import serial
import serial.tools.list_ports
import threading
import time
import fBluetooth
arduinoSerialReady = True
portList = list(serial.tools.list_ports.comports())
if len(portList) > 0:
port = list(portList[0])
print port
portSerial = port[0]
print portSerial
arduinoSerial = serial.Serial(portSerial, 115200, timeout = 1)
else:
arduinoSerial = False
time.sleep(2)

复制代码

3.2. 树莓派和安卓手机的蓝牙通信
在树莓派的配置中将蓝牙设置为“Enable”。树莓派作为主机并开激活“Discoverable”，手机作为从机主动搜索与树莓派配对连接。由于每次开机后树莓派蓝牙的默认为“Undiscoverable”，需要在程序运行时先使用下列代码，剩余代码参见附件FROBOT_PiCamera中的fBluetooth。

os.system('sudo hciconfig hci0 piscan')

复制代码

3.3. 树莓派图像远程共享
这里没有使用mjpg-streamer，因为想将处理过的图像进行共享。建立一个TCP协议的server socket，当每次收到请求时，将需要共享的图像压缩到一个HTTP消息中作为相应发送给Client。代码参见附件FROBOT_PiCamera中的fStreamServer。
P.S. 如果不用局域网，可以使用natapp的内网穿透服务。

3.4. 二维码卡牌识别
用二维码卡牌模拟一个“充电”位置，或者在充电位置上贴一个二维码卡牌。由于二维码卡牌的尺寸固定，可以使用cv2.solvePnP直接计算卡牌相对于摄像头的旋转矩阵和位移。这里使用SONY的PSVITA的AR卡牌，卡牌的四个顶点从左上逆时针顺序分别定义为：0，1，2，3。简单的卡牌识别、旋转矩阵和位移计算可以参考附件Marker_Python中的代码。

因为手机软件是在Unity3D下开发的，这里提前说说坐标系统一的问题。
下图是左右手坐标系的区别：

世界坐标系通常使用右手坐标系，X、Y轴位于水平面，Z轴垂直于水平面向上。

下图是摄像头（OpenCV）的坐标系（右手坐标系，正向旋转为绕轴的逆时针旋转）。若摄像头垂直于水平面，X、Z轴位于水平面，Y轴垂直于水平面向下，相当于世界坐标系绕X轴旋转-90度。

下图是Unity3D的坐标系（左手坐标系，正向旋转为绕轴的顺时针旋转），X、Z轴位于水平面，Y轴垂直于水平面向上。

不同坐标系下的向量转换很简单，就不多说了，主要说一下旋转。这里以摄像头固定的情形作为说明：
利用cv2.solvePnP获得的是卡牌相对于摄像头的旋转矩阵rMat和位移tVec。前面已经说明过，OpenCV摄像头的坐标系相当于世界右手坐标系绕X轴顺时针旋转了90度，对应的旋转矩阵定义为rMatX。那么卡牌相对于世界坐标的旋转为：R = rMatX * rMat。同一个旋转可以通过按不同的轴顺序旋转而得，如XYZ，ZYX等。将R按照ZYX的旋转顺序转换成欧拉角：

rMatX = np.array([[1, 0, 0], [0, 0, 1], [0, -1, 0]])
r = np.dot(r0, rMatX)
# rotation order ZYX
ex = np.arctan2(r[2,1], r[2,2])
ey = -np.arctan(r[2,0] / np.sqrt(1 -r[2,0] * r[2,0]))
ez = np.arctan2(r[1,0], r[0,0])

复制代码

Unity3D下面通常使用四元数来表示旋转，其定义的欧拉角旋转顺序为ZXY（注意：是Unity3D的坐标轴）。有了前面计算出的ex、ey、ez，可以通过下面的方式直接获得Unity3D下的旋转:

q = Quaternion.AngleAxis(-ez, Vector3.up) * Quaternion.AngleAxis(-ey, Vector3.forward) * Quaternion.AngleAxis(-ex, Vector3.right);

复制代码

有的时候会按照OpenCV的坐标系来给卡牌每个点（左上为第一个点，逆时针顺序）分配实际的坐标，如：

markerCorner3D = np.float32([[-29.5, -29.5, 0], [-29.5, 29.5, 0], [29.5, 29.5, 0], [29.5, -29.5, 0]]) # unit: mm

复制代码

这样的话卡牌本身的坐标系就是下图左边的情况：

若要卡牌的坐标系为图中右边的情况，有两种办法：
1. 不改变上面卡牌实际坐标的分配，给Unity3D下面获得的四元数q右乘Quaternion.AngleAxis(180, Vector3.right)：

q = Quaternion.AngleAxis(-ez, Vector3.up) * Quaternion.AngleAxis(-ey, Vector3.forward) * Quaternion.AngleAxis(-ex, Vector3.right) * Quaternion.AngleAxis(180, Vector3.right);

复制代码

2. 直接改变卡牌实际坐标的分配:

markerCorner3D = np.float32([[-29.5, 29.5, 0], [-29.5, -29.5, 0], [29.5, -29.5, 0], [29.5, 29.5, 0]]) # unit: mm

复制代码

可以到网盘链接文件夹“Version1/Marker”内查看Python源码和Unity3D工程文件。提供一段测试视频来看一下：

如果无法播放，请点击此处在新窗口打开

3.5. 网球场识别
机器人在网球场上需要在初始化时知道自己处于什么位置或者修正在网球场上的位置，使用固定的初始位置显然是不太实际的。因此需要借助网球场的一些固定标记（不同球场唯一不变的就是底线、边线、发球线）来解析出位置。受限于摄像头的高度和视角，最终选择了底线和单双打边线的交点作为有效标记，如下图所示。简单说明下识别的过程，先用颜色过滤掉图像中非白色的部分，利用轮廓识别和拓扑结构分析找出底线和单双打边线。若找不到则使用灰度图转二值图，控制不同的阈值进行尝试。参考帖子附件TennisCourt_Python中的代码。
计算摄像头相对于场地的旋转和位移使用cv2.solvePnP，和上面二维码卡牌的运算类似。稍有不同的是，这里提取到的五个点中，只有两个点有固定的实际坐标：单双打线和底线的交点。solvePnP要求至少四个点才能运算。解决办法：底线的端点可以利用线性比例分配一个实际坐标；选择单打或双打先的端点，分配一个最大可能的实际坐标并逐渐减小，每次通过solvePnP计算并求得摄像头到场地的垂直高度；由于摄像头高度几乎不变，用牛顿插值法找到最合适的结果。

3.6. 网球的识别
初步采用“颜色过滤得到二值图+灰度图中圆形轮廓提取”的方案，根据轮廓范围内黑白的比重判断是否为有效的网球。目前测试下来执行效率比较低，不过还是分享下代码，参见附件TennisBall_Python。

3.7. 依靠单摄像头实现运动轨迹的计算
前面提到可以借助网球场边线的特征实现机器人在网球场位置的识别。当无法捕捉到有效的特征时，也需要知道机器人的实时位置，这就要求具备实时计算运动轨迹的功能。没有使用SLAM实现，主要有如下考虑：SLAM占用较大的计算资源、网球场的应用环境下有固定的特征点供位置修正、精度要求不高。使用的解决方案同SLAM类似，捕捉环境特征点，计算摄像头的姿态和位置，与SLAM相比最大的区别在于：只保留上一帧的特征点，无回环检测。具体流程如下：
- 用二维码卡片做初始化：确定第一帧的初始位置；确定运动轨迹的世界比例尺（因为单摄像头无深度）；计算第一批环境特征点（使用ORB特征点）在空间中的位置
- 捕获每一帧画面先截取特征点
- 和上一帧画面的特征点做交叉配对
- 配对后的特征点中包含已知空间位置的特征点，利用这部分特征点计算当前摄像头的姿态和位置，并进行评估。
- 计算当前一帧相对于第一帧，摄像头在世界坐标系下的转动和位移（即运动轨迹）。
- 计算当前一帧相对于上一帧，摄像头的相对转动和位移。若摄像头转动或移动了一定的范围，则更新当前所有配对的特征点的空间位置。

代码可以在附件CameraFeature中找到完整的代码。
P.S. 前面提到没有安装完整的Opencv开发包，因此没有cv2.drawFeatures函数。在该附件中的FeaturesMatch.py文件中，有相关代码，可直接调用

3.8. Arduino代码
Arduino接收来自树莓派的指令控制电机、LED、网球夹持器。连接了一个红外距离传感器检测是否有网球进入可夹持的范围内（最大距离10cm）。还有一颗MPU6050传感器，受限于Arduino的速度直接调用了自带的DMP驱动（输出40Hz），姿态计算倒是没什么问题，不过在DMP的计算结果下直接进行位移的计算还是不靠谱，漂移很严重。参见附件FROBOT_Arduino。

3.9. 安卓手机软件
软件是在Unity3D下编写的，主要是考虑要在软件中对网球建模、及观测机器人的姿态、位置。利用Eclipse为Unity3D（附件不能上传大于10M的文件，Unity3D的工程文件位于网盘链接在文件夹“Version0”中的FRobot_Unity）编写一个插件（附件FRobot_Andoird_Eclipse）用于蓝牙通信，包括搜索、连接、发送消息给树莓派、初步处理从树莓派接收到的消息。同时通过IP和端口向树莓派发送请求，从接收到的HTTP消息中解析摄像头图像并显示，这里不是视频流而是不停刷新每一帧图片。

3.10. 未完成的部分
- 将场地识别、网球识别、二维码识别、依靠单摄像头实现轨迹计算整合到代码框架中

4. 硬件
4.1. Rovio机器人改造
先给大家看下官方原图和二手的照片

二手的确实便宜，不过到手的时候实在是太脏了，主板、电池等也完全不能用了。比较多的时间花在了清洗和改造上。
硬件清单：
树莓派3B、Arduino Mega 2560、直流电机驱动（x2）、树莓派电池、电机供电3S电池、树莓派摄像头、USB摄像头、MPU6050。
改造过程中没有破坏原来的机械结构，也没有在外壳上做任何加工（如打洞、切割），尽可能使用原有的安装孔。涉及到一些部件，原则上板子的支撑使用有机玻璃板，其它部件则使用3D打印机打印。

在机器人前部安装了网球夹持器：

垫高了机器人内部的空间

机器人内部走线

机器人头部改装的USB摄像头和MPU6050

5. 演示视频
由于SLAM没来得及做，球场、网球识别没来得及整合。只能演示一下控制和图像共享了。

如果无法播放，请点击此处在新窗口打开

视频地址：
http://v.youku.com/v_show/id_XMzMxMDI0ODgzMg==.html?spm=a2h3j.8428770.3416059.1#paction

6. 代码

FROBOT_PiCamera_Python.zip (7.89 KB, 下载次数: 4)

[大赛作品提交] 基于树莓派3的网球场球童机器人

站长推荐 /4