AGV軌跡規(guī)劃及其糾偏控制機(jī)理研究*

楊前明，張 君，阮 益

(山東科技大學(xué) 機(jī)械電子工程學(xué)院，山東 青島 266590)

0 引 言

無(wú)人數(shù)字工廠是棉紡車(chē)間未來(lái)的發(fā)展方向[1]。由于棉紡車(chē)間存在高溫度、高濕度、高噪音特點(diǎn)，機(jī)器人換人勢(shì)在必行。因此，為紡織車(chē)間設(shè)計(jì)的背負(fù)式棉桶更換復(fù)合機(jī)器人應(yīng)運(yùn)而生。

磁導(dǎo)航式差速AGV在前進(jìn)、轉(zhuǎn)向過(guò)程中，路面平整度問(wèn)題會(huì)對(duì)AGV行駛產(chǎn)生一定定位精度影響；導(dǎo)航磁帶直線度誤差以及AGV由直道轉(zhuǎn)入彎道或者由彎道轉(zhuǎn)入直道等情況，也會(huì)致使AGV在行駛過(guò)程中產(chǎn)生偏差。

國(guó)內(nèi)外關(guān)于糾偏控制的研究較多，像多窗口實(shí)時(shí)測(cè)距[2]、串級(jí)軌跡跟蹤算法、卡爾曼濾波器定位等[3]，這些算法以及控制方法比較適合于無(wú)軌導(dǎo)航，工程上經(jīng)常采用PID算法對(duì)AGV行駛軌跡進(jìn)行實(shí)時(shí)的糾偏[4-5]，PID控制器、模糊控制器以及模糊PID比較適合于有軌導(dǎo)航。

在磁導(dǎo)引中，在實(shí)現(xiàn)AGV的導(dǎo)引前提下，也不斷研究AGV的定位以及糾偏方法，提高AGV的定位精度。PWM調(diào)速是利用功率開(kāi)關(guān)的器件的導(dǎo)通與關(guān)斷，將直流電壓變成連續(xù)的直流脈沖序列，并通過(guò)控制脈沖寬度或周期達(dá)到變壓調(diào)速的目的。

本文以PLC為控制器，基于PWM調(diào)速與PID控制，探討直流電機(jī)調(diào)速與AGV糾偏調(diào)速控制系統(tǒng)的實(shí)現(xiàn)方法。

1 AGV糾偏模型建立

1.1 AGV運(yùn)動(dòng)學(xué)模型建立

自主式磁導(dǎo)航AGV在行駛過(guò)程中,隨著糾偏傳感器位置信號(hào)連續(xù)采集送入控制器，同時(shí)給定直流電機(jī)糾偏信號(hào)不斷接近目標(biāo)軌跡。

本研究以固定點(diǎn)O為參考點(diǎn)，建立固定參考坐標(biāo)系I[X-O-Y]；以AGV上質(zhì)心點(diǎn)P作為AGV局部參考系位置參考點(diǎn)，建立局部參考系Ⅱ[XR-P-YR][6]，如圖1所示。

圖1 AGV糾偏環(huán)節(jié)運(yùn)動(dòng)模型

(1)

AGV車(chē)體的角速度和線速度為：

(2)

式中：r—車(chē)輪半徑，mm；l—質(zhì)心P點(diǎn)至兩輪距離，mm；VR—右輪運(yùn)動(dòng)速度，mm/s;VL—左輪運(yùn)動(dòng)速度,mm/s。

差速驅(qū)動(dòng)AGV的運(yùn)動(dòng)學(xué)模型為：

(3)

1.2 糾偏調(diào)速系統(tǒng)數(shù)學(xué)模型建立

在圖1所示的參考系中，在參考系P{XR，YR，θ}中，AGV上點(diǎn)P(x,y,θ)是其實(shí)時(shí)位置坐標(biāo)，參考系I中點(diǎn)O為其目標(biāo)位置，OP之間距離偏差為(Δx，Δy)，為Ⅱ平面XR軸與目標(biāo)位置向量之間的夾角，即偏差角。

在坐標(biāo)系I中，AGV運(yùn)動(dòng)模型可表述為：

(4)

其極坐標(biāo)可描述為：

(5)

α=-θ+arctan2(Δy,Δx)

