基于STM32的智能物流機器人

黃學達　楊俊杰　侯志鵬等

關鍵詞：STM32；OpenMV；PID；機械臂；物流機器人

搬運機器人可代替人工在高溫、高壓、多粉塵等危險環境中作業，節省大量人力資源，大幅提高工作效率。在未來智慧工廠建設中，智能物流搬運機器人技術起著舉足輕重的作用[1]。

鑒于智能物流搬運機器人將來在制造業發展中勢必占有較重要的地位，本文設計實現了一種基于STM32的智能物流機器人系統，通過智能車為平臺搭載機械臂和攝像頭，運用AprilTag 標定跟蹤與PID 控制[2] 實現機器人的移動與跟蹤抓取。最終實現自動導航以及智能識別并搬運物品的功能。

1 機器人工作原理

1.1 PID控制算法

PID 控制是指比例、積分、微分控制。將系統的測量反饋經過比例、積分、微分3 種運算后，疊加至輸入中，達到控制的目的。其原理簡單、易于實現，被應用于多種領域。常用的PID 算法有兩種，一種是位置式PID，另一種是增量式PID，兩者應用場合不同，各有優點。

位置式PID 算法的表達式為：

2 機器人硬件設計

2.1 硬件總體設計

本文基于STM32 設計的智能物流機器人系統，選取性能較好的硬件配置作為系統運行的外部環境支撐，根據搬運機器人機械系統功能和技術指標，設置硬件總體結構。系統硬件設備包括攝像頭模塊、OLED 顯示器、轉向舵機、直流電機，系統硬件結構圖，如圖4 所示。

智能物流機器人系外部設備較多，各設備之間工作電壓不一致。本文采用了LM2596 和AMS1117 兩顆降壓芯片，調整系統運行時的電壓、攝像頭模塊和機械臂是機器人抓取目標的基礎硬件設備、直流電機和轉向舵機能控制機器人的移動和方向，OLED 顯示系統運行時的各種參數，是調試時的重要依據。

2.2 移動與轉向設計

本文所設計的機器人移動采用兩路帶光電編碼器的直流電機，通過STM32 輸出PWM 波，同時接收編碼器計數，經處理后達到控制調速目的。OLED 模塊顯示設定速讀和實時速度，選擇TB6612 模塊作為直流電機驅動給電機供電。所有模塊形成一個完整的速度閉環控制，能對直流電機穩定快速準確的調速。機器人的轉向在舵機與連桿連接兩個前輪的方式下，通過STM32 輸出PWM 波控制舵機的轉動從而帶動兩個前輪同步轉動以達到轉向的目的。前輪的束腳[5] 為0° ，從而在不影響轉向效果的前提下能夠穩定地直線行駛。

2.3 機械臂設計

本文所建立的機械臂是由6 個舵機及機械零件組成，其中5 個舵機組成5 個旋轉關節，另外1 個舵機構成末端執行器機械爪，整體為5 自由度，其D-H 參數表如表1 所示。

2.4 機器人視覺設計

本文選用OpenMV 模塊承擔機器人的視覺功能。OpenMV 是集攝像頭、處理器和調試器于一體的智能攝像頭，內含豐富的圖像處理實用庫，在低算力要求下表現良好。將OpenMV 鑲嵌到機械臂上即可通過機械臂控制攝像頭的視野以完成機器人的巡線移動或機械臂的抓取定位等功能。

2.5 電源及驅動設計

機器人電源結構如圖5所示。

本文所設計的機器人在滿足安全穩定的指標下，采用1 個總電源通過多級降壓為機器人的各個模塊進行供電。總電源為12 V、6 000 mAH 鋰電池， 電源通過TB6612 穩壓后給兩路直流電機供電，同時通過LM2596S-ADJ 芯片[6] 將12 V 降為7.5 V 給轉向舵機與機械臂的六個舵機供電。再將7.5 V 輸入至AMS1117-5.0 芯片輸出5 V 至OPENMV H7 模塊。最后將5 V 輸入至AMS1117-3.3 輸出3.3 V 給STM32F103C8T6 主控芯片供電。

3 機器人軟件設計

3.1 系統總體框架

系統運行流程如圖6 所示。機器人開機之后等待帶有二維碼的物品出現，檢測到物品后，掃描物品上的二維碼獲取目標貨柜ID 信息，通過AprilTag 視覺定位獲取物品位置信息，以此控制機械臂進行準確的定位與跟蹤抓取并放入機器人的臨時貨柜中。之后機器人切換為移動狀態，攝像頭識別地面黑線，通過PID 控制機器人轉向與移動直到到達目標位置后將臨時貨柜中的物品放入目標貨柜中。程序采用前后臺控制：基本功能和循環等待作為前臺程序，中斷函數為后臺程序。主程序中完成芯片外設的初始化、功能處理；中斷函數實現機械臂控制、電機控制、舵機控制、串口數據處理、狀態更新。

3.2 視覺跟蹤

攝像頭有兩種工作模式：抓取和巡線模式。抓取模式提供物品的位置信息，巡線模式提供黑線位置信息，主控芯片管理攝像頭的模式。程序流程圖如圖7 所示。

