差速驅(qū)動(dòng)AGV建模和軌跡跟蹤控制研究

郭虎虎，任 芳，龐新宇，金 澤

（1.太原理工大學(xué)機(jī)械與運(yùn)載工程學(xué)院，山西 太原 030024；2.煤礦綜采裝備山西省重點(diǎn)實(shí)驗(yàn)室，山西 太原 030024）

1 引言

自動(dòng)導(dǎo)引車輛（Automated Guided Vehicle，AGV）是一種新型的裝載運(yùn)輸物料的工業(yè)車輛，采用自動(dòng)或人工方式裝裝卸貨物，并按設(shè)定的路徑自動(dòng)行駛至指定地點(diǎn)。AGV 具有自動(dòng)化、柔性化和準(zhǔn)時(shí)性等優(yōu)點(diǎn)，被廣泛應(yīng)用于汽車制造、醫(yī)藥、倉儲(chǔ)、化工等領(lǐng)域［1］。軌跡跟蹤是AGV 實(shí)現(xiàn)運(yùn)動(dòng)精確控制和執(zhí)行任務(wù)成敗的決定性因素，是AGV研究的核心問題。AGV是一種非線性、強(qiáng)耦合的多輸入多輸出的復(fù)雜系統(tǒng)，在實(shí)際應(yīng)用中AGV 軌跡跟蹤會(huì)受到輪胎摩擦、路面情況、負(fù)載變化等多種外界因素的影響［2］，導(dǎo)致AGV 的行駛路徑偏離預(yù)先設(shè)計(jì)的參考軌跡。目前，一些科研工作者針對AGV 軌跡跟蹤控制難問題，提出了各種控制算法，如Backstepping 方法［3-4］、自適應(yīng)控制［4］、滑?？刂疲?-6］、模糊控制［7］和神經(jīng)網(wǎng)絡(luò)控制［8］等。文獻(xiàn)［3］針對機(jī)器人動(dòng)力學(xué)模型，應(yīng)用了Lyapunov直接法構(gòu)造了全局穩(wěn)定軌跡跟蹤控制器，實(shí)現(xiàn)了軌跡跟蹤，但為未考慮未知干擾對機(jī)器人軌跡跟蹤的影響。文獻(xiàn)［6］針對質(zhì)心和幾何中心存在偏差的移動(dòng)機(jī)器人，基于動(dòng)力學(xué)模型設(shè)計(jì)了具有全局穩(wěn)定的自適應(yīng)滑模控制器，該控制器滿足軌跡跟蹤要求并消除滑模輸入抖振。文獻(xiàn)［7］針對差速輪式移動(dòng)機(jī)器人軌跡跟蹤存在的問題，提出一種與PD控制相結(jié)合的模糊滑模變結(jié)構(gòu)控制，保證了對不確定性和干擾的魯棒性，并可以有效地跟蹤參考軌跡。文獻(xiàn)［9］針對移動(dòng)機(jī)器人軌跡跟蹤，提出一種基于Backstepping 運(yùn)動(dòng)學(xué)控制器與雙自適應(yīng)神經(jīng)滑模動(dòng)力學(xué)控制器，不但可以解決系統(tǒng)不確定性問題，還消除了滑?？刂浦械亩墩瘳F(xiàn)象?？紤]到設(shè)計(jì)的差速驅(qū)動(dòng)AGV將應(yīng)用于工廠物料運(yùn)輸，在實(shí)際工作中會(huì)存在多種不確定因素的影響。為了使AGV能夠在未知環(huán)境中能夠具有較高精度的軌跡跟蹤控制，參考文獻(xiàn)中有關(guān)算法，提出并設(shè)計(jì)了一種與Backstepping控制相結(jié)合的自適應(yīng)模糊滑?？刂疲米赃m應(yīng)模糊邏輯控制器取代滑?？刂破髦锌刂菩盘?hào)不連續(xù)的部分，避免了滑模控制輸入抖振，滑?？刂破鞅ＷC了系統(tǒng)對參數(shù)攝動(dòng)和外界干擾的魯棒性?；贚yapunov穩(wěn)定性理論，分析證明了差速驅(qū)動(dòng)AGV系統(tǒng)的魯棒性和跟蹤誤差收斂性和自適應(yīng)規(guī)律。直線和圓弧軌跡仿真試驗(yàn)結(jié)果表明了所設(shè)計(jì)控制策略可以有效地跟蹤參考軌跡，并且可以消除由滑?？刂埔鸬妮斎攵墩?。

