基于STM32的智能物流機(jī)器人

搬運(yùn)機(jī)器人可代替人工在高溫、高壓、多粉塵等危險(xiǎn)環(huán)境中作業(yè)，節(jié)省大量人力資源，大幅提高工作效率。在未來(lái)智慧工廠建設(shè)中，智能物流搬運(yùn)機(jī)器人技術(shù)起著舉足輕重的作用^[1]。

鑒于智能物流搬運(yùn)機(jī)器人將來(lái)在制造業(yè)發(fā)展中勢(shì)必占有較重要的地位，本文設(shè)計(jì)實(shí)現(xiàn)了一種基于STM32的智能物流機(jī)器人系統(tǒng)，通過(guò)智能車(chē)為平臺(tái)搭載機(jī)械臂和攝像頭，運(yùn)用AprilTag 標(biāo)定跟蹤與PID 控制^[2] 實(shí)現(xiàn)機(jī)器人的移動(dòng)與跟蹤抓取。最終實(shí)現(xiàn)自動(dòng)導(dǎo)航以及智能識(shí)別并搬運(yùn)物品的功能。

1 機(jī)器人工作原理

1.1 PID控制算法

PID 控制是指比例、積分、微分控制。將系統(tǒng)的測(cè)量反饋經(jīng)過(guò)比例、積分、微分3 種運(yùn)算后，疊加至輸入中，達(dá)到控制的目的。其原理簡(jiǎn)單、易于實(shí)現(xiàn)，被應(yīng)用于多種領(lǐng)域。常用的PID 算法有兩種，一種是位置式PID，另一種是增量式PID，兩者應(yīng)用場(chǎng)合不同，各有優(yōu)點(diǎn)。位置式PID 算法的表達(dá)式為：

其中，K_p為比例放大系數(shù)，T_i為積分時(shí)間常數(shù)，T_d為微分時(shí)間常數(shù)。

位置式PID 算法是根據(jù)當(dāng)前實(shí)際測(cè)得的輸出與要達(dá)到的預(yù)期輸出的偏差e(k)，進(jìn)行 PID 運(yùn)算。由于積分項(xiàng)會(huì)對(duì)每一次的誤差進(jìn)行累加，使其運(yùn)算量較大，且每次輸出均與過(guò)去的狀態(tài)相關(guān)，可能會(huì)引起大幅度超調(diào)。適用于執(zhí)行機(jī)構(gòu)不帶積分部件的對(duì)象，如電液伺服閥。

增量式PID 算法的表達(dá)式為：

增量式 PID 公式是由將位置式 PID 的輸出 u(k) ?u(k?1) = Δu(k)得到，而不是對(duì)應(yīng)與實(shí)際位置的偏差，沒(méi)有誤差累積。也就是說(shuō)增量式PID 中不需要累加?？刂圃隽喀?em>u(k)的確定僅與最近 3 次的采樣值有關(guān)。所以增量式PID 容易通過(guò)加權(quán)處理獲得比較好的控制效果，相較于位置式PID 算法，在系統(tǒng)發(fā)生問(wèn)題時(shí)，增量式PID 不會(huì)嚴(yán)重影響系統(tǒng)的工作。適用于執(zhí)行機(jī)構(gòu)帶積分部件的對(duì)象，如步進(jìn)電機(jī)等。

圖1 PID控制系統(tǒng)圖

1.2 視覺(jué)定位算法

AprilTag 是一種視覺(jué)基準(zhǔn)系統(tǒng)，通過(guò)特定的標(biāo)志快速檢測(cè)出相對(duì)位置，AprilTag 庫(kù)不需要依賴(lài)，可以輕松移植到嵌入式裝備中。AprilTag 視覺(jué)定位^[3] 實(shí)現(xiàn)主要分為三個(gè)部分：邊緣檢測(cè)，四邊形檢測(cè)，匹配與解析，其流程如圖2所示。

圖2 視覺(jué)定位算法流程圖

首先對(duì)圖像提取邊緣，查找邊緣中邊數(shù)大于四的凸 多邊形，在查找到的多邊形中篩選出四邊形。對(duì)四邊形中的圖像進(jìn)行編碼，白色為0，黑色為1，1 個(gè)四邊形有4 個(gè)編碼，分辨是當(dāng)前圖像的編碼和進(jìn)行3 次90°旋轉(zhuǎn)后的編碼，計(jì)算與庫(kù)中編碼的最小海明距離。如果海明距離小于閾值，則認(rèn)為其為AprilTag，可確定其ID，并通過(guò)矩陣變換得到位姿信息。

