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

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

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

在 Launchpad 上運(yùn)行的代碼可以在 RED_BOARD/labstarter/final_project_main.c

Raspberry Pi
機(jī)器人的大部分控制發(fā)生在 Raspberry Pi Model 4 上?？刂苹芈泛突净芈肥褂?Python 的模塊在 Pi 上同時(shí)運(yùn)行threading。機(jī)器人的狀態(tài)、姿態(tài)估計(jì)和所需姿態(tài)由兩個(gè)線程訪問(wèn)和/或修改。因此，數(shù)據(jù)鎖用于防止競(jìng)爭(zhēng)條件或當(dāng)兩個(gè)進(jìn)程嘗試同時(shí)操作相同數(shù)據(jù)時(shí)導(dǎo)致的錯(cuò)誤。

基站線程

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

控制線程

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

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

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

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

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

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

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

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

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

球檢測(cè)

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

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

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