2 AGV的數(shù)學(xué)模型

2.1 AGV的運(yùn)動(dòng)學(xué)模型

只考慮AGV在二維水平面上運(yùn)動(dòng)情形，其運(yùn)動(dòng)學(xué)模型，如圖1所示。坐標(biāo)系XOY為廣義坐標(biāo)系，G是AGV車體的質(zhì)心點(diǎn)，P是AGV左右驅(qū)動(dòng)輪的中心點(diǎn)，也是AGV車體的幾何中心點(diǎn)。取幾何中心點(diǎn)P作為AGV 小車在廣義坐標(biāo)系中的參考點(diǎn)，用q=[xP，yP，θ]T表示AGV在廣義坐標(biāo)系中位姿狀態(tài)矢量。l為質(zhì)心點(diǎn)與幾何中心點(diǎn)之間的距離；b為AGV幾何中心點(diǎn)P到驅(qū)動(dòng)輪中心線的距離；2r為AGV小車驅(qū)動(dòng)輪的直徑。[v，ω]為AGV車體的線速度和角速度。

圖1 AGV結(jié)構(gòu)簡圖Fig.1 The Structure of AGV

AGV驅(qū)動(dòng)輪在滿足純滾動(dòng)、無側(cè)向滑動(dòng)的條件下，AGV小車受到非完整約束，約束方程的形式如下：

由圖1 可知，AGV 車體的質(zhì)心點(diǎn)G和幾何中心點(diǎn)P并不重合，由點(diǎn)G和點(diǎn)P的位置關(guān)系，求得點(diǎn)P的速度為：

AGV小車以點(diǎn)P作為參考點(diǎn)，將AGV小車的線速度和角速度[v，ω]轉(zhuǎn)化為AGV在廣義坐標(biāo)系下的廣義速度則AGV小車的運(yùn)動(dòng)學(xué)模型可描述為：

2.2 AGV的動(dòng)力學(xué)模型

采用Euler-Lagrange 方法構(gòu)造差速驅(qū)動(dòng)AGV 的動(dòng)力學(xué)模型，分析其動(dòng)力學(xué)特性對系統(tǒng)穩(wěn)定性及軌跡跟蹤控制性和魯棒性的影響。根據(jù)Euler-Lagrange建模方法，AGV小車的動(dòng)力學(xué)模型可由如下Lagrange方程描述：

式中：AT(q)—AGV以點(diǎn)P為參考點(diǎn)時(shí)的約束矢量；λ—附加約束力條件即Lagrange乘子；E(q)—以P為參考點(diǎn)時(shí)的輸入力矩轉(zhuǎn)換矩陣；τ=[τL，τR]T—AGV左右驅(qū)動(dòng)輪電機(jī)控制輸入力矩矢量。

將點(diǎn)P的廣義坐標(biāo)和式（3）代入式（4），通過計(jì)算并整理，則AGV動(dòng)力學(xué)模型可以描述為：

式中：M(q)—系統(tǒng)以P為參考點(diǎn)時(shí)的慣性矩陣，是正定對稱矩陣；B(q，)—系統(tǒng)以P為參考點(diǎn)時(shí)的向心力和哥氏力矩陣；F(q，)—系統(tǒng)表面摩擦力；G(q)—系統(tǒng)重力項(xiàng)；τd—系統(tǒng)有界干擾。

對AGV運(yùn)動(dòng)學(xué)模型式（3）兩邊求微分得：

3 AGV自適應(yīng)模糊滑?？刂破髟O(shè)計(jì)

3.1 AGV運(yùn)動(dòng)學(xué)控制器設(shè)計(jì)

設(shè)AGV的期望位姿矢量為qr=[xr，yr，θr]T，期望速度矢量為Vr=[vr，ωr]T；實(shí)際位姿矢量為q=[x，y，θ]T，速度矢量為V=[v，ω]T。則AGV在廣義坐標(biāo)系的軌跡跟蹤位姿誤差和位姿誤差微分方程分別為：

采用Backstepping技術(shù)設(shè)計(jì)AGV系統(tǒng)運(yùn)動(dòng)學(xué)控制器[6]，選取的Lyapunov候選函數(shù)為：

