全方位移動(dòng)機(jī)器人控制系統(tǒng)設(shè)計(jì)

（南京工程學(xué)院，南京 211100）

0 引言

隨著倉(cāng)儲(chǔ)物流業(yè)、生產(chǎn)制造業(yè)智能化的不斷發(fā)展，人們對(duì)運(yùn)輸機(jī)器人需求越來(lái)越大。全向移動(dòng)機(jī)器人因具備在狹小空間的作業(yè)能力，且具有靈活多樣的運(yùn)動(dòng)姿態(tài)，受到人們的廣泛關(guān)注，其中Mecanum輪機(jī)器人是研究最早、應(yīng)用最廣泛的。Mecanum輪最早由瑞士工程師Bengt Iron于1972年發(fā)明。隨后卡內(nèi)基.梅隆大學(xué)的Muir PF等基于矩陣變換法建立了運(yùn)動(dòng)學(xué)模型，成功地應(yīng)用在了車輪滑動(dòng)的檢測(cè)和航位推算上[1]。L.Gracia采用對(duì)機(jī)器人動(dòng)力學(xué)逐次近似的方法得到了考慮車輪打滑的運(yùn)動(dòng)學(xué)模型，為提高M(jìn)ecanum輪機(jī)器人控制精度、解決打滑問(wèn)題建立了理論基礎(chǔ)[2]。劉磊設(shè)計(jì)了一種全向移動(dòng)機(jī)器人控制方法，通過(guò)對(duì)機(jī)器人和電機(jī)模型的仿真解算反解出機(jī)器人的實(shí)際導(dǎo)航參量，用于機(jī)器人的導(dǎo)航位姿閉環(huán)控制[3]。

由于Mecanum輪由輥?zhàn)咏M成，靠輥?zhàn)拥哪Σ亮ο嗷プ饔卯a(chǎn)生運(yùn)動(dòng)，容易發(fā)生側(cè)滑造成運(yùn)動(dòng)精度相對(duì)較低。輥?zhàn)应两堑膶?shí)際值與理論值也存在一定的偏差，直接影響到機(jī)器人的速度及運(yùn)行軌跡[4]。針對(duì)此問(wèn)題，本文設(shè)計(jì)了基于PD的全方位移動(dòng)機(jī)器人控制系統(tǒng)，通過(guò)PD調(diào)節(jié)減少機(jī)器人的實(shí)際速度和預(yù)定速度之間的誤差，保證其運(yùn)動(dòng)精度，最后通過(guò)實(shí)驗(yàn)驗(yàn)證系統(tǒng)設(shè)計(jì)的效果和控制精度。

1 全方位移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)建模

Mecanum輪由輪轂和安裝在輪轂外緣上的一組輥?zhàn)咏M成，輥?zhàn)虞S線與輪轂軸線成一定角度，并且輥?zhàn)涌衫@自身軸線旋轉(zhuǎn)。使用Mecanum輪的機(jī)器人可以在狹小作業(yè)空間里行進(jìn)自如，不需要較大的轉(zhuǎn)彎半徑。通過(guò)車輪的組合使用和各車輪轉(zhuǎn)動(dòng)速度和方向的協(xié)調(diào)控制，可以使車體沿著平面內(nèi)的任意方向移動(dòng)和轉(zhuǎn)動(dòng)[5]，可大大提高運(yùn)輸效率。

全向移動(dòng)機(jī)器人的車輪布局如圖1所示。設(shè)機(jī)器人的廣義速度為（υx，υy，ωz）T，（ω1，ω2，ω3，ω4）T表示各輪的轉(zhuǎn)速。輥?zhàn)虞S線與輪轂軸線的夾角為α，機(jī)器人的半車長(zhǎng)和半車寬分別為a、b，車輪半徑為R。

圖1 全向移動(dòng)機(jī)器人車輪布局

根據(jù)運(yùn)動(dòng)學(xué)分析，Mecanum輪機(jī)器人的正運(yùn)動(dòng)學(xué)方程為：

其中：L=b+acotα。此次設(shè)計(jì)機(jī)器人采用的Mecanum輪α角為45°。從式(1)可得，賦予四個(gè)輪子不同的轉(zhuǎn)向和轉(zhuǎn)速就可以實(shí)現(xiàn)機(jī)器人向任意位置和方向運(yùn)動(dòng)，這是控制全向移動(dòng)機(jī)器人的理論基礎(chǔ)。

2 運(yùn)動(dòng)控制系統(tǒng)硬件設(shè)計(jì)

