编程控制机器人小车是一件富有乐趣的事情，在这篇文章里，我们会探讨如何让一台机器人小车胜任守门员的工作：发现球、向球移动、将球踢开、返回出发点等待下一个球、保持自己在场地内……并且会讨论这些子任务之上的系统状态和状态转换，以及这些子任务之下的原理和方法。

目标🔗

我们的目标很直白，在地面上向小车推一个网球，小车在网球跑到自己身后之前截住它，然后把球撞回来。

考虑到小车的尺寸和力道，以及维修费用，我没有用足球。

设备🔗

本文使用的机器人小车是大疆创新的机甲大师教育套装，由我开发的RoboMasterPy作为SDK和编程框架进行操纵。

机甲大师由四个麦克纳姆轮驱动，向外暴露API，可以让开发者在PC上读（拉取视频流、设备指标）写（远程遥控）设备。车身的前后左右装甲板相对平整，配有碰撞检测器，在重击时会触发。

如果你没有和我一样的设备，别太担心，你仍然可以在这篇文章里获得一些设备无关的一般思路。

自顶向下的设计🔗

粗略地分析，小车在守门时有三个状态：观察、截球、回踢。小车在启动后会始终处于这三个状态之一，并且在满足特定条件时会切换自己的状态。

例如，在观察状态下，如果球离自己足够远，那么就不用急着去截球，继续淡定观察就好，免得被假动作晃到没电，但是如果球足够接近，就该进入截球模式了。再比如，截球成功时应该进入回踢模式，失败时应该进入观察模式。

另外，在进入一个新的模式之前，可能有一些固定操作需要触发，比如进入观察模式前需要让守门员返回球门前（出发点）。

最后，我们应当在游戏开始之前为小车划定好一个它可以随意行驶的安全区，并且确保它只在这个区域里面行驶，免得撞坏它自己或者其他东西。

分而治之🔗

理清了顶层需求之后，我们来分解子任务：

• 全局：保持自己在场地内；确定球的位置；
• 观察：进入观察模式前需要返回出发点；
• 截球、踢球：小车需要向球移动。

下面将按照从易到难的顺序对各个任务进行剖析。

返回出发点和保持自己在场地内🔗

因为设备內建功能的缘故，这个任务没有看上去那么难。

我们将小车行驶的安全区定义为一个长方形，将上电位置定义为原点。大疆的机甲大师内置了软件里程计，能够提供小车当前相对于上电位置的$(x, y, z)$坐标，即向前距离、向右距离和顺时针旋转角度。我们只需要每隔一段时间去查询小车坐标，发现其出界时下令小车返回原点，即：

对于前后深度$D$、左右宽度$W$的安全区，如果$\mid x \mid > W / 2$或者$\mid y \mid > D / 2$，就让小车移动$(-x, -y, -z)$这么远。

实现的时候发现，软件里程计会有一定程度的漂移，即使车子实际没有动，每次的测量值仍会有微小变化，并且小车由于硬件精度无法响应微小值的移动请求（例如向前移动0.06厘米，向右-0.02厘米，再顺时针旋转-0.37°，硬件响应不了这么细微的指令），于是我们把判定逻辑优化为：

如果$\mid x \mid - W / 2 > \epsilon_{距离}$或者$\mid y \mid - D / 2 > \epsilon_{距离}$，就让小车移动$(-x, -y, 0)$，如果$\mid z \mid > \epsilon_{角度}$，就让小车在移动的同时旋转回到上电时的角度，其中$\epsilon_{距离}$和$\epsilon_{角度}$分别为采取回中动作的最小距离和最小角度，对于我的设备，它们分别是1cm和2°.

向球移动🔗

向球移动这个动作发生在截球和回踢这两个状态中，为了便于实现，我们限定截球时小车只会向左或向右运动，回踢时小车在向左或向右移动的同时还会向前运动。

截球🔗

我的第一想法是收集球过去一段时间内在场地上的位置，画出球路的射线和球门相交，相交点就是预测的进球点，让小车前往这个落点等待即可。

这个方法实现起来问题很多：

