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

星路規(guī)劃：

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

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

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

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

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

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

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

球的收集和整理：

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

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

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

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

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

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

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

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

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

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

避障:

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

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

球的下降:

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

本文編譯自hackster.io