(6)

β=-θ-α

(7)

在新的極坐標(biāo)中，得到一個(gè)系統(tǒng)描述為：

(8)

從式(8)中可以看出，當(dāng)ρ=0時(shí)出現(xiàn)不連續(xù)性，但Brockett定理不妨礙平滑穩(wěn)定性[7]，即：

v=kρρ

(9)

ω=kαα+kββ

(10)

由式(8)可得到閉環(huán)控制系統(tǒng)的數(shù)學(xué)模型為：

(11)

式中：ρ—AGV輪軸的中心與目標(biāo)位置之間的距離，mm；kρ—偏差距離控制參數(shù)；kα—偏差角度α控制參數(shù)；kβ—β角控制參數(shù)。

1.3 糾偏原理

由上述數(shù)學(xué)模型分析可知：AGV的位置糾偏一直貫穿于其行駛過(guò)程，軌跡跟蹤就是減少AGV行駛過(guò)程中實(shí)際軌跡與預(yù)定軌跡間的距離偏差ρ與角度偏差α[8]。

當(dāng)ρ=0時(shí)，即(ρ，α，β)=(0,0,0)是一個(gè)唯一的平衡點(diǎn)，直流電機(jī)驅(qū)動(dòng)AGV行駛至該位置，這就是目標(biāo)位置。

AGV通過(guò)接收磁導(dǎo)航傳感器的位置信息，判斷此時(shí)與預(yù)定軌道的位置。若AGV發(fā)生偏轉(zhuǎn)，與預(yù)定軌道的距離偏差為ρ，角度偏差α，控制器通過(guò)PID指令向驅(qū)動(dòng)電機(jī)發(fā)送調(diào)節(jié)指令，對(duì)AGV軌跡糾偏回歸到預(yù)定軌道。此時(shí)磁導(dǎo)航傳感器將位置信號(hào)傳遞給PLC，PLC根據(jù)磁導(dǎo)航輸出的信號(hào)進(jìn)行處理，從而發(fā)出相應(yīng)的控制指令調(diào)節(jié)直流電機(jī)轉(zhuǎn)速[9]。

2 直流電機(jī)糾偏控制系統(tǒng)

2.1 直流電機(jī)調(diào)速控制系統(tǒng)

本研究采用PWM的調(diào)速方式實(shí)現(xiàn)對(duì)電機(jī)轉(zhuǎn)速以及電流的控制。在直流無(wú)刷電機(jī)控制系統(tǒng)中，采用電流內(nèi)環(huán)、PID外環(huán)的閉環(huán)控制[10]，直流電機(jī)動(dòng)態(tài)PWM調(diào)速模型如圖2所示。

圖2 直流電機(jī)動(dòng)態(tài)PWM調(diào)速模型

無(wú)刷直流電機(jī)調(diào)速系統(tǒng)常常會(huì)受到一定外界干擾，雖然直流電機(jī)在一定程度上可以對(duì)隨機(jī)干擾進(jìn)行調(diào)節(jié)，但是當(dāng)系統(tǒng)輸入值受到外界干擾產(chǎn)生變化時(shí)，同時(shí)也會(huì)引起輸出值的變化。若這些偏差不能及時(shí)處理，就會(huì)產(chǎn)生一定誤差。采用PID控制進(jìn)行補(bǔ)償，減少干擾對(duì)轉(zhuǎn)速的影響是通常采用的方法之一[11]。

2.2 PID糾偏實(shí)現(xiàn)方法

將AGV運(yùn)動(dòng)過(guò)程以左、右輪轉(zhuǎn)速偏差Δv的方式體現(xiàn)，用e表示，輸出u為兩驅(qū)動(dòng)電機(jī)控制電壓，則傳統(tǒng)的位置式PID糾偏控制表達(dá)式可以表示為：

(12)

式中：e(k)，ec—系統(tǒng)偏差和系統(tǒng)偏差變化率；KP,KI,KD—比例系數(shù)、積分系數(shù)和微分系數(shù)。

由上式可知，位置式PID算法需要對(duì)偏差e進(jìn)行累加求和，每次輸出均與以前的狀態(tài)相關(guān)，計(jì)算時(shí)需要對(duì)e(k)進(jìn)行累加，工作量比較大。為了避免初次啟動(dòng)造成瞬間微分值過(guò)大的缺點(diǎn)，本研究采用監(jiān)看測(cè)定值(PV)的微分狀況，PID指令控制框圖如圖3所示。

