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

星路規(guī)劃：

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

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

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

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

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

機器人的目的地:

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

球的收集和整理：

機器人的收球系統基于開關箱結構，使用多個開關箱來設置和控制系統的速度、轉彎、電機控制和延遲。閘門和滑舌由伺服電機控制，伺服電機由控制變量Gangle和Tangle設置，如下圖所示，每40毫秒檢查和更新一次。

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

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

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

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

四月標簽檢測：

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

四月標簽定位：

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

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

避障:

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

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

球的下降:

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

本文編譯自hackster.io