協(xié)同搬運(yùn)系統(tǒng)中的麥克納姆輪AGV運(yùn)動(dòng)控制研究

李彥文，樓佩煌，樓航飛，翟晶晶

(南京航空航天大學(xué) 機(jī)電學(xué)院，江蘇 南京 210016)

0 引言

隨著智能物流的提出，多AGV協(xié)同搬運(yùn)技術(shù)的研究逐漸成為此領(lǐng)域的研究熱點(diǎn)[1]。協(xié)同搬運(yùn)的目的是使被搬運(yùn)工件穩(wěn)定到達(dá)工位點(diǎn)，其關(guān)鍵技術(shù)在于選取合適的控制方法來保證系統(tǒng)內(nèi)AGV在路徑跟蹤過程中能夠穩(wěn)定運(yùn)行。本文基于麥克納姆輪AGV作為協(xié)同搬運(yùn)的運(yùn)動(dòng)平臺(tái)，利用運(yùn)動(dòng)學(xué)建模分析偏差產(chǎn)生的主要因素，運(yùn)用模糊PID控制算法實(shí)時(shí)調(diào)整PID參數(shù)，快速平穩(wěn)地消除系統(tǒng)內(nèi)AGV路徑跟蹤過程中產(chǎn)生的角度偏差和距離偏差，提高了搬運(yùn)系統(tǒng)的穩(wěn)定性。

1 麥克納姆輪AGV運(yùn)動(dòng)學(xué)模型分析

基于麥克納姆輪結(jié)構(gòu)的全向AGV由于其運(yùn)動(dòng)控制靈活，在智能化運(yùn)輸車間得到廣泛應(yīng)用[2]，通過對(duì)麥克納姆輪系有規(guī)律地布局，AGV可以在平面內(nèi)任意自由度運(yùn)動(dòng)。本文通過建立AGV車體速度與電機(jī)實(shí)際轉(zhuǎn)速的矩陣分配方程，將系統(tǒng)的整體運(yùn)動(dòng)控制轉(zhuǎn)換成各輪的運(yùn)動(dòng)控制。

如圖1所示，建立全局坐標(biāo)系Oxy-z，車輪坐標(biāo)系Oixiyi-zi，設(shè)AGV的速度為vx、vy，角速度為ωz；車輪的轉(zhuǎn)速為ωi，半徑為R，輥?zhàn)拥木€速度為vir，則車輪的線速度為viω=ωiR；L1、L2分別為垂直、水平方向上輪距的一半，輥?zhàn)悠媒铅翞?5°，由圖1可得四驅(qū)麥克納姆輪AGV的正運(yùn)動(dòng)學(xué)方程為

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

(1)

J為全方位AGV系統(tǒng)的雅可比矩陣，rank(J)=3，所以vx、vy、ωz相互獨(dú)立[3]。由式(1)可知：

1)當(dāng)ω1=ω2=ω3=ω4=ω時(shí)，vx=0，vy=Rω，ωz=0,小車沿y方向平移。

2)當(dāng)ω1=-ω2=-ω3=ω4=ω時(shí)，vx=Rω，vy=0，ωz=0,小車沿x方向平移。

其中：vx、vy和路徑跟蹤的距離偏差ed以及沿軌跡的切線速度vτ相關(guān)；ωz與路徑跟蹤的角度偏差eθ相關(guān)，為提高搬運(yùn)系統(tǒng)內(nèi)AGV路徑跟蹤的穩(wěn)定性奠定基礎(chǔ)。

2 AGV路徑跟蹤運(yùn)動(dòng)學(xué)模型分析

AGV路徑跟蹤過程中的距離偏差ed和角度偏差eθ如圖2(a)所示，θ為AGV實(shí)際運(yùn)行速度v與車身方向的夾角，vτ為AGV在路徑跟蹤過程中的期望切線速度。vx、vy、vτ和v存在如下關(guān)系：

(2)

設(shè)t時(shí)刻角度偏差為eθ(t)，經(jīng)過控制周期T到達(dá)t+T時(shí)刻的角度偏差為eθ(t+T)。由于控制周期極短，ωz可看作固定值，此時(shí)AGV轉(zhuǎn)過的角度為ωzT，則有

eθ(t+T)=eθ(t)+ωzT

(3)

