基于树莓派的ME461：乒乓球追球机器人

乒乓球追逐机器人是我们在伊利诺伊大学 Dan Block 的“机械系统计算机控制”课程的最终项目。在这个项目中，一个带有摄像头的基站定位乒乓球和机器人车，并指示机器人车“踢”球，直到所有球都在边界之外。基站使用 OpenCV 的 Python 包装器进行球检测和 ArUco 标签检测。ArUco 标签用于定义原点和虚拟边界。机器人汽车上的 ArUco 标签允许基站计算机器人相对于原点的位置。差速转向机器人汽车，在一个学期内在一系列实验室中建造，采用德州仪器 TMS320F28379D 处理器，可控制两个驱动电机和一个额外的直流有刷电机，用于驱动踢球板。TMS320F28379D 运行 PI 控制回路，以使用电机编码器数据作为反馈来跟踪指令的正向速度和角速度。Raspberry Pi 4 通过 UART 提供这些所需的速度，以及打开或关闭桨的命令。

MS320F28379D 启动板
Texas Instruments TMS320F28379D Launchpad 通过串行 UART 接口与 Raspberry Pi 连接。通信协议由树莓派触发的四种状态组成：用于调试的回声状态、“发送数据”状态、“接收数据”状态和桨电机控制状态。在接收状态下，Launchpad 从 Raspberry Pi 接收两个浮点值，即所需的前进速度和角速度。在发送状态，Launchpad 发送估计的前进速度和角速度。在桨电机控制状态下，驱动晶体管栅极的 GPIO 输出根据接收到的数据设置为高电平或低电平。

电机编码器用于使用增强型正交编码器脉冲 (eQEP) 外围设备计算两个车轮的角位置。从编码器脉冲转换为线性距离后，左右轮速度使用离散导数计算。前进速度为左右速度的平均值，角速度为左右轮速度之差除以机器人宽度。PI（比例积分）控制用于跟踪所需的前进速度和角速度。左右速度的误差计算为所需速度、转弯率误差和估计的左右速度的函数。控制器计算由每 4 毫秒的定时器中断触发。

在 Launchpad 上运行的代码可以在 RED_BOARD/labstarter/final_project_main.c

Raspberry Pi
机器人的大部分控制发生在 Raspberry Pi Model 4 上。控制回路和基站回路使用 Python 的模块在 Pi 上同时运行threading。机器人的状态、姿态估计和所需姿态由两个线程访问和/或修改。因此，数据锁用于防止竞争条件或当两个进程尝试同时操作相同数据时导致的错误。

基站线程

基站线程通过连接到运行在基站 PC 上的 TCP 服务器进行初始化。机器人的初始状态是通过在几帧上平均机器人的位置数据来计算的，并且控制线程访问的期望位置被初始化为最近球的位置。然后，基站线程不断轮询机器人的位置、最近球的位置以及机器人和球到最近边界的距离。如果最近的球在虚拟边界外或机器人在虚拟边界外超过一英尺，则机器人的状态设置为WAIT。在这种状态下，机器人是静止的，桨电机是关闭的。在另一种情况下，状态设置为KICK。在里面KICK状态，控制线程驱动机器人到最近的球并触发桨电机。来自基站的机器人姿态信息与来自具有卡尔曼滤波器的 Launchpad 的估计前进速度和角速度相融合。每次基站循环迭代都会校正机器人的状态。最后，所需位置更新为最近球的位置。基站环路以 75ms 的周期运行，受基站帧速率的限制。如果没有实时操作系统或计时器中断，确定性计时很困难，但软件计时器允许我们考虑运行循环所需的时间并获得足够准确的计时。

控制线程