• 球在场地上的位置并不容易获得，在场地上没有辅助标志的情况下，这需要同时知道球相对于车子的位置和车子在场地上的位置，计算比较复杂，误差累计较大；
• 球走得未必是直线，因为球会旋转，打滑，地面未必平整，同理，小车也未必一直走直线；

要想可靠地截住球，我们就需要对小车进行不只一次控制，并且每次控制都得考虑上次控制得到的结果，这种控制方式称为闭环控制。

既然估计球在场地上的位置不容易，我们直接以球相对于车子的横向位置误差$\Delta y$作为控制的输入量，车子横向的速度$v_y$作为输出量，控制函数$v_y = f(\Delta y)$在输入$\Delta y$后给出$v_y$，车子在下次控制之前以$v_y$速度行进，我们期待在多轮控制后，$\Delta y$会趋向于0，即车子在横向方向上和球处于相同位置且共速。在实现时，控制频率约为每秒10次。

那么如何找到这个函数$f$呢？

简朴的思路是，给$\Delta y$乘上一个常数：

$$f(\Delta y) = K_p \Delta y$$

因为我们的目的是让$\Delta y$趋向于0，常数$K_p > 0$，这样当球在小车右边时，小车的速度会向右，并且球越在右边，小车向右的速度应当越大。这称为比例控制。

比例控制考虑了当下的误差$\Delta y$，但这还不够，精明的守门员还要考虑过去和未来，我们需要的是PID控制（比例-积分-微分控制）：

$$f(\Delta y) = K_p \Delta y + K_i \int_0^t \Delta y \cdot dt + K_d \frac{d}{dt} \Delta y$$

其中：

• $t$为时间；
• $K_p, K_i, K_d$分别为比例单元、积分单元、微分单元的系数；
• $\Delta y$是系统的目前误差；
• $\int_0^t \Delta y \cdot dt$代表了系统过去累计的误差；
• $\frac{d}{dt} \Delta y$代表了系统的未来误差。

别为数学而忧心，如果你正使用Python，simple-pid是一个不错的PID控制器实现。

确定了函数$f$，截球的流程就相对清晰了：

1. 获取当前的$\Delta y$；
2. 计算$v_y = f(\Delta y)$，更新小车的横向速度为$v_y$；
3. 返回步骤1，重复。

根据小车和地面的属性确定合适的系数$K_p, K_i, K_d$后，我们的守门员已经可以截住网球了。

踢球🔗

踢球的核心是，确定时机，给小车加上向前的速度。

我的做法是当球距离小车还有0.3m的前向距离时，就进入踢球模式，这时小车的横向速度仍然使用截球的方式来确定，前向速度则设为0.4m/s.

为什么是0.3m？因为再近一些的话，网球就要从视野中消失了，由于其自身的构造，我的小车看不到在它跟前的东西（想象一个有着巨大油肚的男人挺身低头看自己的脚）。

因为小车要通过碰撞将球踢回去，网球总是要到小车跟前。为了让系统工作稳定，在球脱离视野的3秒内，我们让程序认为球相对于小车的位置没有改变。

球在哪里🔗

重头戏终于来了，在这一节，我们会讨论如何让小车确定球相对于自己的位置，以确定上面提到的$\Delta y$和$\Delta x$。

球在图像中的位置🔗

小车通过视频流上报它看到的一切，视频流其实就是一帧一帧的图像，如何在一幅图像中找到和定位网球呢？

答案很简朴，让我们的程序找到和定位图像中最大号的绿色的圆。

第一步是找到网球的绿色。

我们平时熟悉的颜色表示方案是RGB，即用红绿蓝颜色分量来定义颜色。在这里，为了让颜色范围的表示更加容易，我们使用HSV方案，颜色由色相（Hue）、饱和度（saturation）、值（value）来定义，下面这张图可以让你对这三个分量有个直观的感受：

用OpenCV找出图中HSV值介于$(33, 90, 90)$到$(64, 255, 255)$之间的颜色，效果是这样：