圖3 PID指令控制框圖

則PID表達(dá)式為：

(13)

式中：u(k)—輸出值；uSV(k)—目標(biāo)值；uPV(k)—測(cè)定值。

PID控制原理是將輸入值SV與測(cè)定值PV偏差e輸入PID控制器中，設(shè)定PID控制中的KP，KI，KD3個(gè)參數(shù)，對(duì)直流電機(jī)進(jìn)行調(diào)速[12-13]。

該指令采用監(jiān)看測(cè)定值PV的微分狀況，當(dāng)測(cè)定值PV變化量過(guò)大時(shí)，即車(chē)輪速度變化量過(guò)大時(shí)，則將會(huì)降低輸出值MV的輸出，從而以PWM波的形式輸出對(duì)電機(jī)進(jìn)行調(diào)速。

PLC作為控制器，通過(guò)其內(nèi)部PID處理器，將輸入輸出信號(hào)進(jìn)行AD轉(zhuǎn)換。基于PLC的PID指令，首先要進(jìn)行參數(shù)初始化，在指定時(shí)間點(diǎn)上向模擬量取值，取出模擬量后，需要采用模擬量輸入(A/D)轉(zhuǎn)換模塊進(jìn)行模擬量輸入，最后通過(guò)模擬量輸出模塊(D/A)輸出或采用PWM波形輸出[14]。

3 仿真與實(shí)驗(yàn)分析

3.1 仿真模型建立

根據(jù)圖2可知，直流電機(jī)PWM調(diào)速環(huán)節(jié)的傳遞函數(shù)為[15]：

(14)

直流電機(jī)經(jīng)PWM調(diào)速后的轉(zhuǎn)速與輸入電壓傳遞函數(shù)為：

(15)

式中：Ce—電動(dòng)勢(shì)常數(shù)，0.2；Tm—電氣傳動(dòng)時(shí)間常數(shù)，0.009 8；T1—時(shí)間常數(shù)，L/R∑；L—電樞回路總電感，0.18 mH；R∑—電樞回路總電阻，0.18 Ω。

PID環(huán)節(jié)傳遞函數(shù)為：

(16)

直流電機(jī)PID調(diào)速控制系統(tǒng)閉環(huán)傳遞函數(shù)為：

(17)

式中：α—反饋系數(shù)，0.015 V·min/r。

3.2 系統(tǒng)仿真及分析

系統(tǒng)仿真的思想是根據(jù)直流電機(jī)PID調(diào)速控制系統(tǒng)的傳遞函數(shù)，考查在不同PID參數(shù)下其響應(yīng)曲線特征規(guī)律。

PID參數(shù)采用臨界比例度法[16]，按照P-I-D環(huán)節(jié)操作順序，整定參數(shù)。PID參數(shù)調(diào)節(jié)過(guò)程及AGV運(yùn)行軌跡如圖4所示。

圖4 PID參數(shù)調(diào)節(jié)過(guò)程及AGV運(yùn)行軌跡

經(jīng)Matlab仿真，在參數(shù)整定后得到的理想的AGV糾偏階躍響應(yīng)曲線如圖5所示。

圖5 AGV糾偏階躍響應(yīng)

由圖5可知：KP=4 000，KI=10，KD=1時(shí)，系統(tǒng)響應(yīng)快速、性能穩(wěn)定，無(wú)超調(diào)、無(wú)振蕩，調(diào)節(jié)時(shí)間約為3 s，滿足糾偏性能要求。

根據(jù)式(4，8)，對(duì)直流電機(jī)進(jìn)行糾偏調(diào)速，可得到AGV軌跡仿真曲線，如圖6所示。

圖6 AGV軌跡仿真曲線及實(shí)驗(yàn)原理圖

圖6中，AGV以(0,0)點(diǎn)為起始點(diǎn)，直線距離為3 m，彎道半徑為1.5 m的環(huán)形跑道進(jìn)行行駛，該仿真軌跡基本與標(biāo)準(zhǔn)軌跡重合，滿足糾偏要求。

