基于FreeRTOS的桶裝水搬運機器人控制系統(tǒng)研究

何高清，賈辰昊，彭良霖

（合肥工業(yè)大學(xué) 機械工程學(xué)院，合肥 230009）

0 引言

搬運機器人是可以替代工人進(jìn)行自動化搬運的工業(yè)機器人，服務(wù)于各個行業(yè)的自動化生產(chǎn)線。隨著各個行業(yè)的發(fā)展，搬運機器人的需求日益增多，對其性能的靈活性和穩(wěn)定性也提出了越來越多的要求。所以設(shè)計一款兼?zhèn)溲b載量和運載穩(wěn)定性的搬運機器人，使搬運機器人在裝載大容量貨物時有著優(yōu)良的穩(wěn)定性，對于提高搬運效率、減輕工人勞作強度、減少生產(chǎn)成本有著重要的意義[1]。本文設(shè)計一個以STM32系列單片機作為主控芯片的搬運機器人，遵循FreeRTOS 操作系統(tǒng)優(yōu)化任務(wù)調(diào)度算法，以CANopen協(xié)議基礎(chǔ)實現(xiàn)伺服通信，完善整個桶裝水系列生產(chǎn)線自動化水平。

1 系統(tǒng)總體硬件設(shè)計

桶裝水搬運機器人控制系統(tǒng)由多個硬件模塊組成，包括主控芯片模塊、電源模塊、存儲模塊、CAN通信模塊、RS485通信模塊、輸入和輸出模塊[2]。

主控芯片采用STM32F405RGT6，主要負(fù)責(zé)處理數(shù)據(jù)，執(zhí)行控制程序；電源模塊外接24 V直流電源，根據(jù)模塊不同需求，對各模塊進(jìn)行供電；存儲模塊通過外部Flash進(jìn)行系統(tǒng)參數(shù)的讀取和保存；CAN通信模塊實現(xiàn)控制器與伺服驅(qū)動器的通信，通過報文的傳輸進(jìn)而控制伺服電動機的運轉(zhuǎn)；RS485通信模塊實現(xiàn)控制器與觸摸屏的通信，實現(xiàn)人機交互；輸入模塊負(fù)責(zé)接收開關(guān)、行程按鈕、模式按鈕信號，傳輸給主控芯片，實現(xiàn)功能的調(diào)度；輸出模塊通過控制繼電器的關(guān)斷實現(xiàn)氣缸的伸縮，系統(tǒng)正常運行時，控制運行燈閃爍，產(chǎn)生報警故障時，閃爍故障燈來提醒操作人員。系統(tǒng)總體硬件設(shè)計如圖1所示。

圖1 總硬件設(shè)計結(jié)構(gòu)圖

2 控制系統(tǒng)軟件設(shè)計

桶裝水搬運機器人控制系統(tǒng)軟件部分主要由FreeRTOS操作系統(tǒng)、驅(qū)動器驅(qū)動程序及系統(tǒng)應(yīng)用程序組成。系統(tǒng)開始運行，首先進(jìn)行系統(tǒng)初始化，從外部存儲器讀取保存的參數(shù)，并檢查系統(tǒng)是否出現(xiàn)故障；當(dāng)選擇自動模式時，機構(gòu)按照預(yù)設(shè)程序進(jìn)行搬運工作；選擇手動模式時，可自行控制機構(gòu)運行；對于重新輸入的系統(tǒng)參數(shù)，系統(tǒng)也會自動保存[3]。相應(yīng)程序流程如圖2所示。

圖2 系統(tǒng)總體軟件設(shè)計

2.1 嵌入式操作系統(tǒng)FreeRTOS

FreeRTOS是實時型操作系統(tǒng)，相較于傳統(tǒng)裸機控制，它把控制系統(tǒng)的各種功能轉(zhuǎn)化為任務(wù)的形式，當(dāng)系統(tǒng)需要執(zhí)行某種功能時，則進(jìn)行任務(wù)調(diào)度，合理的任務(wù)調(diào)度規(guī)劃可使控制系統(tǒng)的執(zhí)行效率更高、安全性更好[4]。

2.2 FreeRTOS任務(wù)調(diào)度機制優(yōu)化

FreeRTOS對于同一優(yōu)先級任務(wù)的調(diào)用會提供一個固定的時間片，為了保證系統(tǒng)的實時性，所設(shè)時間片通常都小于1 ms，通常無法滿足任務(wù)處理所需的時間，系統(tǒng)對任務(wù)進(jìn)行頻繁切換。當(dāng)同優(yōu)先級的任務(wù)數(shù)量較多時，會造成系統(tǒng)的處理效率降低[5]。