1.3 機(jī)械臂運(yùn)動(dòng)學(xué)

機(jī)械臂運(yùn)動(dòng)學(xué)^[4] 研究的是通過(guò)建立機(jī)械臂運(yùn)動(dòng)學(xué)模型、分析機(jī)械臂各關(guān)節(jié)的位姿、速度、加速度與機(jī)械臂末端在空間中的變換規(guī)律。正運(yùn)動(dòng)學(xué)通過(guò)各個(gè)關(guān)節(jié)之間的關(guān)系，求出機(jī)器人末端執(zhí)行器的位姿，以關(guān)節(jié)角度作自變量，建立機(jī)械臂的正運(yùn)動(dòng)模型。

為方便的表達(dá)兩個(gè)相鄰關(guān)節(jié)連桿的參數(shù)關(guān)系，需要對(duì)所有關(guān)節(jié)指定一個(gè)參考系，D-H 參數(shù)法是一種常用的對(duì)機(jī)器人相鄰兩個(gè)連桿之間參數(shù)關(guān)系建模的數(shù)學(xué)方法，其中的參數(shù)有5 個(gè)：

各個(gè)關(guān)節(jié)坐標(biāo)系之間的變換矩陣依次相乘，求出從基坐標(biāo)系到機(jī)械臂末端執(zhí)行器坐標(biāo)系之間的坐標(biāo)變換矩陣。

圖3 機(jī)械臂簡(jiǎn)化幾何模型

2 機(jī)器人硬件設(shè)計(jì)

2.1 硬件總體設(shè)計(jì)

本文基于STM32 設(shè)計(jì)的智能物流機(jī)器人系統(tǒng)，選取性能較好的硬件配置作為系統(tǒng)運(yùn)行的外部環(huán)境支撐，根據(jù)搬運(yùn)機(jī)器人機(jī)械系統(tǒng)功能和技術(shù)指標(biāo)，設(shè)置硬件總體結(jié)構(gòu)。系統(tǒng)硬件設(shè)備包括攝像頭模塊、OLED 顯示器、轉(zhuǎn)向舵機(jī)、直流電機(jī)，系統(tǒng)硬件結(jié)構(gòu)圖，如圖4 所示。

圖4 系統(tǒng)硬件結(jié)構(gòu)圖

智能物流機(jī)器人系外部設(shè)備較多，各設(shè)備之間工作電壓不一致。本文采用了LM2596 和AMS1117 兩顆降壓芯片，調(diào)整系統(tǒng)運(yùn)行時(shí)的電壓、攝像頭模塊和機(jī)械臂是機(jī)器人抓取目標(biāo)的基礎(chǔ)硬件設(shè)備、直流電機(jī)和轉(zhuǎn)向舵機(jī)能控制機(jī)器人的移動(dòng)和方向，OLED 顯示系統(tǒng)運(yùn)行時(shí)的各種參數(shù)，是調(diào)試時(shí)的重要依據(jù)。

2.2 移動(dòng)與轉(zhuǎn)向設(shè)計(jì)

本文所設(shè)計(jì)的機(jī)器人移動(dòng)采用兩路帶光電編碼器的直流電機(jī)，通過(guò)STM32 輸出PWM 波，同時(shí)接收編碼器計(jì)數(shù)，經(jīng)處理后達(dá)到控制調(diào)速目的。OLED 模塊顯示設(shè)定速讀和實(shí)時(shí)速度，選擇TB6612 模塊作為直流電機(jī)驅(qū)動(dòng)給電機(jī)供電。所有模塊形成一個(gè)完整的速度閉環(huán)控制，能對(duì)直流電機(jī)穩(wěn)定快速準(zhǔn)確的調(diào)速。機(jī)器人的轉(zhuǎn)向在舵機(jī)與連桿連接兩個(gè)前輪的方式下，通過(guò)STM32 輸出PWM 波控制舵機(jī)的轉(zhuǎn)動(dòng)從而帶動(dòng)兩個(gè)前輪同步轉(zhuǎn)動(dòng)以達(dá)到轉(zhuǎn)向的目的。前輪的束腳^[5] 為0° ，從而在不影響轉(zhuǎn)向效果的前提下能夠穩(wěn)定地直線(xiàn)行駛。