3.3 實(shí)驗(yàn)測(cè)試與分析

圖6中，磁條軌跡與AGV構(gòu)成了系統(tǒng)測(cè)試方案，在模擬作業(yè)區(qū)鋪設(shè)磁條軌跡，復(fù)合機(jī)器人沿著磁條軌跡進(jìn)行試運(yùn)行。

為進(jìn)一步分析調(diào)節(jié)參數(shù)對(duì)電機(jī)調(diào)速的影響，筆者讓AGV沿著固定軌跡進(jìn)行連續(xù)行駛，在AGV運(yùn)行軌跡上設(shè)置4個(gè)檢測(cè)點(diǎn)，并分別在4個(gè)檢測(cè)點(diǎn)粘貼白紙，使白紙的中心位置與磁條的中心位置重合，以藍(lán)線標(biāo)識(shí)。AGV沿著磁條運(yùn)行，當(dāng)運(yùn)行到檢測(cè)點(diǎn)時(shí)，固定在車(chē)體中心處的記號(hào)筆將會(huì)在白紙上留下AGV運(yùn)行痕跡，并檢測(cè)實(shí)際軌跡與理想軌跡的偏差距離，以磁條中心線右側(cè)的偏差為正。

現(xiàn)場(chǎng)實(shí)際運(yùn)行測(cè)試如圖7所示。

圖7 現(xiàn)場(chǎng)模擬運(yùn)行實(shí)驗(yàn)測(cè)試照片

圖7中右上角給出了AGV軌跡上4個(gè)點(diǎn)的實(shí)驗(yàn)糾偏檢測(cè)結(jié)果分布情況。當(dāng)KP過(guò)小時(shí)，實(shí)際軌跡偏差較大；當(dāng)KP=4 000時(shí)，實(shí)際軌跡偏差較小。

不同調(diào)節(jié)參數(shù)下AGV上某固定點(diǎn)與標(biāo)準(zhǔn)軌跡間的偏差Δx測(cè)試如表1所示。

表1 AGV偏差距離表

由圖7以及表1可以看出：當(dāng)KP=4 000時(shí)，經(jīng)過(guò)多次實(shí)驗(yàn)，檢測(cè)到在檢測(cè)點(diǎn)的偏差平均值在±10 mm以內(nèi)，滿足AGV的定位要求，同時(shí)根據(jù)實(shí)際AGV行駛路徑以及仿真路徑發(fā)現(xiàn)，AGV運(yùn)行軌跡與標(biāo)準(zhǔn)軌跡基本一致。

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

針對(duì)紡織車(chē)間棉桶搬運(yùn)自動(dòng)化問(wèn)題，本文提出了復(fù)合機(jī)器人及配套技術(shù)總體解決方案：

(1)針對(duì)復(fù)合機(jī)器人糾偏調(diào)速問(wèn)題，本文建立了其基于軌跡跟蹤系統(tǒng)運(yùn)動(dòng)學(xué)模型，運(yùn)用Matlab軟件進(jìn)行了仿真，結(jié)果表明：AGV運(yùn)行軌跡與標(biāo)準(zhǔn)軌跡基本重合，驗(yàn)證了基于軌跡跟蹤系統(tǒng)運(yùn)動(dòng)學(xué)建模方法的正確性；

(2)針對(duì)直流電機(jī)PWM調(diào)速方式，本文給出了基于PID控制的直流電機(jī)脈寬調(diào)速方法，建立了系統(tǒng)數(shù)學(xué)模型，采用Matlab進(jìn)行了仿真。結(jié)果顯示：糾偏過(guò)程無(wú)超調(diào)、無(wú)振蕩、調(diào)節(jié)速度快；對(duì)現(xiàn)場(chǎng)實(shí)驗(yàn)測(cè)試表明：復(fù)合機(jī)器人運(yùn)行軌跡上檢測(cè)點(diǎn)的偏差<±10 mm，滿足AGV定位精度要求。

現(xiàn)場(chǎng)運(yùn)行過(guò)程發(fā)現(xiàn)，AGV能夠平穩(wěn)運(yùn)行，可以實(shí)現(xiàn)自主導(dǎo)航以及糾偏功能，糾偏效果較好，可以完成定點(diǎn)停靠以及棉桶的搬運(yùn)工作。