為了彌補同優(yōu)先級任務(wù)調(diào)度的不足，引進(jìn)了最短時間任務(wù)調(diào)度算法，其核心算法思想是對于同優(yōu)先級任務(wù)，任務(wù)執(zhí)行時間長短決定任務(wù)的調(diào)用順序，時間越短的任務(wù)優(yōu)先調(diào)用。短任務(wù)優(yōu)先調(diào)用減少了系統(tǒng)任務(wù)的平均周轉(zhuǎn)時間，在相同的時間內(nèi)系統(tǒng)可以處理更多的任務(wù)，提高執(zhí)行效率。同時，最短時間任務(wù)調(diào)度算法也有相應(yīng)缺點，同優(yōu)先級的長任務(wù)會一直得不到CPU資源，被“餓死”[6]。

為了彌補不足，對調(diào)度算法做了改進(jìn)，不同優(yōu)先級仍采用優(yōu)先級搶占調(diào)度算法，對于相同優(yōu)先級任務(wù)，在原有基礎(chǔ)上，引入“最短時間閾值”概念，該變量為同優(yōu)先級短時間任務(wù)獨有，每當(dāng)短時間任務(wù)調(diào)度完成，會對該值加1，每當(dāng)長時間任務(wù)調(diào)度完成，會對短時間任務(wù)閾值減2，當(dāng)短時間任務(wù)閾值到達(dá)一定值，會提醒CPU優(yōu)先執(zhí)行長時間任務(wù)，這樣長時間任務(wù)也會得到合理的CPU資源。把改進(jìn)后的算法稱為最短時間閾值任務(wù)調(diào)度算法。最短時間閾值任務(wù)調(diào)度算法流程如圖3所示。

圖3 最短時間閾值任務(wù)調(diào)度算法時序圖

2.3 系統(tǒng)應(yīng)用程序

系統(tǒng)應(yīng)用程序由多任務(wù)組成，包括LED工作燈任務(wù)、系統(tǒng)參數(shù)的讀取和輸入任務(wù)、機構(gòu)控制任務(wù)、人機交互界面通信任務(wù)、伺服驅(qū)動器CAN通信、I/O輸入檢測、軌跡規(guī)劃任務(wù)及傳感器檢測任務(wù)等，根據(jù)任務(wù)調(diào)用頻率和重要性，劃分不同優(yōu)先級[7]。具體任務(wù)優(yōu)先級和功能介紹如表1所示。

表1 搬運機器人程序任務(wù)分配

3 路徑規(guī)劃設(shè)計

3.1 搬運機器人結(jié)構(gòu)分析

搬運機器人是一個直角坐標(biāo)機器人，具體結(jié)構(gòu)如圖4所示。以搬運小車移動方向為X軸，以縱梁沿移動小車上下移動方向為Y軸，以夾爪沿縱梁移動方向為Z軸。

圖4 搬運機器人結(jié)構(gòu)圖

3.2 碼垛方式

在搬運碼垛過程中，工人會按照一定方式進(jìn)行桶裝水碼垛，保證堆放的桶裝水不會傾斜掉落，本文采取桶裝水碼垛方式如圖5所示。從圖5可以看出，第1層桶裝水列與列之間交錯堆放，且桶和桶之間緊密相連；通過第1層桶裝水的交錯排列，桶口之間形成了一個三角空間，第2層的桶裝水則依次放置其中。這樣從里到外、從上到下地裝載，既保證了堆放結(jié)構(gòu)的穩(wěn)定性，也大大節(jié)省空間，盡可能多放桶裝水[8]。

圖5 桶裝水碼垛

3.3 路徑規(guī)劃

圖6為桶裝水搬運路徑規(guī)劃，圖6（a）中搬運機器人的位置為起始位置，起始點的X軸位置與從輸送線取桶裝水的位置相同，并且夾具在Y軸方向上的最低面要高于桶裝水的最高面。

圖6 搬運路徑規(guī)劃

從初始位置沿著Y軸方向向下移動，使得夾爪到達(dá)可夾取位置，通過氣缸夾緊桶裝水，如圖6（b）所示。從輸送線上夾取到桶裝水后，沿著Y軸方向向上移動，使得桶裝水桶底的底面高于輸送線，如圖6（c）所示。再根據(jù)所需碼垛的層數(shù)調(diào)整Y軸方向的高度，沿橫梁X軸運送到指定位置，如圖6（d）所示。