2.3 機(jī)械臂設(shè)計(jì)

本文所建立的機(jī)械臂是由6 個(gè)舵機(jī)及機(jī)械零件組成，其中5個(gè)舵機(jī)組成5個(gè)旋轉(zhuǎn)關(guān)節(jié)，另外1個(gè)舵機(jī)構(gòu)成末端執(zhí)行器機(jī)械爪，整體為5自由度，其D-H參數(shù)表如表1 所示。

2.4 機(jī)器人視覺(jué)設(shè)計(jì)

本文選用OpenMV 模塊承擔(dān)機(jī)器人的視覺(jué)功能。OpenMV是集攝像頭、處理器和調(diào)試器于一體的智能攝像頭，內(nèi)含豐富的圖像處理實(shí)用庫(kù)，在低算力要求下表現(xiàn)良好。將OpenMV 鑲嵌到機(jī)械臂上即可通過(guò)機(jī)械臂控制攝像頭的視野以完成機(jī)器人的巡線(xiàn)移動(dòng)或機(jī)械臂的抓取定位等功能。

2.5 電源及驅(qū)動(dòng)設(shè)計(jì)

機(jī)器人電源結(jié)構(gòu)如圖5 所示。

圖5 電源結(jié)構(gòu)圖

本文所設(shè)計(jì)的機(jī)器人在滿(mǎn)足安全穩(wěn)定的指標(biāo)下，采用1 個(gè)總電源通過(guò)多級(jí)降壓為機(jī)器人的各個(gè)模塊進(jìn)行供電?？傠娫礊?2 V、6 000 mAH 鋰電池， 電源通過(guò)TB6612 穩(wěn)壓后給兩路直流電機(jī)供電，同時(shí)通過(guò)LM2596S-ADJ 芯片^[6] 將12 V 降為7.5 V 給轉(zhuǎn)向舵機(jī)與機(jī)械臂的六個(gè)舵機(jī)供電。再將7.5 V 輸入至AMS1117-5.0 芯片輸出5 V 至OPENMV H7 模塊。最后將5 V 輸入至AMS1117-3.3 輸出3.3 V 給STM32F103C8T6 主控芯片供電。

3 機(jī)器人軟件設(shè)計(jì)

3.1 系統(tǒng)總體框架

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

圖6 軟件設(shè)計(jì)流程

3.2 視覺(jué)跟蹤

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

圖7 OpenMV程序流程圖

首先初始化攝像頭，將模式設(shè)為抓取模式，循環(huán)執(zhí)行獲得圖像、檢測(cè)AprilTag、如果存在AprilTag 則解析AprilTag位置信息、串口發(fā)送數(shù)據(jù)等操作，直到接收到模式切換命令。當(dāng)收到巡線(xiàn)命令時(shí)，循環(huán)執(zhí)行獲得圖像、二值化、解析黑線(xiàn)位置和偏角信息、串口發(fā)送數(shù)據(jù)等操作。

3.3 移動(dòng)與轉(zhuǎn)向控制

機(jī)器人的移動(dòng)主要通過(guò)直流電機(jī)的轉(zhuǎn)動(dòng)，為了能準(zhǔn)確控制電機(jī)轉(zhuǎn)動(dòng)速度，本文采用了增量式PID 算法來(lái)控制電機(jī)，其控制流程如圖8所示。

圖8 調(diào)速流程圖

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

當(dāng)其中一路接口信號(hào)跳變時(shí)，判斷另一路的相對(duì)電平確定計(jì)數(shù)方向，在設(shè)置為4 倍頻模式下實(shí)際運(yùn)行計(jì)數(shù)效果如圖9所示。

圖9 4倍頻編碼器模式下的計(jì)數(shù)器操作實(shí)例

結(jié)合本文所用電機(jī)的物理參數(shù)，減速比i ，光電編碼器線(xiàn)數(shù)P ，車(chē)輪半徑r ，采樣頻率f ，可得計(jì)數(shù)值Count與實(shí)際速度V 的計(jì)算公式為：

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

圖10 攝像頭解析圖像