本設(shè)計(jì)采用以STM32F103ZET6控制芯片[6]為核心的控制器，其高級(jí)定時(shí)器和通用定時(shí)器具有正交編碼計(jì)數(shù)、產(chǎn)生四路獨(dú)立PWM波功能，具有豐富的內(nèi)、外設(shè)資源，滿足本系統(tǒng)對(duì)多路電機(jī)的控制和轉(zhuǎn)速采集要求。系統(tǒng)總體結(jié)構(gòu)如圖2所示?？刂颇K產(chǎn)生PWM波，通過(guò)驅(qū)動(dòng)器驅(qū)動(dòng)直流電機(jī)，電機(jī)經(jīng)減速器傳動(dòng)后驅(qū)動(dòng)4個(gè)Mecanum輪?；魻柧幋a器與控制器相連，用于測(cè)量電機(jī)轉(zhuǎn)速。使用紅外遙控器進(jìn)行控制指令的發(fā)送。帶有電池保護(hù)板的12V電源為電機(jī)供電，經(jīng)變壓模塊調(diào)壓后為控制模塊、霍爾編碼器等控制電路供電。

圖2 控制系統(tǒng)基本組成

3 運(yùn)動(dòng)控制系統(tǒng)軟件設(shè)計(jì)

3.1 PD控制模型

本文采用增量式PD算法，設(shè)計(jì)方案如圖3所示。PD算法完整定義為u(k)=KPΔe(k)+KD[Δe(k)-Δe(k-1)][7]，其中，u(k)為第k次采樣時(shí)刻控制器的輸出；e(k)為第k次采樣時(shí)刻輸入的偏差；KP為比例系數(shù)，主要作用是對(duì)系統(tǒng)出現(xiàn)的誤差做出快速響應(yīng)；KD為微分系數(shù)，主要作用是給出預(yù)調(diào)量提升系統(tǒng)動(dòng)態(tài)特性。具體思想是將閉環(huán)系統(tǒng)反饋回來(lái)的實(shí)際運(yùn)行速度與給定的目標(biāo)運(yùn)行速度進(jìn)行比較，并將速度誤差作為輸入量送入PD控制器中，經(jīng)過(guò)控制器的比例、積分運(yùn)算得出相應(yīng)的誤差補(bǔ)償值，再將補(bǔ)償過(guò)的控制信號(hào)送入被控對(duì)象進(jìn)行控制。

圖3 PD控制設(shè)計(jì)框圖

3.2 程序設(shè)計(jì)

本文設(shè)計(jì)的控制系統(tǒng)程序采用模塊化設(shè)計(jì)，由系統(tǒng)初始化程序、PD算法程序、正交編碼測(cè)速程序、運(yùn)動(dòng)控制程序、定時(shí)器程序、PWM脈寬調(diào)制程序等組成，程序流程如圖4所示。根據(jù)運(yùn)動(dòng)學(xué)模型編寫運(yùn)動(dòng)控制程序。機(jī)器人運(yùn)行時(shí)，首先進(jìn)行系統(tǒng)初始化，包括定時(shí)器、串口函數(shù)、延時(shí)函數(shù)、PWM函數(shù)的初始化，接著開(kāi)啟內(nèi)外部中斷。當(dāng)接收到控制指令時(shí)，STM32F103ZET6控制芯片產(chǎn)生四路獨(dú)立的PWM信號(hào)，運(yùn)行控制程序，此時(shí)機(jī)器人將按期望的方向和速度運(yùn)動(dòng)，可通過(guò)控制器進(jìn)行加減速和換向控制。電機(jī)位置的變化引起霍爾信號(hào)變化，定時(shí)器的正交編碼計(jì)數(shù)功能將計(jì)15ms內(nèi)電機(jī)脈沖總數(shù)，輸入到PD算法計(jì)算實(shí)際轉(zhuǎn)速，與設(shè)定轉(zhuǎn)速比較后，輸出新的PWM占空比，重新對(duì)電機(jī)調(diào)速，在定時(shí)周期內(nèi)不斷進(jìn)行該步驟，使其與預(yù)定轉(zhuǎn)速相同，保證機(jī)器人的直線運(yùn)動(dòng)精度。

圖4 機(jī)器人運(yùn)動(dòng)控制系統(tǒng)程序流程

4 電機(jī)調(diào)試

本文設(shè)計(jì)了四輪結(jié)構(gòu)的Mecanum輪機(jī)器人，如圖5所示。外形尺寸為390×380mm，車輪半徑為100mm，沿y軸最大運(yùn)行速度為1.2m/s。采用12V直流減速電機(jī)，額定扭矩1N·m，提供輸出軸每轉(zhuǎn)663個(gè)反饋的脈沖信號(hào)，為提高控制精度，脈沖信號(hào)的上升沿和下降沿均計(jì)數(shù)。電機(jī)調(diào)試時(shí)預(yù)設(shè)速度為電機(jī)在15ms周期內(nèi)走過(guò)40個(gè)脈沖數(shù)。建立控制芯片的串口與PC串口之間的通信，可將機(jī)器人在設(shè)定周期內(nèi)的速度值發(fā)送到PC保存并繪制PD速度響應(yīng)曲線。

