下面描述了流程中使用的各個(gè)步驟和功能是如何工作的，以及如何對(duì)它們進(jìn)行測(cè)試。

星路規(guī)劃：

機(jī)器人的尋路系統(tǒng)基于A*算法，該算法能有效地計(jì)算出有障礙物環(huán)境中的最優(yōu)路徑。該系統(tǒng)實(shí)現(xiàn)為分布式架構(gòu)，其中計(jì)算密集型尋路運(yùn)行在機(jī)器人的樹(shù)莓派4上，而機(jī)器人的TI板執(zhí)行計(jì)算路徑。

機(jī)器人使用2D網(wǎng)格表示在其環(huán)境中導(dǎo)航，其中地圖(14英尺x 13英尺的單元)區(qū)分障礙物(“x”)和自由空間(“0”)。該映射存儲(chǔ)在反映環(huán)境當(dāng)前狀態(tài)的字符數(shù)組(map和mapstart)中。當(dāng)機(jī)器人需要計(jì)算新的路徑時(shí)，它會(huì)將地圖數(shù)據(jù)壓縮成更有效的二進(jìn)制格式進(jìn)行傳輸。由于我們的項(xiàng)目是在走廊中執(zhí)行的，所以我們需要重新創(chuàng)建一個(gè)不同于初始代碼的新地圖。走廊的90度角和預(yù)定義的盒子障礙物通過(guò)在地圖上設(shè)置帶有“x”的障礙物來(lái)模擬。

當(dāng)機(jī)器人確定它需要一條新路徑(由StartAstar標(biāo)志觸發(fā))時(shí)，尋路過(guò)程開(kāi)始。機(jī)器人將其當(dāng)前位置、目的地坐標(biāo)和壓縮地圖數(shù)據(jù)打包在SendAStarInfo結(jié)構(gòu)中，并通過(guò)串行通信將其發(fā)送給樹(shù)莓派4。在此處理期間，機(jī)器人暫停其運(yùn)動(dòng)(由AstarRunning標(biāo)志控制)，直到接收到新的路徑。

一旦外部系統(tǒng)計(jì)算出路徑，它就會(huì)返回一系列避開(kāi)障礙物的路徑點(diǎn)。機(jī)器人處理這些信息，并將路徑坐標(biāo)存儲(chǔ)在pathRow和pathCol數(shù)組中，系統(tǒng)最多支持40個(gè)路徑點(diǎn)。使用statpos和numpts等變量跟蹤機(jī)器人沿著這條路徑的進(jìn)程，并使用xy_control函數(shù)處理路徑點(diǎn)之間的移動(dòng)。

進(jìn)行了廣泛的測(cè)試，以調(diào)整尋路參數(shù)，確保在各種環(huán)境中可靠導(dǎo)航。另一個(gè)關(guān)鍵方面是在不同的坐標(biāo)系之間進(jìn)行轉(zhuǎn)換。機(jī)器人在全局坐標(biāo)系中運(yùn)行，而a *算法使用基于網(wǎng)格的表示。這個(gè)轉(zhuǎn)換是通過(guò)一個(gè)特定的映射過(guò)程來(lái)處理的，該過(guò)程解決了幾個(gè)挑戰(zhàn)：反轉(zhuǎn)路徑序列(因?yàn)閍 *返回從目的地到起點(diǎn)的路徑)，將網(wǎng)格列直接轉(zhuǎn)換為x坐標(biāo)，并使用操作10 - pathRow翻轉(zhuǎn)y坐標(biāo)系統(tǒng)，以說(shuō)明網(wǎng)格和機(jī)器人坐標(biāo)系統(tǒng)之間的不同原點(diǎn)。如果沒(méi)有這種仔細(xì)的坐標(biāo)轉(zhuǎn)換，機(jī)器人將試圖導(dǎo)航到物理世界中不正確的位置。通過(guò)適當(dāng)?shù)貥蚪舆@些坐標(biāo)系，代碼確保當(dāng)機(jī)器人執(zhí)行xy_control()函數(shù)在路徑點(diǎn)之間移動(dòng)時(shí)，它準(zhǔn)確地遵循由A*算法計(jì)算的最佳路徑，同時(shí)成功地避開(kāi)其環(huán)境中的障礙物。