为什么网球的绿色是$(33, 90, 90)$到$(64, 255, 255)$？这个范围是根据图像拍摄的环境尝试出来的，网球自身的颜色是不变的，但图像中网球的颜色受光照、白平衡、阴影等因素的影响。你要做的就是找到一个在当前环境下合适的颜色范围，它不能太小，否则球很容易跟丢，也不能太大，否则程序会把其他像绿色的东西当做网球。

我在自然光，无阳光直射的情况下使用的HSV范围是$(29, 90, 90)$到$(64, 255, 255)$，你可以使用它们作为尝试的起点。

第二步是找到圆形。

我使用OpenCV的approxPolyDP()方法以边数尽量少的多边形去拟合各个轮廓，要求拟合出的多边形的周长和原始轮廓的周长的差距不大于1%，如果一个轮廓需要8条边以上才能达到拟合要求，就将其视作圆形。

结合上面两个步骤，可以找到图像中的绿色圆形，略去大片的绿色和很小块的绿色（它们一般是背景中的其他东西），取边数最多且面积最大的轮廓，使用minEnclosingCircle()方法求出其外接圆，我们就得到了网球在图像中的位置$(x_{图}, y_{图})$和半径$r_{图}$.

值得注意的是图片的坐标系原点在左上角，$x$轴向右，$y$轴向下。

球相对于机器人小车的位置🔗

由于小车和网球都在地面上（共面），小车到网球的前向距离$\Delta x$和侧向距离（向右为正）$\Delta y$唯一确定了球相对于小车的位置。

球到小车相机的直线距离，也就是三角形的斜边，可以用相机针孔模型计算出来：

$$\frac{物体实际尺寸}{物体在相机传感器上的尺寸} = \frac{物体到相机的距离}{焦距}$$

知道四个量中的其中三个就可以求出第四个。

物体在相机传感器上的尺寸和物体在图像中的尺寸成正比，我们可以在固定图像分辨率的情况下，使用卷尺和实际尺寸已知的物体通过多次定距和拍照来求得当前分辨率的焦距。得到焦距之后，在当前分辨率下，网球距离相机的直线距离$L_{实际}$为：

$$L_{实际} = K_{相机} \cdot \frac{r_{实际}}{r_{图}}$$

其中$K_{相机}$为相机当前分辨率的焦距。

我的设备的相机在水平方向的视角为96°，为了简化问题，我们认为视角在图像$x$轴方向上分布均匀，此时网球相对于小车（相机）的偏航角$\theta_{偏航}$为：

$$\theta_{偏航} = 96 \cdot (\frac{x_{图}}{X_{图}} - 0.5)$$

其中，$X_{图}$为图像横向的像素数目，我的为1280.

至此，我们可以计算出小车到网球的前向距离$\Delta x$和侧向距离$\Delta y$：

$$\begin{aligned}\Delta x &= L_{实际} \cdot \cos\theta_{偏航}\\ \Delta y &= L_{实际} \cdot \sin\theta_{偏航} \end{aligned}$$

自顶向下的设计（再一次）🔗

在考虑了上面的各个子任务后，我们能够画出详细的小车的状态和状态变化图：

大功告成🔗

我得说，在大量的尝试，编程和排错后，和机器人小车一起玩的第一局让人非常地兴奋、开心和自豪——我培养出了一名还不算太坏的守门员！

下面是一段我的妻子和守门员小车玩耍的视频，如你所见，为了让我培养出的守门员小车显得不太坏，她放了不少水。:D

我开源了守门员小车的程序代码，你可以点击这里查看。

你也可以在Bilibili观看上面的视频。

祝你和我一样玩得愉快！

致谢🔗

这篇文章中的作品是在机甲大师开发者比赛中孵化的，作者对DJI提供的硬件和技术支持表示感谢。

参考资料🔗

• https://en.wikipedia.org/wiki/PID
• https://en.wikipedia.org/wiki/Pinhole_camera_model
• https://web.stanford.edu/class/cs231a/course_notes/01-camera-models.pdf
• https://unsplash.com/photos/6D2Lmtv_X8A
• https://en.wikipedia.org/wiki/HSL_and_HSV#/media/File:HSV_color_solid_cylinder_saturation_gray.png
• https://en.wikipedia.org/wiki/Pi#/media/File:Archimedes_pi.svg