這是我和 sszxc 為本學期《機器人軟件工程》課程所做的的課程設計，整個項目開發歷時 1 個月，模型全部在 Webots2020a 中搭建完成。為完成在仿真環境下的超市自主裝卸貨物任務，我們設計了一套復雜的控制器進行運動控制。演示視頻：基於Webots平台的超市補貨機器人。
1. 任務需求
如圖所示，超市中 A,B,C,D 四個貨架上各有12個貨窗，每個貨窗里最多可以擺放1件貨物，現每個貨架上都有至少3個貨窗缺少貨物。
要求設計並制作一台自主超市機器人。機器人從場地的紅色“起點區”出發，完成貨物的補貨操作，最后回到起點。
題目比較開放，我們自行擬定的任務需求是：
- 超市機器人能夠自主定位，能按照既定路線進行巡邏並檢查貨架狀態
- 當發現貨架有空位時能根據空位周圍的貨物自主判別出該空位應該裝填的貨物類型
- 能夠從場地中心的倉庫中識別並抓取待裝填貨物
- 能夠選取最優路徑返回存貨點並完成上貨任務
2.仿真模型搭建
2.1 測試場景搭建
創建仿真測試場景如下，其中 A、B、C、D 分別為四個貨架，4 個橙色點為巡邏定點。
2.2 機器人模型設計
2.2.1 簡介
- 四個麥克納姆輪組成的全向運動底盤，保證了機器人在狹小空間內的移動性能，內置了GPS以及電子羅盤模塊實現自主定位。
- 前后分別設置一顆攝像頭，用於識別貨架上、貨艙內的貨物，並返回相應的類型和位置信息。
- 模仿叉車外形，前部設置了一個二自由度的機械臂，末端抓手設置了一對力傳感器，實現對不同大小的貨物的抓取功能，並能將貨物提升到任意高度，對應題目中的雙層貨架要求。
2.2.2 參數介紹
-
遵照軟件工程復用思想，將例程中“KUKA youBot”的麥克納姆輪打包成PROTO，從而實現麥輪節點的復用。四個麥輪直徑0.10m，通過HingeJoint安裝在車身主體結構上；
-
車身整體尺寸為0.59m×0.40m×0.64m（長寬高），搭建車身時采用模塊化設計，主要分為車身、機械臂兩部分。車身主要由Transform、Shape嵌套而成，主要利用到的幾何體包括Cylinder、Box等；機械臂主要是在Pioneer3所配置的機械臂的基礎上進行適當改造，包括尺寸、電機扭矩及直線電機伸展長度等，同時在機械臂的抓手增加了一對力傳感器，以實現對物品抓取力度的精准控制；
-
機器人配置的攝像頭分辨率為256*128，視角1.8，配備Webots自帶的Recognition API，一定程度上減少了物品識別的難度，使仿真專注於整體軟件功能的開發，體現了離線編程的優勢；
-
為了實現更精確的物理仿真，我們還對機械臂抓手、麥輪的摩擦力參數進行了設置，以保證機器人能夠在當前環境保持正常的運動；此外，還設置了合適的車身重量，以保證車輛的運動性能更加優越、抓取穩定性更好。
2.3 控制器設計
2.3.1 狀態機設計
為方便進行機器人各運動狀態之間的復雜控制，設計狀態機如下：
switch (*main_state)
{
case Init_Pose
case Recognize_Empty
case Arround_Moving
case Grab_Item
case Back_Moving
case TurnBack_To_LoadItem
case Item_Loading
case RunTo_NextShelf
default
}
-
Init_Pose 為初始默認進入工作區狀態，到達指定位置后跳轉到 Recognize_Empty，對貨架進行識別。
-
Recognize_Empty 為識別空貨架狀態，這個狀態主要調用了識別貨架的函數，識別完成后將貨架上的貨物狀態更新到數組，若有缺貨則進行補貨判斷，計算出需要的貨物名稱以及需要放置的位置，然后跳轉到 Arround_Moving 尋找貨品；若沒有缺貨則跳轉到RunTo_NextShelf 狀態，前往下一個貨架進行檢測。
-
Arround_Moving 為環游巡檢狀態，運動過程中一直保持之前暫存的貨物名稱，以對貨倉進行實時檢測判斷，直到找到了合適可抓取的貨物，跳轉至 Grab_Item。
-
Grab_Item 為抓取倉庫貨物狀態，根據暫存的貨物名稱行進到指定貨物的抓取位置，控制機械臂進行抓取，提升后跳轉至 Back_Moving 返程。
-
Back_Moving為回程存貨環游狀態，機器人會攜帶着需要的貨物返回之前的貨架，此時需要進行路徑規划選擇一條最短路線，到達貨架前后跳轉至 TurnBack_To_LoadItem。
-
TurnBack_To_LoadItem 為轉身面向貨架狀態，緊接着跳轉至Item_Loading狀態。
Item_Loading 為上貨狀態，根據貨物需要放置的位置計算出機器人的目標位置，之后完成上貨操作，跳轉至 Init_Pose。 -
RunTo_NextShelf 為前往下一貨架狀態，根據存儲的坐標進行路徑規划，當到達下一貨架定點時，跳轉至 Recognize_Empty。
-
default為錯誤報警狀態。
利用上述九個狀態構成的狀態機，能夠較地完成對機器人裝卸貨物的任務要求，各狀態內部操作的封裝也使得我們的控制器看起來更加簡潔干練。
2.3.2 控制器關鍵函數
1. 基本動作
-
void lift(double position);
給定高度，使機械臂上升直線電機運動到指定位置。 -
void moveFingers(double position);
給定寬度，使機械臂手中左右兩個直線電機運動到指定位置。 -
bool Moveto_CertainPoint(double fin_posture[], double reach_precision);
給定位姿 fin_posture，包括 x、z 坐標與姿態角 angle；給定位控精度 reach_precision；內嵌函數 caculate_tmp_target()，給定目標位姿 double fin_posture[]，根據當前位姿與目標位置，進行位姿的線性插值，差值結果保存在 tmp_posture[] 中作為下一臨時目標位姿。
為了解決羅盤值在2π與0之間跳轉時出現的機器人原地旋轉一圈的冗余運動，因此在姿態角的線性差值前引入邏輯判斷：if (fabs(fin_posture[2] – compass_angle) > PI) 以達到向旋轉角度更小的方向作旋轉運動。
2.空貨架識別
- bool Find_Empty(WbDeviceTag camera)
選定車體后側攝像頭，調用 API 識別貨物，根據貨架大小擬合公式，計算貨架上貨物的位置更新存入數組。具體來說，每格貨架的寬度為 0.25，給機器人設置的定點位於貨架水平方向中間，因此可以由檢測到的貨物與攝像頭的相對位置，來判斷貨物位於哪一列貨架。為高矮兩種貨架高度設置不同的閾值后，也可以根據縱向的相對位置來判斷貨物位於哪一層。
3.尋找貨物
- bool Find_Goods(WbDeviceTag camera,char *good_name,int *item_grasped_id);
選定車體前側攝像頭，給定物品名 good_name，貨物 ID 號 item_grasped_id，通過調用 API 識別並判斷視野中貨物信息（包括 ID、尺寸等），返回是否找到待存放貨物的 bool 類型變量。考慮到機械臂的抓取能力，尋找的過程中濾除了距離機器人過遠的貨物，只考慮當前面對的貨倉區域。
4.對准並抓取
- bool Aim_and_Grasp(int *grasp_state, WbDeviceTag camera, int objectID);
選定車體前側攝像頭，傳入給定的貨物 ID 號來鎖定攝像頭目標。將抓取貨物分為 “調整位置、抓、舉” 三個步驟。首先針對不同貨物的大小計算一個最合適的抓取位置，然后根據攝像頭返回的物體大小粗略地計算手抓的位置，此時激活力傳感器，不斷收緊抓手直到力傳感器達到閾值，表示已經抓緊物體，最后抬升物體達到指定高度，完成抓取功能。
5.返回貨架
- TurnBack_To_LoadItem
這部分算法直接包含在狀態機內，根據貨物的目標位置，計算機器人水平方向相應的偏移距離，給定一個固定的前后偏移，將上貨路徑分為兩步，由此確定機器人的行駛路徑。
3. 總結 - - 超市機器人軟件開發及其質量管理
在本次超市機器人的軟件開發課程設計中，首要任務是完成實現機器人軟件工程的功能性，其次再考慮其他方面的要求。因此，在最開始的開發過程中，在明確我們的需求后，我們將任務重心放在底層基礎功能的開發上，包括底盤及移動輪的設計、抓手的開合及升降運動設計以及物品識別等多個方面。在完成相當一部分的開發之后，轉而進行機器人核心狀態機的設計，並留出各部分的接口，這一任務的完成不僅一定程度上減輕了后期的開發難度，並且還加快了后期的開發速度。
在完整的超市機器人軟件開發的過程中，為了實現其功能性、可靠性、可維護性、可理解性、可測試性及可修改性等多個方面的要求，我們貫徹並采用了如下的措施：
-
模塊化與抽象化技術
在整個超市機器人補貨的復雜任務的前提下，將其拆分為“查貨架”、“尋貨”、“取貨”、“回庫”及“上貨”五個易解決的小任務，實現了任務的模塊化；對於每個獨立的任務，只考慮該任務本身所需要的入參出參，忽略其細節（比如如何實現、如何計算等），實現了任務的抽象化。 -
合適的程序設計語言及風格
實驗中選擇了 C 語言進行開發。相較於 python，由於 C 語言特殊的變量聲明及函數定義要求，使得它能夠使得程序更加地干凈整潔，更加利於軟件的維護。同時，在程序編寫過程中，采用合適的變量名的命名、注釋的使用、規律的數據說明格式、清晰簡潔的語句結構和輸入/輸出格式、有序的層次結構，規避了合作開發過程中的理解困難問題，使得整個項目的開發更加快速。 -
信息隱藏及局部化技術
對於獲取機器人位置、方向角以及商品名的獲取等類似只讀操作，將其封裝成功能函數，只返回需要讀取的內容，而不能對其源頭進行改動，實現了一定程度上的信息隱藏；而對於主狀態的跳轉，將其約束在狀態機函數中，只能在此處進行更改，而不允許在子函數中發生改變，體現了主狀態的局部化。 -
意外情況的防范
當發生主狀態機跳轉錯誤時，打印警告信息 “Error form State Machine！”，起到一定的警示作用。 -
復用技術
在超市機器人的模型設計中，大量使用 USE 操作，實現各節點屬性的復用；同時，將麥克納姆輪的模型封裝成 PROTO 節點，使其能夠為機器人底盤搭建起到重復使用效果。利用這些復用技術一定程度上減少了超市機器人模型搭建的工作量。 -
項目管理
為了實現項目良好的管理，我們采用了 Github 倉庫進行版本控制，每次工作時進行拉取、合並、提交等必須的流程，同時附以簡短的提交日志便於對方了解提交內容。除此之外，每完成一個功能之后，利用騰訊會議及 QQ 屏幕共享等軟件進行匯報並討論改進的方案，同時對接下來的工作進行適當的調整和分配，明確各自的任務，並督促對方及時完成。
4.項目開源
本項目為 貝_塔 與 sszxc 的合作項目，托管在 Github。
項目資源：
- 貝_塔 的倉庫地址：Super_Transbot
- sszxc 的倉庫地址：Super_Transbot
- 演示視頻：基於Webots平台的超市補貨機器人