機(jī)器人的目的地:

機(jī)器人陣列與我們?cè)趯?shí)驗(yàn)室里使用的非常相似。主要區(qū)別在于，Astar算法使用的是標(biāo)準(zhǔn)的robotdest數(shù)組，所以為了在Astar之后迭代路徑點(diǎn)，我們需要?jiǎng)?chuàng)建一個(gè)新的robotdest數(shù)組，并在單獨(dú)的xy_control中實(shí)現(xiàn)。為了實(shí)現(xiàn)第二個(gè)控件，我們使用了新的數(shù)組、一個(gè)新的statpos變量和一個(gè)路徑選擇標(biāo)志來(lái)在控件之間切換。

球的收集和整理：

機(jī)器人的收球系統(tǒng)基于開(kāi)關(guān)箱結(jié)構(gòu)，使用多個(gè)開(kāi)關(guān)箱來(lái)設(shè)置和控制系統(tǒng)的速度、轉(zhuǎn)彎、電機(jī)控制和延遲。閘門(mén)和滑舌由伺服電機(jī)控制，伺服電機(jī)由控制變量Gangle和Tangle設(shè)置，如下圖所示，每40毫秒檢查和更新一次。

機(jī)器人通過(guò)使用狀態(tài)機(jī)來(lái)控制機(jī)器人所處的狀態(tài)以及根據(jù)條件采取的行動(dòng)，從而在各種功能之間進(jìn)行切換。對(duì)于避障、撿球和分揀功能，設(shè)置某些參數(shù)以觸發(fā)機(jī)器人狀態(tài)根據(jù)最小條件變化。機(jī)器人的第一個(gè)狀態(tài)是檢查狀態(tài)，檢查機(jī)器人上的任何傳感器是否滿(mǎn)足相應(yīng)狀態(tài)的最低要求。第一個(gè)檢查是避障，如果機(jī)器人檢測(cè)到前方6英寸的障礙物，則使用激光雷達(dá)傳感器使機(jī)器人減速，并切換到右墻跟隨狀態(tài)。第二次和第三次檢查，用相機(jī)掃描洋紅色和綠色高爾夫球，用OpenMV處理圖像，確定哪個(gè)在它前面。當(dāng)機(jī)器人在2秒內(nèi)檢測(cè)到至少6個(gè)相應(yīng)顏色的像素時(shí)，切換到粉紅色和綠色，然后切換到案例20和30。

品紅球的球收集和分類(lèi)在案例20到26中完成，并遵循跟蹤球、決定收集倉(cāng)、抓取球、關(guān)閉門(mén)和重置收集器的過(guò)程。案例20，如下圖所示，是跟蹤案例，跟蹤并將球居中于機(jī)器人前方。在這種狀態(tài)下，機(jī)器人的相機(jī)視覺(jué)中心被定義為相機(jī)列值的中間的球心。然后，代碼讓機(jī)器人遵循一系列if語(yǔ)句，這些語(yǔ)句將設(shè)置vref和turn值來(lái)跟隨并接近球的位置，直到它在機(jī)器人前面的設(shè)定距離(由它距離視覺(jué)范圍頂部的像素?cái)?shù)決定)。

一旦達(dá)到最小行閾值，機(jī)器人狀態(tài)變?yōu)?1，并遵循如下所示的結(jié)構(gòu)。狀態(tài)21是一個(gè)簡(jiǎn)單的延遲狀態(tài)，它會(huì)讓機(jī)器人停止3秒，讓收集箱中的球沉淀下來(lái)，防止它們?cè)陂T(mén)打開(kāi)時(shí)滾出。狀態(tài)22通過(guò)改變控制門(mén)的角度打開(kāi)門(mén)，并設(shè)置左倉(cāng)的舌角進(jìn)行分揀，然后變?yōu)闋顟B(tài)24。在這種情況下，機(jī)器人向前移動(dòng)1秒，然后進(jìn)入狀態(tài)25，狀態(tài)25繼續(xù)向前移動(dòng)機(jī)器人一秒，同時(shí)關(guān)閉它身后的門(mén)。最后，案例26用了另外2秒來(lái)重置舌和門(mén)的角度，然后繼續(xù)前進(jìn)并收集更多的球。第二個(gè)綠色球的收集過(guò)程與此相同，但使用為其設(shè)置的顏色閾值，并將收集到的球匯集到正確的容器中。