ρ 為黑線(xiàn)頂部中點(diǎn)與圖像寬度一半的差，θ 為黑線(xiàn)與圖像底部的夾角。將偏距ρ 和偏角θ 分別帶入位置式PID 算法公式，計(jì)算出的數(shù)值相加即可得到應(yīng)輸出PWM 波以控制轉(zhuǎn)向舵機(jī)修正攝像頭觀測(cè)到的誤差。

3.4 機(jī)械臂控制

機(jī)械臂控制流程如圖11 所示。

圖11 機(jī)械臂控制流程圖

在接收到目標(biāo)物品的相對(duì)坐標(biāo)x、y、z后，先通過(guò)正運(yùn)動(dòng)學(xué)由當(dāng)前關(guān)節(jié)角度計(jì)算出當(dāng)前末端執(zhí)行器的坐標(biāo)，然后再加上物品的相對(duì)坐標(biāo)，就可得到目標(biāo)物品的絕對(duì)坐標(biāo)X、Y、Z。通過(guò)逆運(yùn)動(dòng)學(xué)將目標(biāo)絕對(duì)坐標(biāo)位置轉(zhuǎn)換為目標(biāo)關(guān)節(jié)角度，利用增量式PID 完成對(duì)機(jī)械臂的控制輸出。

4 實(shí)驗(yàn)結(jié)果及分析

整合各模塊后，現(xiàn)對(duì)各模塊進(jìn)行性能測(cè)試，包括電機(jī)調(diào)速性能、AprilTag 姿態(tài)獲取范圍、機(jī)械臂跟蹤能力。

4.1 機(jī)器人運(yùn)動(dòng)控制性能

控制系統(tǒng)性能的評(píng)價(jià)可以采用動(dòng)態(tài)性能指標(biāo)分析，即在典型輸入信號(hào)作用下，計(jì)算系統(tǒng)響應(yīng)的上升時(shí)間t_r ，峰值時(shí)間t_p ，調(diào)節(jié)時(shí)間t_s ，超調(diào)量σ %。以上指標(biāo)可以滿(mǎn)足系統(tǒng)動(dòng)態(tài)過(guò)程的特征體現(xiàn)。

在零初始條件下，對(duì)系統(tǒng)給定不同的階躍輸入，分別為20、40、60、80 cm/s，測(cè)試得到系統(tǒng)的響應(yīng)如圖12 所示。

通過(guò)圖 12 可知，系統(tǒng)輸出最后趨近于給定值，能夠滿(mǎn)足控制系統(tǒng)最基本的穩(wěn)定性要求，然后分析其動(dòng)態(tài)指標(biāo)如表 3。

分析表3 動(dòng)態(tài)指標(biāo)可知，隨著目標(biāo)速度的增大上升時(shí)間，峰值時(shí)間都略微增大，調(diào)節(jié)時(shí)間受誤差影響較大，平均調(diào)節(jié)時(shí)間為560 ms，從總體上看，系統(tǒng)響應(yīng)快速，在700 ms 內(nèi)穩(wěn)定至目標(biāo)速度，雖然略微有部分超調(diào)，但仍在穩(wěn)定可靠范圍內(nèi)，不影響整個(gè)系統(tǒng)的安全性。

4.2 AprilTag檢測(cè)范圍

實(shí)際中二維碼可能會(huì)從各個(gè)角度各種距離面向攝像頭，為保證識(shí)別準(zhǔn)確性需要測(cè)量出二維碼的有效范圍。經(jīng)過(guò)測(cè)試后有效范圍如表4 所示。

其中坐標(biāo)軸以機(jī)械臂末端執(zhí)行器關(guān)節(jié)為原點(diǎn)，關(guān)節(jié)軸指向?yàn)?em>Z 軸，X 軸與Y 軸分別為初始狀態(tài)下的水平與豎直方向。由表4 分析可知，AprilTag 能在攝像頭15.8 cm 的距離內(nèi)準(zhǔn)確識(shí)別，左右范圍19.6 cm，上下范圍12.4 cm 內(nèi)能檢測(cè)到AprilTag。同時(shí)ArilTag 與攝像頭在70.8° 的夾角內(nèi)能夠被識(shí)別到。總體來(lái)講已經(jīng)能夠滿(mǎn)足機(jī)器人的視覺(jué)功能要求。