采用幾何方法計(jì)算距離偏差[4]，假設(shè)當(dāng)前狀態(tài)下∑xOy不隨AGV運(yùn)動(dòng)，觀察AGV運(yùn)動(dòng)到下一個(gè)時(shí)刻的距離偏差變化情況，如圖2(b)所示，假設(shè)當(dāng)前t時(shí)刻AGV相較于軌跡線的距離偏差為ed(t)=lOE，經(jīng)過控制周期T到達(dá)t+T時(shí)刻的距離偏差為ed(t+T)=lBD。由于控制周期T很短，角度偏差變化量較小，AGV運(yùn)動(dòng)軌跡可近似看作直線OB，且lOB=vT。距離偏差的減小量AB為lAB=vxT，距離偏差的增加量CD為lCD=lCEtaneθ=vy(t)·Ttaneθ(t)。由于lBD=lOE+lCD-lAB，可得下一時(shí)刻距離偏差ed(t+1)為

圖2 AGV路徑跟蹤的偏差分析

ed(t+1)=ed(t)+vy(t)Ttaneθ(t)-Tvx(t)

(4)

由于在小角度偏差情況下，eθ(t)很小，由式(2)可知，vy=vτ-vxeθ，代入式(4)并結(jié)合式(3)可得控制周期為T的路徑糾偏下AGV的運(yùn)動(dòng)學(xué)偏差模型為

(5)

根據(jù)式(5)可知，角度偏差eθ(t+T)的變化完全取決于ωz，距離偏差ed(t+T)的變化不僅取決于vx，還與前一狀態(tài)的角度偏差eθ(t)有關(guān)，因此本文通過控制vx和ωz來消除這兩種偏差，調(diào)整位姿狀態(tài)。

3 模糊PID控制方法

傳統(tǒng)PID控制被廣泛應(yīng)用在自動(dòng)化領(lǐng)域，但存在響應(yīng)速度較慢、魯棒性差、不具備在線整定Kp、Ki、Kd控制參數(shù)等缺點(diǎn)，無法保證系統(tǒng)在不同偏差情況下實(shí)時(shí)穩(wěn)定運(yùn)行的要求[5]。而模糊控制具有抗干擾能力強(qiáng)、響應(yīng)速度快、對(duì)系統(tǒng)參數(shù)變化有較強(qiáng)的魯棒性等特點(diǎn)，適合于非線性、時(shí)變的系統(tǒng)控制。通過將兩者結(jié)合，在線整定Kp、Ki、Kd控制參數(shù)，不僅使機(jī)器人系統(tǒng)具有PID控制的控制精度高，也擁有模糊控制的適應(yīng)性強(qiáng)特點(diǎn)[6]。

本文采用模糊PID控制器在AGV路徑跟蹤過程中，通過傳感器采集到的距離偏差ed和角度偏差eθ及其變化率在線改變PID控制參數(shù)，從而提高對(duì)vx和ωz控制的動(dòng)態(tài)性，提高AGV在路徑跟蹤過程中的穩(wěn)定性,其原理圖如圖3所示。

圖3 模糊PID控制原理圖

3.1 輸入變量模糊化

將距離偏差ed和角度偏差eθ及其變化率作為模糊控制的輸入量，語言變量Ed、ECd、Eθ、ECθ，基本論域?yàn)閧-3,-2,-1,0,1,2,3}。PID控制器中各參數(shù)的增量ΔKP、ΔKI、ΔKD作為模糊控制的輸出量，語言變量為Up、Ui、Ud，基本論域分別為(-0.5,0.5)，(-0.1,0.1)，(-0.5,0.5)。輸入與輸出的模糊子集都設(shè)定為{NB,NM,NS,ZO,PS,PM,PB}，即負(fù)大、負(fù)中、負(fù)小、零、正小、正中、正大。考慮到系統(tǒng)的實(shí)時(shí)性，輸入隸屬度函數(shù)以及輸出隸屬度函數(shù)選擇三角函數(shù)。

3.2 建立模糊規(guī)則

本文基于AGV路徑跟蹤過程中產(chǎn)生的距離偏差和角度偏差，設(shè)計(jì)了兩個(gè)模糊PID控制器。依據(jù)大量實(shí)驗(yàn)結(jié)果，結(jié)合AGV應(yīng)用場(chǎng)景，建立了以下輸出變量的模糊規(guī)則：

1)當(dāng)距離偏差ed或角度偏差eθ較大，且偏差變化率較大時(shí)，KP應(yīng)較大幅度糾偏，加快AGV響應(yīng)速度，為避免出現(xiàn)超出許可范圍的超調(diào)現(xiàn)象，應(yīng)減小KI、KD的取值。