進(jìn)行了多次測(cè)試，以調(diào)整收集、延遲和角度的閾值，并確?？煽亢涂芍貜?fù)的收集過(guò)程。顏色閾值是通過(guò)將高爾夫球放置在適當(dāng)?shù)沫h(huán)境中，并使用相機(jī)的視覺(jué)來(lái)捕獲球的圖像，并進(jìn)行處理以隔離顏色，并考慮燈光和反射的眩光。收集測(cè)試具有多種用途，并且有助于對(duì)機(jī)器人進(jìn)行故障排除以避免干擾。標(biāo)準(zhǔn)的測(cè)試程序是將兩個(gè)彩色高爾夫球放在機(jī)器人的路徑周?chē)?，看看它如何跟蹤和收集它們。在最初的測(cè)試中，門(mén)的位置會(huì)阻擋攝像機(jī)對(duì)球的視線，阻止它離開(kāi)控制狀態(tài)，導(dǎo)致它卡在球的前面。為了說(shuō)明這一點(diǎn)，打開(kāi)狀態(tài)的柵極角進(jìn)一步打開(kāi)，以清除相機(jī)視野。測(cè)試發(fā)現(xiàn)的另一個(gè)問(wèn)題是，在收集過(guò)程中，由于球在垃圾箱中的動(dòng)量，一些球會(huì)滾出來(lái)。因此，延遲情況被添加到收集步驟中，以確保球在收集過(guò)程中和門(mén)打開(kāi)時(shí)不會(huì)繼續(xù)向前滾動(dòng)。為了進(jìn)一步說(shuō)明滾動(dòng)情況，增加了25個(gè)程序，以關(guān)閉并舀回球到垃圾箱中，以防它們?cè)O(shè)法向前滾動(dòng)。最后，當(dāng)能夠可靠地、重復(fù)地按照預(yù)期收集各種球，并進(jìn)行分類(lèi)，而不被卡住或溢出球時(shí)，就可以確定球的收集和分選完成。

四月標(biāo)簽檢測(cè)：

為了允許機(jī)器人檢測(cè)四月標(biāo)記，提供了一個(gè)main.py文件。唯一需要做的更改是設(shè)置runtag=1和set tag。Id <=所需的標(biāo)簽數(shù)。

四月標(biāo)簽定位：

定位算法從檢測(cè)航位推算箱結(jié)構(gòu)中的四月標(biāo)簽開(kāi)始。如果之前沒(méi)有檢測(cè)到四月標(biāo)簽，機(jī)器人切換到60號(hào)箱子，停止運(yùn)動(dòng)半秒，由計(jì)數(shù)變量計(jì)時(shí)。然后另一個(gè)開(kāi)關(guān)將Robot設(shè)置為case 61，其中vref又被設(shè)置為1.0半秒(由count變量決定)。這是為了使機(jī)器人更靠近四月標(biāo)簽，以提高OpenMV相機(jī)內(nèi)置的z距離和z角度四月標(biāo)簽檢測(cè)的精度。最后，機(jī)器人切換到機(jī)箱62，這是四月標(biāo)簽和相機(jī)之間距離計(jì)算的位置。