式中：kɑ、kb—正常數(shù)。將式（13）代入式（12），得：

當(dāng)kɑ＞0，kb＞0，且vr＞0 時(shí)，由式（11）和式（14）可知L1≥0，≤0。由此根據(jù)AGV軌跡跟蹤位姿誤差模型選取的AGV虛擬控制速度滿足Lyapunov穩(wěn)定性條件，能使AGV的軌跡跟蹤位姿誤差qe=[xe，ye，θe]T在有限時(shí)間內(nèi)趨向于零。

3.2 AGV動(dòng)力學(xué)控制器設(shè)計(jì)

選取AGV虛擬控制速度矢量為VC=[vC，ωC]T，定義AGV的速度誤差為：

當(dāng)AGV系統(tǒng)不存在外部干擾并且模型參數(shù)確定時(shí)，則等效控制律（18）可使系統(tǒng)穩(wěn)定在滑模面上[7]?？紤]到在現(xiàn)實(shí)使用環(huán)境中，AGV系統(tǒng)模型存在參數(shù)攝動(dòng)和外部干擾的影響，必須考慮引入切換控制律τsw來進(jìn)行補(bǔ)償，則等效控制律和切換控制律的聯(lián)合控制律為：

3.3 自適應(yīng)模糊滑?？刂破髟O(shè)計(jì)與穩(wěn)定性分析

AGV系統(tǒng)的滑?？刂坡桑?9）中存在不連續(xù)切換特性，將會(huì)引起系統(tǒng)抖振。利用模糊系統(tǒng)的萬能逼近特性，用模糊控制增益G^(S)來逼近滑?？刂浦械腒sgn(S)項(xiàng)，來消除抖振現(xiàn)象[8]，這樣新的控制律可描述為：

自適應(yīng)模糊控制器擬采用單值模糊器、中心平均解模糊器、Sugeno型模糊邏輯推理系統(tǒng)和乘積推理機(jī)，模糊系統(tǒng)的輸入為系統(tǒng)的滑模面S，模糊系統(tǒng)的輸出為替換模糊增益G^(S)。根據(jù)自適應(yīng)模糊控制器的設(shè)計(jì)思想，模糊規(guī)則設(shè)計(jì)為：

模糊系統(tǒng)的輸入隸屬度函數(shù)選擇高斯函數(shù)，并且其參數(shù)是預(yù)先確定的[10]，則輸入隸屬度函數(shù)圖，如圖2所示。其隸屬函數(shù)可表示為：

圖2 輸入隸屬度函數(shù)圖Fig.2 Input Membership Function

模糊系統(tǒng)的輸出隸屬度函數(shù)為單值型，模糊系統(tǒng)的輸出(si)可表示為：

自適應(yīng)模糊滑模控制器穩(wěn)定性可通過Lyapunov函數(shù)證明，選取總的Lyapunov 函數(shù)為：L=L1+L2。AGV 運(yùn)動(dòng)學(xué)控制器設(shè)計(jì)中Lyapunov函數(shù)L1已得到證明，現(xiàn)在只需要分析Lyapunov函數(shù)L2：

4 仿真分析

仿真試驗(yàn)以圖1 所示的差速驅(qū)動(dòng)AGV 為研究對象，利用MATLAB/Simulink 建立AGV 軌跡跟蹤控制器的仿真模型，驗(yàn)證所設(shè)計(jì)的控制算法的有效性。AGV 軌跡跟蹤控制器的結(jié)構(gòu)框架，如圖3 所示。AGV 系統(tǒng)參數(shù)為：m=60kg，JG=24kg·m2，r=0.12m，b=0.25m，l=0.25m；控制器參數(shù)為：kɑ=kb=1，γ1=γ2=0.05，c=[-1.5，-1.0，-0.5，0，0.5，1.0，1.5 ]，λ1=λ2=1。仿真過程中，在仿真時(shí)間為15s時(shí)，AGV系統(tǒng)受到外力沖擊（一個(gè)矩形脈沖信號(hào)，幅值為5，時(shí)間寬度為0.5s）。

圖3 AGV軌跡跟蹤控制器結(jié)構(gòu)框圖Fig.3 Block Diagram of AGV Trajectory Tracking Controller

4.1 直線軌跡