控制线程首先初始化 Pi 和 Launchpad 之间的串行通信。然后控制回路以 5ms 的周期连续运行。机器人的位置在控制循环的每次迭代中被估计。从 LaunchPad 读取前向速度和角速度并进行数值积分以计算预测姿态。由于这些估计容易发生漂移，因此使用卡尔曼滤波器使用更准确但响应更慢的基站数据定期对其进行校正。然后，控制回路检查机器人的状态，这是在基站回路中设置的。在该WAIT状态下，所需的前进速度和角速度 0 与关闭桨电机的命令一起发送到 Launchpad。在里面KICK状态，所需的前进速度和角速度由位置控制器确定。位置控制器将当前位置和所需位置作为输入。然后计算所需的标题atan2，并计算当前航向和所需航向之间的误差，以便机器人根据哪个角度较小而顺时针或逆时针转动。所需的角速度是一个常数增益乘以误差。所需的前进速度是恒定增益、当前位置和所需位置之间距离的标准以及防止机器人向前移动直到指向正确方向的窗口函数的乘积。最后，所需的前进速度和角速度饱和到某个最大值。在向 Launchpad 发送所需的速度命令后，控制回路确定桨电机的状态。如果机器人在球的一英尺内或到达球的估计时间少于两秒，则桨叶马达开启。

在树莓派运行的代码发现RASPI/main.py,RASPI/PiThreads.py，RASPI/RobotControl.py和RASPI/SerialCommuncation.py

基站
基站的目的是确定机器人在全局坐标系中的姿态，找到机器人附近球的位置，并托管一个 TCP 服务器，机器人可以从中请求这些数据。基站程序在 PC 上运行，其中一个线程托管 TCP 服务器，而主线程使用罗技 C920X Pro 网络摄像头和OpenCV确定机器人姿势和球位置。为视频的每一帧计算姿势，然后在 TCP 服务器上更新。

标记跟踪和机器人姿态估计

为了跟踪机器人的位置并建立全局坐标系，使用带有ArUco模块的OpenCV跟踪基准标记，该模块提供生成、跟踪和估计 ArUco 基准标记位姿的功能。

为了估计标记的姿态，首先找到每个标记的角。使用标记的角和边长，可以根据旋转和平移向量确定相机相对于每个标记的姿态。可以在此处找到有关标记检测和姿势估计的更多信息。

为了准确估计标记的姿态，需要考虑相机的内在参数和失真系数。这些参数是通过执行相机校准确定的。按照本教程，使用 ChArUco 板校准相机，该板将用于相机校准的典型棋盘图案与 ArUco 标记相结合。这允许校准板在用于校准的图像中被遮挡或部分可见，从而使过程更容易。用于相机校准的代码可以在 中找到BASE_STATION/cameraCalibration.py。

为了建立世界坐标系，在地板上放置了四个标记，左下角的标记被定义为原点。接下来，使用 将相机相对于原点的旋转矢量转换为旋转矩阵cv.Rodrigues()。将反转旋转矩阵乘以所有标记的平移向量，得出每个标记相对于原点的位置。标记在全局坐标系平面中的方向是通过首先使用 将每个标记的旋转向量转换为旋转矩阵cv.Rodrigues()，然后使用提取每个旋转矩阵的欧拉角来获得的cv.RQDecomp3x3()。这会导致全局坐标系中任何标记的姿态。可以在此处找到有关坐标变换的更多信息。

使用全局坐标系中地板上四个标记的姿态及其位置和像素坐标，可以使用 找到透视变换矩阵cv.findHomography()，将像素坐标映射到全局坐标。这假设所有像素坐标都在由四个标记（即地板）建立的平面上。因此，它不考虑平面上方的高度。

球检测

为了检测视频帧中的球，通过查找帧中处于特定 HSV 值范围内的部分来创建二进制掩码。这仅隔离了位于该范围内的图像部分，从而隔离了帧中的球。由于球的每种颜色的 HSV 范围不同，因此离线创建了对应于球的四种颜色（蓝色、橙色、粉红色和绿色）的四个二元掩码。接下来，找到二值掩码中的闭合轮廓，然后按区域过滤以过滤掉其他可能在HSV范围内但太大或太小而不能成为球的对象。通过找到轮廓的最小包围圆的中心来确定球的像素坐标。由于球比较靠近地板，cv.perspectiveTransform() 以最小的错误。

确定目标球的逻辑如下： 1）找到边界内的第一个球（由形成世界坐标系的标记设置）并将其设置为目标 2）如果到其他球的距离为小于到当前目标的距离减去 1 英尺，将其设为新目标 3) 比较所有球后，将目标球的位置存储在 TCP 服务器上以供机器人访问。以这种方式进行瞄准以减轻机器人在与机器人距离大致相同的两个球之间切换的情况。

在基站上运行的代码可以在BASE_STATION/main.py和 中找到BASE_STATION/BaseStationControl.py。