基于樹莓派的ME461：乒乓球追球機器人

乒乓球追逐機器人是我們在伊利諾伊大學(xué) Dan Block 的“機械系統(tǒng)計算機控制”課程的最終項目。在這個項目中，一個帶有攝像頭的基站定位乒乓球和機器人車，并指示機器人車“踢”球，直到所有球都在邊界之外。基站使用 OpenCV 的 Python 包裝器進行球檢測和 ArUco 標(biāo)簽檢測。ArUco 標(biāo)簽用于定義原點和虛擬邊界。機器人汽車上的 ArUco 標(biāo)簽允許基站計算機器人相對于原點的位置。差速轉(zhuǎn)向機器人汽車，在一個學(xué)期內(nèi)在一系列實驗室中建造，采用德州儀器 TMS320F28379D 處理器，可控制兩個驅(qū)動電機和一個額外的直流有刷電機，用于驅(qū)動踢球板。TMS320F28379D 運行 PI 控制回路，以使用電機編碼器數(shù)據(jù)作為反饋來跟蹤指令的正向速度和角速度。Raspberry Pi 4 通過 UART 提供這些所需的速度，以及打開或關(guān)閉槳的命令。

MS320F28379D 啟動板
Texas Instruments TMS320F28379D Launchpad 通過串行 UART 接口與 Raspberry Pi 連接。通信協(xié)議由樹莓派觸發(fā)的四種狀態(tài)組成：用于調(diào)試的回聲狀態(tài)、“發(fā)送數(shù)據(jù)”狀態(tài)、“接收數(shù)據(jù)”狀態(tài)和槳電機控制狀態(tài)。在接收狀態(tài)下，Launchpad 從 Raspberry Pi 接收兩個浮點值，即所需的前進速度和角速度。在發(fā)送狀態(tài)，Launchpad 發(fā)送估計的前進速度和角速度。在槳電機控制狀態(tài)下，驅(qū)動晶體管柵極的 GPIO 輸出根據(jù)接收到的數(shù)據(jù)設(shè)置為高電平或低電平。

電機編碼器用于使用增強型正交編碼器脈沖 (eQEP) 外圍設(shè)備計算兩個車輪的角位置。從編碼器脈沖轉(zhuǎn)換為線性距離后，左右輪速度使用離散導(dǎo)數(shù)計算。前進速度為左右速度的平均值，角速度為左右輪速度之差除以機器人寬度。PI（比例積分）控制用于跟蹤所需的前進速度和角速度。左右速度的誤差計算為所需速度、轉(zhuǎn)彎率誤差和估計的左右速度的函數(shù)。控制器計算由每 4 毫秒的定時器中斷觸發(fā)。

在 Launchpad 上運行的代碼可以在 RED_BOARD/labstarter/final_project_main.c

Raspberry Pi
機器人的大部分控制發(fā)生在 Raspberry Pi Model 4 上。控制回路和基站回路使用 Python 的模塊在 Pi 上同時運行threading。機器人的狀態(tài)、姿態(tài)估計和所需姿態(tài)由兩個線程訪問和/或修改。因此，數(shù)據(jù)鎖用于防止競爭條件或當(dāng)兩個進程嘗試同時操作相同數(shù)據(jù)時導(dǎo)致的錯誤。

基站線程

基站線程通過連接到運行在基站 PC 上的 TCP 服務(wù)器進行初始化。機器人的初始狀態(tài)是通過在幾幀上平均機器人的位置數(shù)據(jù)來計算的，并且控制線程訪問的期望位置被初始化為最近球的位置。然后，基站線程不斷輪詢機器人的位置、最近球的位置以及機器人和球到最近邊界的距離。如果最近的球在虛擬邊界外或機器人在虛擬邊界外超過一英尺，則機器人的狀態(tài)設(shè)置為WAIT。在這種狀態(tài)下，機器人是靜止的，槳電機是關(guān)閉的。在另一種情況下，狀態(tài)設(shè)置為KICK。在里面KICK狀態(tài)，控制線程驅(qū)動機器人到最近的球并觸發(fā)槳電機。來自基站的機器人姿態(tài)信息與來自具有卡爾曼濾波器的 Launchpad 的估計前進速度和角速度相融合。每次基站循環(huán)迭代都會校正機器人的狀態(tài)。最后，所需位置更新為最近球的位置?；经h(huán)路以 75ms 的周期運行，受基站幀速率的限制。如果沒有實時操作系統(tǒng)或計時器中斷，確定性計時很困難，但軟件計時器允許我們考慮運行循環(huán)所需的時間并獲得足夠準(zhǔn)確的計時。

控制線程

控制線程首先初始化 Pi 和 Launchpad 之間的串行通信。然后控制回路以 5ms 的周期連續(xù)運行。機器人的位置在控制循環(huán)的每次迭代中被估計。從 LaunchPad 讀取前向速度和角速度并進行數(shù)值積分以計算預(yù)測姿態(tài)。由于這些估計容易發(fā)生漂移，因此使用卡爾曼濾波器使用更準(zhǔn)確但響應(yīng)更慢的基站數(shù)據(jù)定期對其進行校正。然后，控制回路檢查機器人的狀態(tài)，這是在基站回路中設(shè)置的。在該WAIT狀態(tài)下，所需的前進速度和角速度 0 與關(guān)閉槳電機的命令一起發(fā)送到 Launchpad。在里面KICK狀態(tài)，所需的前進速度和角速度由位置控制器確定。位置控制器將當(dāng)前位置和所需位置作為輸入。然后計算所需的標(biāo)題atan2，并計算當(dāng)前航向和所需航向之間的誤差，以便機器人根據(jù)哪個角度較小而順時針或逆時針轉(zhuǎn)動。所需的角速度是一個常數(shù)增益乘以誤差。所需的前進速度是恒定增益、當(dāng)前位置和所需位置之間距離的標(biāo)準(zhǔn)以及防止機器人向前移動直到指向正確方向的窗口函數(shù)的乘積。最后，所需的前進速度和角速度飽和到某個最大值。在向 Launchpad 發(fā)送所需的速度命令后，控制回路確定槳電機的狀態(tài)。如果機器人在球的一英尺內(nèi)或到達球的估計時間少于兩秒，則槳葉馬達開啟。

