乒乓球追逐機器人是我們在伊利諾伊大學 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。