3.4 路徑規(guī)劃算法

常見軌跡規(guī)劃算法包括梯形速度曲線、三次多項式曲線和五次多項式曲線等，通過比較速度和加速度曲線的平滑度、對末端執(zhí)行機構(gòu)的沖擊等，本文選用五次多項式曲線[9]。

位移計算公式為

對位移公式進(jìn)行求導(dǎo)可以得到速度公式為

對速度公式進(jìn)行求導(dǎo)可以得到加速度公式為

已知初始位置和速度和加速度、目標(biāo)位置和速度和加速度，有S（t0）=S0，S（tn）=Sn，V（t0）=V0，V（tn）=Vn，a（t0）=a0，a（tn）=an六個條件。取t0=0，tn=3，S0=0，Sn=100，V0=Vn=0，a0=an=0，分別計算出三次多項式的位移、速度、加速度方程：

通過MATLAB進(jìn)行曲線仿真繪制，得到的曲線如圖7所示，圖7（b）中當(dāng)速度達(dá)到峰值時，圖7（c）加速度等于0，并在之后開始減速，直至終點速度將為0，且圖7（c）中加速度曲線為連續(xù)曲線，沒有突變問題，減少了搬運機器人運行時的沖擊振動，改善了其動力學(xué)性能，符合其軌跡要求。

圖7 五次多項式曲線

4 測試分析

設(shè)計搭建好控制系統(tǒng)，調(diào)試運動控制器，檢測機構(gòu)運行狀況及其誤差度。在已搭建好控制平臺的基礎(chǔ)上，用戶無需了解底層硬件的運行，只需要通過觸摸屏進(jìn)行參數(shù)設(shè)置和模式選擇完成相應(yīng)的工作配置[10]，即可實現(xiàn)對桶裝水搬運機器人的控制。系統(tǒng)控制界面和工業(yè)參數(shù)界面如圖8所示，搬運機器人實物如圖9所示，實驗平臺如圖10所示。

圖8 控制主頁面和系統(tǒng)參數(shù)

圖9 搬運機器人實物圖

圖10 實驗平臺

為了進(jìn)一步驗證系統(tǒng)的穩(wěn)定性和準(zhǔn)確性，在搬運機器人平臺進(jìn)行測試，設(shè)定搬運機器人起始坐標(biāo)為（0，0，0），目標(biāo)坐標(biāo)為（300，200，100），單位為cm。通過CAN分析儀截取運行報文進(jìn)行分析，如圖11所示。

階段1，系統(tǒng)運行模式默認(rèn)設(shè)為速度模式，處于使能狀態(tài)，3個電動機的當(dāng)前位置皆為零點。通過觸摸屏設(shè)置系統(tǒng)運行模式為手動模式，并設(shè)置目標(biāo)位置（300，200，100）。

階段2為插補模式運行階段，圖中ID號為0×0201、0×0202、0×0203的報文分別代表三軸的數(shù)據(jù)報文，分析可得，數(shù)據(jù)位前兩個字節(jié)為當(dāng)前電動機運行位置，其值分別為0×55F0、0×3621、0×2114。

階段3為電動機到達(dá)目標(biāo)位置階段，根據(jù)圖中ID號為0×0181、0×0182、0×0183的報文，分析可得，當(dāng)前三軸位置為0×0493E0、0×030D40、0×0186A0，十進(jìn)制轉(zhuǎn)換后為300000、200000、100000。轉(zhuǎn)換當(dāng)前位置為300、200、100 cm，由此可知電動機已到達(dá)指定位置。

5 結(jié)論

本文驗證了自主研發(fā)的桶裝水搬運機器人系統(tǒng)，硬件采用模塊化結(jié)構(gòu)，安全高效，具有較高的擴展性，其上層應(yīng)用控制程序和底層通信程序的設(shè)計結(jié)構(gòu)層次分明，通過優(yōu)化的調(diào)度算法合理高效地調(diào)度應(yīng)用任務(wù)，保證了程序的高效性和可靠性。在搬運測試中，通過系統(tǒng)控制，搬運機器人可以準(zhǔn)確到達(dá)設(shè)定的目標(biāo)位置，完成搬運任務(wù)，為工業(yè)搬運機器人系統(tǒng)開發(fā)提供了一種可靠、高效的參考依據(jù)。