選取直線軌跡為參考軌跡，其軌跡方程為：xr(t)=t，yr(t)=t，θr(t)=π/4；參考速度為選取參考軌跡的初始位姿為：xr(0)=0，yr(0)=0，θr(0)=π/4；受控AGV的實(shí)際初始位姿為：x(0)=3，y(0)=-1，θ(0)=π。試驗(yàn)仿真結(jié)果，如圖4所示；在未加入外力沖擊前，AGV在初始位姿穩(wěn)定調(diào)節(jié)過程中向前移動(dòng)了約5.6m；當(dāng)AGV進(jìn)入穩(wěn)定狀態(tài)后，初始位姿誤差逐漸收斂到零，實(shí)際速度收斂到參考速度，電機(jī)控制輸入力矩平滑地收斂到穩(wěn)定狀態(tài)。當(dāng)AGV受到外力沖擊時(shí)，AGV系統(tǒng)實(shí)際運(yùn)行軌跡會(huì)在參考軌跡附近波動(dòng)，大約4.8s后再次實(shí)現(xiàn)對參考軌跡的跟蹤；其位姿跟蹤誤差、實(shí)際運(yùn)行速度和電機(jī)輸出力矩均能夠較快的恢復(fù)到原來的穩(wěn)定狀態(tài)。

圖4 直線軌跡仿真曲線Fig.4 Linear Trajectory Simulation Curve

4.2 圓弧軌跡

選取圓弧軌跡為參考軌跡，其軌跡方程為：xr(t)=-sin(t)，yr(t)=cos(t)，θr(t)=t；參考速度為：vr=1m/s，ωr=1rad/s。選取參考軌跡的初始位姿為：xr(0)=0，yr(0)=1，θr(0)=π；受控AGV的實(shí)際初始位姿為：x(0)=0，y(0)=0，θ(0)=π/4。

試驗(yàn)仿真結(jié)果，如圖5所示。在未加入外力沖擊前，AGV系統(tǒng)大約在5.4s后進(jìn)入穩(wěn)定狀態(tài)，沿著期望軌跡運(yùn)行，位姿誤差收斂到零，電機(jī)控制輸入力矩平滑地收斂到穩(wěn)定狀態(tài)；當(dāng)AGV受到外力沖擊時(shí)，AGV會(huì)偏離參考軌跡行駛，位姿誤差瞬間增大，大約4.5s后系統(tǒng)達(dá)到穩(wěn)定狀態(tài)；其位姿跟蹤誤差、實(shí)際運(yùn)行速度和電機(jī)輸出力矩均能夠較快的恢復(fù)到原來的穩(wěn)定狀態(tài)。

圖5 圓弧軌跡仿真曲線Fig.5 Arc Trajectory Simulation Curve

5 結(jié)論

針對四輪式差速驅(qū)動(dòng)AGV 的軌跡跟蹤問題，首先建立了AGV 的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型；其次考慮到其動(dòng)力學(xué)模型中存在系統(tǒng)參數(shù)攝動(dòng)和外部干擾等情況，建立了基于Backstepping技術(shù)的運(yùn)動(dòng)學(xué)控制律和基于滑?？刂萍夹g(shù)的動(dòng)力學(xué)控制律；雖然滑模控制器能有效的克服系統(tǒng)參數(shù)攝動(dòng)和外部干擾，但會(huì)出現(xiàn)抖振現(xiàn)象；利用模糊系統(tǒng)的萬能逼近特性，用模糊控制增益來逼近滑?？刂浦械牟贿B續(xù)切換部分，實(shí)現(xiàn)自適應(yīng)調(diào)節(jié)；最后用Lyapunov穩(wěn)定性理論證明了所設(shè)計(jì)的軌跡跟蹤控制律的穩(wěn)定性和跟蹤誤差的漸進(jìn)收斂性。仿真試驗(yàn)結(jié)果表明，AGV系統(tǒng)的Backstepping運(yùn)動(dòng)學(xué)控制器和自適應(yīng)模糊滑模動(dòng)力學(xué)控制器的混合控制器能夠有效的跟蹤給定的參考軌跡，并且對系統(tǒng)中存在的參數(shù)攝動(dòng)和外部干擾具有較強(qiáng)魯棒性，同時(shí)可以消除滑?？刂频妮斎攵墩瘛?/p>