機(jī)器人的速度和轉(zhuǎn)彎速度設(shè)置為0。開(kāi)始對(duì)相機(jī)和四月標(biāo)簽之間的z距離進(jìn)行求和操作(該操作將為負(fù)值，因?yàn)樗鼘⑺脑聵?biāo)簽視為初始點(diǎn)，而四月標(biāo)簽視為與該點(diǎn)之間的負(fù)距離)。這個(gè)總和將持續(xù)5秒，之后它將除以被計(jì)數(shù)的次數(shù)(5000次)，由count變量計(jì)時(shí)，并將比例因子設(shè)為12，以便以英尺為單位計(jì)算平均值。這個(gè)平均值減少了相機(jī)在z距離上產(chǎn)生的噪聲誤差。四月標(biāo)簽的預(yù)期x和y坐標(biāo)在全局變量中設(shè)置，并且根據(jù)機(jī)器人是否面向全局正x或負(fù)x方向?qū)⒂?jì)算機(jī)器人的當(dāng)前位置。這些方程式的詳細(xì)描述如下。機(jī)器人的當(dāng)前位置設(shè)置為這些計(jì)算點(diǎn)，使用x = xpred[0][0]和y = xpred[1][0]來(lái)解釋漂移，設(shè)置計(jì)數(shù)器變量，因此標(biāo)簽不會(huì)被重復(fù)檢測(cè)，然后機(jī)器人切換回鋸齒模式。

避障:

避障遵循一個(gè)簡(jiǎn)單的右墻跟隨程序，以避免在其路徑上的任何東西。當(dāng)激光雷達(dá)在機(jī)器人前方6英寸處檢測(cè)到障礙物時(shí)，它將一個(gè)名為right_wall_follow_state的標(biāo)志設(shè)置為1，并切換到下圖所示的情形10。當(dāng)標(biāo)志為1時(shí)，機(jī)器人將根據(jù)前方激光雷達(dá)距離向左轉(zhuǎn)彎，一旦前方距離大于最小閾值，機(jī)器人將停止左轉(zhuǎn)，標(biāo)志將變?yōu)?。當(dāng)它為2時(shí)，機(jī)器人將切換到右墻跟隨，并根據(jù)設(shè)定的距離和右側(cè)前方激光雷達(dá)的值平衡與右墻的距離。

旋轉(zhuǎn)速度受到一個(gè)旋轉(zhuǎn)閾值的限制，該閾值使數(shù)值飽和，并防止旋轉(zhuǎn)過(guò)快。如果前方出現(xiàn)低于閾值的另一面墻，機(jī)器人將再次轉(zhuǎn)向并重復(fù)該過(guò)程，直到整整5秒過(guò)去，并且與前方讀數(shù)至少有1.5英尺的距離，然后返回狀態(tài)1。通過(guò)在正方形測(cè)試區(qū)域中測(cè)試正確的墻壁，并在其路徑中放置多個(gè)障礙物并測(cè)量其避開(kāi)這些障礙物的性能，可以微調(diào)閾值和值。除了激光雷達(dá)的前方和右側(cè)讀數(shù)外，還定義了多個(gè)其他范圍，如下圖所示，以便進(jìn)行其他避障過(guò)程或檢查參數(shù)。這些值也可以用來(lái)調(diào)整機(jī)器人在走廊和通道的中心。

球的下降:

與大多數(shù)其他流程一樣，創(chuàng)建了一個(gè)案例結(jié)構(gòu)，這次我們?cè)谧x取與丟棄區(qū)相對(duì)應(yīng)的特定AprilTag時(shí)進(jìn)入了案例結(jié)構(gòu)。這些掉落區(qū)域是顏色協(xié)調(diào)的，所以每個(gè)標(biāo)簽都有自己的盒子結(jié)構(gòu)，可以掉落綠色或品紅的高爾夫球。該結(jié)構(gòu)包括一個(gè)方法，一個(gè)延遲(防止高爾夫球滾動(dòng)太多)，一個(gè)門(mén)打開(kāi)和舌驅(qū)動(dòng)，一個(gè)反向箱，和一個(gè)門(mén)關(guān)閉箱。每個(gè)case結(jié)構(gòu)還需要一個(gè)標(biāo)志為‘ 0 ’，在丟棄序列期間設(shè)置為1。

本文編譯自hackster.io