首先初始化攝像頭，將模式設為抓取模式，循環執行獲得圖像、檢測AprilTag、如果存在AprilTag 則解析AprilTag 位置信息、串口發送數據等操作，直到接收到模式切換命令。當收到巡線命令時，循環執行獲得圖像、二值化、解析黑線位置和偏角信息、串口發送數據等操作。

3.3 移動與轉向控制

機器人的移動主要通過直流電機的轉動，為了能準確控制電機轉動速度，本文采用了增量式PID 算法來控制電機，其控制流程如圖8 所示。

首先初始化外設，設置PWM 頻率為10 kHz，PID采樣頻率為50 Hz，即每20 ms 進行一次定時器中斷，在定時器中斷函數中對讀取到的光電編碼器計數值進行處理，STM32F103C8T6 的編碼器接口計數方向與編碼器信號的關系如表2 所示。

當其中一路接口信號跳變時，判斷另一路的相對電平確定計數方向，在設置為4 倍頻模式下實際運行計數效果如圖9 所示。

結合本文所用電機的物理參數，減速比i ，光電編碼器線數P ，車輪半徑r ，采樣頻率f 可得計數值Count與實際速度V 的計算公式為：

將由公式6得到的實際速度V 帶入增量式PID 算法公式2 中即可得到應輸出的PWM 量以修正實際測得的誤差。機器人的轉向主要通過舵機通過連桿帶動前輪實現轉向，在直線行駛的情況下，黑線在攝像頭中應處于圖像中間且豎直，由此確定轉向的反饋數據為黑線在圖中的偏距ρ 和偏角θ如圖10所示。

ρ為黑線頂部中點與圖像寬度一半的差， θ 為黑線與圖像底部的夾角。將偏距ρ 和偏角θ 分別帶入位置式PID 算法公式，計算出的數值相加即可得到應輸出PWM 波以控制轉向舵機修正攝像頭觀測到的誤差。

3.4 機械臂控制

機械臂控制流程如圖11所示。

在接收到目標物品的相對坐標x、y、z后，先通過正運動學由當前關節角度計算出當前末端執行器的坐標，然后再加上物品的相對坐標，就可得到目標物品的絕對坐標X、Y、Z。通過逆運動學將目標絕對坐標位置轉換為目標關節角度，利用增量式PID 完成對機械臂的控制輸出。

4 實驗結果及分析

整合各模塊后，現對各模塊進行性能測試，包括電機調速性能、AprilTag 姿態獲取范圍、機械臂跟蹤能力。

4.1 機器人運動控制性能

控制系統性能的評價可以采用動態性能指標分析，即在典型輸入信號作用下，計算系統響應的上升時間t_r，峰值時間t_p，調節時間t_s，超調量σ%。以上指標可以滿足系統動態過程的特征體現。

在零初始條件下，對系統給定不同的階躍輸入，分別為20、40、60、80 cm/s，測試得到系統的響應如圖12所示。

通過圖12 可知，系統輸出最后趨近于給定值，能夠滿足控制系統最基本的穩定性要求，然后分析其動態指標如表3。

分析表3 動態指標可知，隨著目標速度的增大上升時間，峰值時間都略微增大，調節時間受誤差影響較大，平均調節時間為560 ms，從總體上看，系統響應快速，在700 ms 內穩定至目標速度，雖然略微有部分超調，但仍在穩定可靠范圍內，不影響整個系統的安全性。

4.2 AprilTag檢測范圍

實際中二維碼可能會從各個角度各種距離面向攝像頭，為保證識別準確性需要測量出二維碼的有效范圍。經過測試后有效范圍如表4所示。

其中坐標軸以機械臂末端執行器關節為原點，關節軸指向為Z 軸，X 軸與Y 軸分別為初始狀態下的水平與豎直方向。由表4 分析可知，AprilTag 能在攝像頭15.8 cm 的距離內準確識別，左右范圍19.6 cm，上下范圍12.4 cm 內能檢測到AprilTag。同時ArilTag 與攝像頭在70.8° 的夾角內能夠被識別到。總體來講已經能夠滿足機器人的視覺功能要求。

4.3機械臂控制性能

機械臂為機器人物流功能的主要執行器，由于其控制模型為多輸入多輸出，涉及坐標變換較為復雜，系統響應曲線不能直觀的表達系統的動態指標，所以本文采用末端執行器距目標位置的距離隨時間的變化來反應機械臂的控制性能。如圖13 所示。

在距離末端執行器5 cm 時機械臂到達目標位置并開始抓取，抓取后距離不再變化。機械臂從目標物品出現到完成抓取用時2.5 s，一開始快速接近目標物品，靠近目標后不斷修正位置直到能抓取目標物品。靠近目標后沒有過多的振蕩滿足了快速性的同時也能保證系統的安全。相較于傳統開環控制，減少了振蕩，提高了系統的穩定性與抗干擾能力，大幅提高了抓取的成功率。

5結束語

本文設計了基于STM32 的智能物流機器人，采用了PID算法控制機器人移動與機械臂抓取，優化了物流機器人的控制性能，提高了系統的穩定性，實現了精準的識別與抓取物品。但本文設計的機器人未優化網絡應用層協議，沒有采用數據庫存儲數據，希望在下一次研究中優化機器人的網絡通訊，以滿足互聯的要求。