在樹莓派運行的代碼發(fā)現(xiàn)RASPI/main.py,RASPI/PiThreads.py，RASPI/RobotControl.py和RASPI/SerialCommuncation.py

基站
基站的目的是確定機器人在全局坐標(biāo)系中的姿態(tài)，找到機器人附近球的位置，并托管一個 TCP 服務(wù)器，機器人可以從中請求這些數(shù)據(jù)。基站程序在 PC 上運行，其中一個線程托管 TCP 服務(wù)器，而主線程使用羅技 C920X Pro 網(wǎng)絡(luò)攝像頭和OpenCV確定機器人姿勢和球位置。為視頻的每一幀計算姿勢，然后在 TCP 服務(wù)器上更新。

標(biāo)記跟蹤和機器人姿態(tài)估計

為了跟蹤機器人的位置并建立全局坐標(biāo)系，使用帶有ArUco模塊的OpenCV跟蹤基準(zhǔn)標(biāo)記，該模塊提供生成、跟蹤和估計 ArUco 基準(zhǔn)標(biāo)記位姿的功能。

為了估計標(biāo)記的姿態(tài)，首先找到每個標(biāo)記的角。使用標(biāo)記的角和邊長，可以根據(jù)旋轉(zhuǎn)和平移向量確定相機相對于每個標(biāo)記的姿態(tài)?？梢栽诖颂幷业接嘘P(guān)標(biāo)記檢測和姿勢估計的更多信息。

為了準(zhǔn)確估計標(biāo)記的姿態(tài)，需要考慮相機的內(nèi)在參數(shù)和失真系數(shù)。這些參數(shù)是通過執(zhí)行相機校準(zhǔn)確定的。按照本教程，使用 ChArUco 板校準(zhǔn)相機，該板將用于相機校準(zhǔn)的典型棋盤圖案與 ArUco 標(biāo)記相結(jié)合。這允許校準(zhǔn)板在用于校準(zhǔn)的圖像中被遮擋或部分可見，從而使過程更容易。用于相機校準(zhǔn)的代碼可以在 中找到BASE_STATION/cameraCalibration.py。

為了建立世界坐標(biāo)系，在地板上放置了四個標(biāo)記，左下角的標(biāo)記被定義為原點。接下來，使用 將相機相對于原點的旋轉(zhuǎn)矢量轉(zhuǎn)換為旋轉(zhuǎn)矩陣cv.Rodrigues()。將反轉(zhuǎn)旋轉(zhuǎn)矩陣乘以所有標(biāo)記的平移向量，得出每個標(biāo)記相對于原點的位置。標(biāo)記在全局坐標(biāo)系平面中的方向是通過首先使用 將每個標(biāo)記的旋轉(zhuǎn)向量轉(zhuǎn)換為旋轉(zhuǎn)矩陣cv.Rodrigues()，然后使用提取每個旋轉(zhuǎn)矩陣的歐拉角來獲得的cv.RQDecomp3x3()。這會導(dǎo)致全局坐標(biāo)系中任何標(biāo)記的姿態(tài)?？梢栽诖颂幷业接嘘P(guān)坐標(biāo)變換的更多信息。

使用全局坐標(biāo)系中地板上四個標(biāo)記的姿態(tài)及其位置和像素坐標(biāo)，可以使用 找到透視變換矩陣cv.findHomography()，將像素坐標(biāo)映射到全局坐標(biāo)。這假設(shè)所有像素坐標(biāo)都在由四個標(biāo)記（即地板）建立的平面上。因此，它不考慮平面上方的高度。

球檢測

為了檢測視頻幀中的球，通過查找?guī)刑幱谔囟?HSV 值范圍內(nèi)的部分來創(chuàng)建二進制掩碼。這僅隔離了位于該范圍內(nèi)的圖像部分，從而隔離了幀中的球。由于球的每種顏色的 HSV 范圍不同，因此離線創(chuàng)建了對應(yīng)于球的四種顏色（藍色、橙色、粉紅色和綠色）的四個二元掩碼。接下來，找到二值掩碼中的閉合輪廓，然后按區(qū)域過濾以過濾掉其他可能在HSV范圍內(nèi)但太大或太小而不能成為球的對象。通過找到輪廓的最小包圍圓的中心來確定球的像素坐標(biāo)。由于球比較靠近地板，cv.perspectiveTransform() 以最小的錯誤。

確定目標(biāo)球的邏輯如下： 1）找到邊界內(nèi)的第一個球（由形成世界坐標(biāo)系的標(biāo)記設(shè)置）并將其設(shè)置為目標(biāo) 2）如果到其他球的距離為小于到當(dāng)前目標(biāo)的距離減去 1 英尺，將其設(shè)為新目標(biāo) 3) 比較所有球后，將目標(biāo)球的位置存儲在 TCP 服務(wù)器上以供機器人訪問。以這種方式進行瞄準(zhǔn)以減輕機器人在與機器人距離大致相同的兩個球之間切換的情況。

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