對(duì)機(jī)器人的1號(hào)電機(jī)采用PD閉環(huán)控制，不斷調(diào)整閉環(huán)模型的參數(shù)，當(dāng)取KP=65，KD=90時(shí)，電機(jī)具有較好性能。閉環(huán)后的速度相應(yīng)曲線如圖6所示，此時(shí)超調(diào)量是10%，調(diào)節(jié)時(shí)間0.33s，穩(wěn)態(tài)誤差2.5%，0.33s之后電機(jī)的實(shí)際速度與設(shè)定速度大致相同。對(duì)機(jī)器人2、3、4號(hào)電機(jī)進(jìn)行調(diào)試，超調(diào)量均低于15%，調(diào)節(jié)時(shí)間0.33s以下，穩(wěn)態(tài)誤差控制在2.5%，結(jié)果匯總?cè)绫?所示。

圖5 全向移動(dòng)機(jī)器人實(shí)體

圖6 電機(jī)1實(shí)際環(huán)境PD響應(yīng)曲線

表1 電機(jī)1、2、3、4號(hào)電機(jī)調(diào)試結(jié)果

5 運(yùn)行軌跡實(shí)驗(yàn)

電機(jī)調(diào)試完畢后，將機(jī)器人放置在工業(yè)攝像頭下進(jìn)行運(yùn)行軌跡記錄實(shí)驗(yàn)。參照機(jī)器人車輪布局圖，規(guī)定機(jī)器人運(yùn)動(dòng)時(shí)沿Y軸正方向?yàn)榍斑M(jìn)，沿X軸正方向運(yùn)動(dòng)為右平移，以原點(diǎn)為圓心順時(shí)針轉(zhuǎn)動(dòng)為右旋轉(zhuǎn)，沿45°角方向?yàn)橛仪斑M(jìn)。相機(jī)可視的像素坐標(biāo)范圍為600×500，轉(zhuǎn)換為實(shí)際坐標(biāo)為300×250cm，圖像采樣頻率為0.1s/次，實(shí)驗(yàn)前設(shè)置好軌跡記錄點(diǎn)和理想軌跡點(diǎn)，建立攝像頭與PC機(jī)之間的數(shù)據(jù)傳輸，記錄機(jī)器人的運(yùn)動(dòng)情況，軌跡信息將以坐標(biāo)形式保存。圖7為前進(jìn)、右平移、右前進(jìn)的軌跡圖及局部放大圖，圖8為原地右旋轉(zhuǎn)軌跡圖及局部放大圖。

圖7 三種典型運(yùn)動(dòng)方向軌跡

圖8 原地右旋轉(zhuǎn)運(yùn)動(dòng)軌跡

表2 四種運(yùn)行狀態(tài)數(shù)據(jù)分析

由實(shí)驗(yàn)數(shù)據(jù)作圖并分析，前進(jìn)時(shí)最大誤差為0.854cm，實(shí)驗(yàn)記錄距離為300cm。右平移時(shí)最大誤差為0.753cm，實(shí)驗(yàn)記錄距離為250cm。右前進(jìn)時(shí)最大誤差為1.632cm，實(shí)驗(yàn)記錄距離為347cm。前進(jìn)、右平移、右前進(jìn)、右旋轉(zhuǎn)的實(shí)際軌跡的坐標(biāo)點(diǎn)與理想軌跡之間的標(biāo)準(zhǔn)差分別為0.62、0.54、1.13、0.36，匯總?cè)绫?所示。由運(yùn)動(dòng)時(shí)的最大誤差及實(shí)際軌跡的坐標(biāo)點(diǎn)與理想軌跡之間的標(biāo)準(zhǔn)差可知，該控制系統(tǒng)在一定程度上保證了機(jī)器人的直線運(yùn)動(dòng)精度和原地旋轉(zhuǎn)精度，其中右前行進(jìn)時(shí)直線精度相對(duì)較差。

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

本文開(kāi)發(fā)設(shè)計(jì)了全方位移動(dòng)機(jī)器人并設(shè)計(jì)了其運(yùn)動(dòng)控制系統(tǒng)，運(yùn)用PD閉環(huán)控制方法，通過(guò)調(diào)節(jié)KP、KD參數(shù)，得到了較為理想的速度響應(yīng)曲線，使機(jī)器人能夠較為準(zhǔn)確的按程序給定的速度和方向運(yùn)行，保證了機(jī)器人運(yùn)行的直線精度，實(shí)現(xiàn)了Mecanum輪機(jī)器人的運(yùn)動(dòng)控制。

在實(shí)驗(yàn)過(guò)程中，發(fā)現(xiàn)此控制系統(tǒng)仍沒(méi)有達(dá)到最理想狀態(tài)，其可能原因是機(jī)器人本體設(shè)計(jì)時(shí)的機(jī)械公差、輪體打滑、負(fù)載不均勻、霍爾傳感器精度等因素的影響，而且本文的PD控制方法只在15ms周期內(nèi)進(jìn)行計(jì)算，還可在更短的采樣周期內(nèi)進(jìn)行閉環(huán)控制，盡量提高控制算法的實(shí)時(shí)性。在今后的研究中，將對(duì)機(jī)器人進(jìn)行動(dòng)力學(xué)建模并設(shè)計(jì)控制算法，實(shí)現(xiàn)更加精確的控制。