2)當(dāng)距離偏差ed或角度偏差eθ和偏差變化率為中等時(shí)，為保證響應(yīng)速度，KP應(yīng)較小幅度糾偏，為防止出現(xiàn)過大的超調(diào)，KI、KD應(yīng)較小幅度糾偏。

3)當(dāng)距離偏差ed或角度偏差eθ較小，且偏差變化率較大時(shí)，KI應(yīng)較小幅度糾偏，盡可能提高AGV的穩(wěn)態(tài)性能，KD應(yīng)較大幅度糾偏。

以ed為例，列出輸出規(guī)則見表1，eθ也可以類似地推算出來。

表1 模糊控制規(guī)則表

3.3 去模糊化

通過上述的模糊推理過程得到兩控制器輸出的模糊集，此時(shí)還需要將模糊量轉(zhuǎn)換為精確量，即去模糊化。常用的解模糊算法有最大隸屬度法、質(zhì)心法和加權(quán)平均法，本文選用加權(quán)平均化法去模糊化，其公式如下：

(6)

通過上式離線計(jì)算得到的去模糊化后的精確控制量存放在系統(tǒng)控制決策表中，在程序運(yùn)行過程中通過查表讀取當(dāng)前狀態(tài)下模糊控制器的輸出數(shù)據(jù)ΔKP、ΔKI、ΔKD，與前一狀態(tài)的控制參數(shù)相加即為PID控制器更新后的參數(shù)。利用此方法可以優(yōu)化計(jì)算時(shí)間，提高實(shí)時(shí)性。

4 實(shí)驗(yàn)分析

為驗(yàn)證本文提出的模糊PID控制器對(duì)提高搬運(yùn)系統(tǒng)穩(wěn)定性的有效性，搭建AGV協(xié)同搬運(yùn)系統(tǒng)，在此平臺(tái)上進(jìn)行實(shí)驗(yàn)驗(yàn)證。其中L1=400 mm,L2=280 mm,R=120 mm，色帶寬度為30 mm。當(dāng)平臺(tái)沿固定色帶運(yùn)行時(shí)，設(shè)定初始的期望參數(shù)為vx=0、ωz=0、vy=vτ=0.5 m/s，實(shí)驗(yàn)結(jié)果的有效性通過其中一臺(tái)AGV車底中心的PGV傳感器反饋的偏差數(shù)據(jù)來印證。由于平臺(tái)存在初始位置擺放偏差，所以起始偏差較大，AGV在模糊PID控制下的路徑跟蹤偏差如圖4所示，在傳統(tǒng)PID控制下的路徑跟蹤偏差如圖5所示。

圖4 模糊PID控制下的偏差

圖5 傳統(tǒng)PID控制下的偏差

對(duì)比兩種控制下PGV反饋的偏差數(shù)據(jù)可以得出，AGV在直線路段路徑跟蹤過程中，模糊PID控制下的距離偏差相對(duì)傳統(tǒng)PID控制較小，且響應(yīng)時(shí)間較快，波動(dòng)較小；當(dāng)AGV行駛在圓弧路段時(shí)的距離偏差都比行駛在直線時(shí)的距離偏差較大，且模糊PID控制下的距離偏差相對(duì)波動(dòng)平緩，提前結(jié)束圓弧路段；對(duì)于角度偏差，模糊PID控制相比傳統(tǒng)PID控制偏差較小。綜上所述，本文針對(duì)AGV設(shè)計(jì)的模糊PID控制算法能使協(xié)同搬運(yùn)系統(tǒng)穩(wěn)定運(yùn)行。

5 結(jié)語

隨著機(jī)器人應(yīng)用場(chǎng)景的復(fù)雜化、大型化，多機(jī)器人協(xié)作搬運(yùn)是機(jī)器人技術(shù)發(fā)展的必然趨勢(shì)。多機(jī)器人系統(tǒng)可以有效提高單機(jī)器人在功能、效率以及負(fù)載能力等方面的不足，日后多機(jī)器人系統(tǒng)的協(xié)作能力也會(huì)成為智能機(jī)器人技術(shù)的重要衡量指標(biāo)[7]。本文針對(duì)協(xié)同搬運(yùn)系統(tǒng)內(nèi)的AGV在路徑跟蹤過程中產(chǎn)生的距離偏差和角度偏差，設(shè)計(jì)了模糊PID控制器，通過在線調(diào)整 PID控制參數(shù)，提升了協(xié)同搬運(yùn)系統(tǒng)的穩(wěn)定性，仿真實(shí)驗(yàn)結(jié)果驗(yàn)證了該算法的有效性。