4.3 機(jī)械臂控制性能

機(jī)械臂為機(jī)器人物流功能的主要執(zhí)行器，由于其控制模型為多輸入多輸出，涉及坐標(biāo)變換較為復(fù)雜，系統(tǒng)響應(yīng)曲線(xiàn)不能直觀的表達(dá)系統(tǒng)的動(dòng)態(tài)指標(biāo)，所以本文采用末端執(zhí)行器距目標(biāo)位置的距離隨時(shí)間的變化來(lái)反應(yīng)機(jī)械臂的控制性能。如圖13 所示。

圖13 末端執(zhí)行器與目標(biāo)距離的變化

在距離末端執(zhí)行器5 cm 時(shí)機(jī)械臂到達(dá)目標(biāo)位置并開(kāi)始抓取，抓取后距離不再變化。機(jī)械臂從目標(biāo)物品出現(xiàn)到完成抓取用時(shí)2.5 s，一開(kāi)始快速接近目標(biāo)物品，靠近目標(biāo)后不斷修正位置直到能抓取目標(biāo)物品。靠近目標(biāo)后沒(méi)有過(guò)多的振蕩滿(mǎn)足了快速性的同時(shí)也能保證系統(tǒng)的安全。相較于傳統(tǒng)開(kāi)環(huán)控制，減少了振蕩，提高了系統(tǒng)的穩(wěn)定性與抗干擾能力，大幅提高了抓取的成功率。

5 結(jié)束語(yǔ)

本文設(shè)計(jì)了基于STM32 的智能物流機(jī)器人，采用了PID 算法控制機(jī)器人移動(dòng)與機(jī)械臂抓取，優(yōu)化了物流機(jī)器人的控制性能，提高了系統(tǒng)的穩(wěn)定性，實(shí)現(xiàn)了精準(zhǔn)的識(shí)別與抓取物品。但本文設(shè)計(jì)的機(jī)器人未優(yōu)化網(wǎng)絡(luò)應(yīng)用層協(xié)議，沒(méi)有采用數(shù)據(jù)庫(kù)存儲(chǔ)數(shù)據(jù)，希望在下一次研究中優(yōu)化機(jī)器人的網(wǎng)絡(luò)通訊，以滿(mǎn)足互聯(lián)的要求。

參考文獻(xiàn)：

[1] 董智.5G商業(yè)化應(yīng)用背景下“互聯(lián)網(wǎng)+物流”的發(fā)展趨勢(shì)研究[J].中國(guó)商論,2019(22):12-13.

[2] 雷慧杰.基于STM32的直流電機(jī)PID調(diào)速系統(tǒng)設(shè)計(jì)[J].現(xiàn)代電子技術(shù),2016,39(8):165-170.

[3] 徐慶坤,竇紅乾,杜志強(qiáng),李凱,譚威.基于AprilTag視覺(jué)定位的移動(dòng)機(jī)器人設(shè)計(jì)[J].工業(yè)控制計(jì)算機(jī),2022,35(4):42-43.

[4] 王裕民,秦飛舟.五自由度機(jī)械臂的運(yùn)動(dòng)學(xué)建模和軌跡規(guī)劃研究[J].電工技術(shù),2022(8):62-69.

[5] 彭鋒,丁良奇,劉尚鴻,潘筱.車(chē)輪外傾角和前束角匹配研究[J].北京汽車(chē),2021(3):21-23,31.

[6] 鄭琪,劉琳,尚冬梅,CHEN M.基于LM2596的輸出無(wú)級(jí)可調(diào)直流穩(wěn)壓電源的設(shè)計(jì)與實(shí)現(xiàn)[J].電腦知識(shí)與技術(shù),2018,14(27):258-260.

[7] 單芝慶.基于STM32的搬運(yùn)機(jī)器人機(jī)械系統(tǒng)設(shè)計(jì)[J].自動(dòng)化應(yīng)用,2021(6):1-6.

[8] 鄔合林.PID在機(jī)械臂位置控制中的應(yīng)用[J].冶金管理,2020(19):43-44.

[9] 楊曉峰,柳笑辰,王佳奇.基于OPENMV的智能送藥小車(chē)設(shè)計(jì)[J].電大理工,2022(1):1-7.

（本文來(lái)源于《電子產(chǎn)品世界》雜志2023年4月期）