舒靜青,時(shí) 偉,汪宇陽(yáng)

(中南大學(xué)航空航天學(xué)院,湖南 長(zhǎng)沙 410083)

1 引言

近年來(lái),地震等自然災(zāi)害給人類(lèi)的生命和財(cái)產(chǎn)帶來(lái)極大的損失[1],災(zāi)后快速響應(yīng)和救援工作的及時(shí)開(kāi)展對(duì)保障人民生命財(cái)產(chǎn)安全至關(guān)重要。救援自動(dòng)導(dǎo)引車(chē)(Automated Guided Vehicle,AGV)是一種災(zāi)后廢墟救援機(jī)器人,能夠在復(fù)雜、危險(xiǎn)的救援任務(wù)中能夠承擔(dān)物資運(yùn)輸和轉(zhuǎn)移、人員搜尋和營(yíng)救等重要工作[2]。

震后環(huán)境復(fù)雜,救援AGV小車(chē)在救援現(xiàn)場(chǎng)環(huán)境中按規(guī)劃路徑行駛時(shí),可能受到未知外界因素干擾,使之偏離指定路徑,對(duì)于救援AGV小車(chē)軌跡跟蹤的研究,能讓小車(chē)在行駛中對(duì)沿規(guī)劃路徑運(yùn)動(dòng)時(shí)產(chǎn)生的偏差進(jìn)行糾正,從而實(shí)現(xiàn)對(duì)給定路線(xiàn)的逼近[3],是AGV小車(chē)自主移動(dòng)的關(guān)鍵環(huán)節(jié)。由于車(chē)輛系統(tǒng)具有非完整約束條件,是強(qiáng)非線(xiàn)性、高度耦合的復(fù)雜系統(tǒng),很難建立精確的車(chē)輛動(dòng)力學(xué)系統(tǒng)模型[4],無(wú)人駕駛車(chē)輛的軌跡跟蹤控制始終是一個(gè)難點(diǎn)。目前傳統(tǒng)的控制方法在面對(duì)多參數(shù)的非線(xiàn)性系統(tǒng)被控對(duì)象時(shí),已經(jīng)表現(xiàn)出劣勢(shì),需要用其它智能控制算法進(jìn)行優(yōu)化,如模糊控制、模型預(yù)測(cè)算法等無(wú)需建立精確模型的控制算法。

模糊控制算法適用于大部分非線(xiàn)性模型[5],與其它控制算法結(jié)合,能有效解決控制中的非線(xiàn)性、強(qiáng)耦合等問(wèn)題,提高控制系統(tǒng)的魯棒性。Wang等人[6]將模糊控制用于PI控制器的參數(shù)自整定,以控制機(jī)器人的伺服電機(jī),發(fā)現(xiàn)模糊控制的加入能有效提升軌跡的跟蹤效果。M. Septyan等人[7]提出了一種非線(xiàn)性動(dòng)力學(xué)的模糊控制器,旨在解決跟蹤具有初始誤差的非光滑軌跡誤差問(wèn)題,仿真結(jié)果表明該控制器能夠精確跟蹤非光滑參考軌跡,且能有效消除初始位置誤差引起的大姿態(tài)變化。Z.Sun[8]將模糊控制與滑膜控制相結(jié)合,設(shè)計(jì)了基于麥克納姆輪AGV車(chē)輛模型的聯(lián)合控制器,其控制性能比傳統(tǒng)滑模控制器跟蹤精度更高,平滑度和穩(wěn)定度更優(yōu)。

模型預(yù)測(cè)算法[9]能預(yù)測(cè)未來(lái)一段時(shí)域內(nèi)系統(tǒng)的輸出量,通過(guò)解決有約束的最優(yōu)控制問(wèn)題,不斷逼近參考值。Wang等人[10]基于簡(jiǎn)化的車(chē)輛非線(xiàn)性模型,將MPC控制器用于車(chē)輛的橫向控制,通過(guò)引入松弛變量來(lái)軟化輸出約束,避免了最優(yōu)解計(jì)算的不可行性,并提出了一種在預(yù)測(cè)視域內(nèi)獲取局部參考軌跡的新方法,大幅提升了對(duì)車(chē)輛不同速度時(shí)軌跡跟蹤的性能。Liu等人[11]將PI控制算法與MPC結(jié)合,推導(dǎo)出基于非線(xiàn)性模型預(yù)測(cè)控制比例積分自整定策略,利用預(yù)測(cè)控制的思想克服了AGV系統(tǒng)的復(fù)雜性和其它不確定的干擾,實(shí)現(xiàn)AGV對(duì)于給定軌跡的執(zhí)行。Chen等人[12]基于橫向縱向控制,將模型預(yù)測(cè)算法與PID聯(lián)合,對(duì)自動(dòng)駕駛車(chē)輛進(jìn)行軌跡跟蹤。在橫向控制中基于模型預(yù)測(cè)算法,采用8自由度預(yù)測(cè)模型,生成最優(yōu)車(chē)輪轉(zhuǎn)向角,同時(shí)采用PID控制器對(duì)縱向控制以驅(qū)動(dòng)或制動(dòng)車(chē)輛。仿真表明模型預(yù)測(cè)與PID聯(lián)合控制能得到精確的跟蹤路徑和準(zhǔn)確的速度控制。

本文針對(duì)救援AGV小車(chē)的軌跡跟蹤算法進(jìn)行了優(yōu)化設(shè)計(jì),將模糊控制與模型預(yù)測(cè)控制結(jié)合,以模糊控制為核心,用模型預(yù)測(cè)控制中的反饋矯正和滾動(dòng)優(yōu)化機(jī)理彌補(bǔ)模糊算法中控制精度不高、動(dòng)態(tài)品質(zhì)差等問(wèn)題,以達(dá)到對(duì)設(shè)定參考軌跡的無(wú)誤跟蹤?；贏GV運(yùn)動(dòng)學(xué)模型設(shè)計(jì)控制器,將單純的模糊控制器和模糊-預(yù)測(cè)控制兩種控制器情況進(jìn)行仿真對(duì)比研究。

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

震后環(huán)境以廢墟為主,是極不穩(wěn)固的建筑結(jié)構(gòu);救援任務(wù)的多樣性要求救援AGV有一定的負(fù)載能力,因此本文以平穩(wěn)性更優(yōu),負(fù)載能力更強(qiáng)的四輪差速AGV小車(chē)為研究對(duì)象。圖1為AGV小車(chē)的簡(jiǎn)化模型示意圖,其中,前輪為萬(wàn)向輪,用以方向控制,后輪為驅(qū)動(dòng)輪,用以提供車(chē)輛前進(jìn)的驅(qū)動(dòng)力。圖2為AGV運(yùn)動(dòng)學(xué)模型,AGV的位姿可表示為P=[x,y,θ]T,則期望位姿表示為Pr=[xr,yr,θr]T,位姿誤差為Pe=[xe,ye,θe]T;小車(chē)的線(xiàn)速度和角速度為v和ω。

圖1 四輪差速AGV小車(chē)

圖2 AGV運(yùn)動(dòng)學(xué)模型(M為軸中點(diǎn),θ為導(dǎo)航角)

根據(jù)圖2,小車(chē)的運(yùn)動(dòng)學(xué)方程[13]為

(1)

期望軌跡設(shè)定為qr=[xr,yr,θr]T,運(yùn)動(dòng)控制量設(shè)定為[vr,ωr],AGV小車(chē)模型滿(mǎn)足非完整移動(dòng)機(jī)器人非完整約束條件[14],即

(2)

小車(chē)的誤差方程為

(3)

對(duì)其誤差方程求導(dǎo)可得位姿誤差微分方程

(4)

3 軌跡跟蹤的控制器設(shè)計(jì)

3.1 模糊控制器設(shè)計(jì)

3.1.1 控制率

由小車(chē)的位姿誤差方程(4)和Lyapunov穩(wěn)定性條件[15],設(shè)定小車(chē)的控制量為

(5)

其中,k1,k2,k3為系統(tǒng)參數(shù),由式(5)可知,k1控制AGV的運(yùn)動(dòng)速度,k2和k3聯(lián)合控制AGV的角速度。要滿(mǎn)足AGV小車(chē)軌跡跟蹤目的,實(shí)現(xiàn)Lyapunov穩(wěn)定性條件,需三個(gè)參數(shù)均大于零。

AGV小車(chē)是非線(xiàn)性的復(fù)雜系統(tǒng),使用模糊控制對(duì)控制率進(jìn)行設(shè)計(jì)能夠解決多個(gè)系統(tǒng)參數(shù)的不確定性,對(duì)參數(shù)實(shí)時(shí)進(jìn)行調(diào)整,提升系統(tǒng)的穩(wěn)定性和魯棒性。模糊控制器原理圖如圖3所示,以誤差e作為輸入變量,誤差e包含位置誤差ed和角度誤差ea;由模糊推理得到的控制量u為k1,k2,k3三個(gè)參數(shù)。

圖3 模糊控制原理圖

3.1.2 模糊化規(guī)則

模糊規(guī)則[16]包含兩個(gè)重點(diǎn):

1)模糊詞集:將輸入輸出變量的狀態(tài)以模糊語(yǔ)言描述,確定模糊子集,實(shí)際應(yīng)用中通常取{負(fù)大,負(fù)中,負(fù)小,零,正小,正中,正大}7個(gè)模糊狀態(tài)。

2)量化因子:記論域?yàn)镹=[-n,-n+1,-L,0,n-1,n],通常n的取值為6,論域值為X=[-a,a],則量化因子ke表達(dá)式為

ke=n/a

(6)

由于輸入變量和輸出變量模糊語(yǔ)言和論域均相同,因此模糊隸屬度函數(shù)也相同,如圖4。

圖4 模糊隸屬度函數(shù)

3.1.3 建立模糊規(guī)則庫(kù)

模糊控制器的規(guī)則語(yǔ)句[17]為:

If ed=A And ea=B Then k1=C,

If ed=A And ea=B Then k2=C,

If ed=A And ea=B Then k3=C．

建立模糊規(guī)則表如下:

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

表2 k2,k3模糊控制規(guī)則表

3.1.4 解模糊化

解模糊化是將模糊集合中的模糊值轉(zhuǎn)化為清晰值的過(guò)程。本文采用重心法[18]解模糊化。

若有n個(gè)輸出值,則求取清晰值的算法為

(7)

3.2 模型預(yù)測(cè)控制器設(shè)計(jì)

模型預(yù)測(cè)算法主要由三個(gè)部分組成:預(yù)測(cè)模型、滾動(dòng)優(yōu)化、反饋控制。模型預(yù)測(cè)控制器原理圖如圖5所示。

圖5 模型預(yù)測(cè)控制器原理圖

3.2.1 預(yù)測(cè)模型

被控對(duì)象AGV小車(chē)的運(yùn)動(dòng)學(xué)方程如式(1),是線(xiàn)性時(shí)變方程,而模型預(yù)測(cè)控制需在離散化模型下計(jì)算,因此先進(jìn)行離散化操作[19],得到離散化模型

(8)

由離散狀態(tài)方程,可以預(yù)測(cè)系統(tǒng)未來(lái)的狀態(tài),記

(9)

則得到

(10)

推導(dǎo)可得預(yù)測(cè)的系統(tǒng)狀態(tài)為

Y(t)=ψtξ(t|t)+ΘtΔU(t)

(11)

其中

3.2.2 滾動(dòng)優(yōu)化

模型預(yù)測(cè)控制算法在本質(zhì)上是最優(yōu)化控制問(wèn)題,要得到與參考軌跡差值最小的預(yù)測(cè)軌跡,可以通過(guò)優(yōu)化算法來(lái)優(yōu)化未來(lái)時(shí)間內(nèi)系統(tǒng)的控制量。首先設(shè)計(jì)目標(biāo)函數(shù)J(k),當(dāng)目標(biāo)函數(shù)最小時(shí),預(yù)測(cè)軌跡與參考軌跡之間的差值最小,AGV能夠精確且快速地對(duì)軌跡進(jìn)行跟蹤。參考文獻(xiàn)[20]對(duì)目標(biāo)函數(shù)的設(shè)計(jì),設(shè)定本文的目標(biāo)函數(shù)形式如下

(12)

其中,Q為誤差權(quán)重矩陣,R為控制權(quán)重矩陣;Np為預(yù)測(cè)時(shí)域,Nc為控制時(shí)域,ρ為權(quán)重系數(shù),ε為松弛因子。

控制量及控制增量的約束條件設(shè)定如下

將式(9)帶入式(11),每一周期都能得到控制時(shí)域內(nèi)的控制增量:Δu(k),……,Δu(k+Nc-1),將該控制增量中的第一項(xiàng)Δu(k)作用于系統(tǒng),下一周期重復(fù)該過(guò)程得到新的控制增量,循環(huán)往復(fù),逼近參考軌跡,最終實(shí)現(xiàn)對(duì)軌跡的跟蹤。

3.3 模糊預(yù)測(cè)控制器設(shè)計(jì)

AGV小車(chē)的模糊預(yù)測(cè)控制系統(tǒng)是利用模型預(yù)測(cè)控制的反復(fù)預(yù)測(cè)優(yōu)化求解,以彌補(bǔ)模糊控制中存在的精度問(wèn)題。模糊預(yù)測(cè)控制器的原理圖如圖6所示。

圖6 模糊預(yù)測(cè)控制器的原理圖

由圖6可知,模糊預(yù)測(cè)控制分為模糊控制部分和模型預(yù)測(cè)反饋部分,輸入期望位姿qr,模糊控制部分,以輸出u=[v,ω],實(shí)時(shí)控制AGV小車(chē)的線(xiàn)速度和角速度,實(shí)時(shí)調(diào)整小車(chē)的運(yùn)動(dòng)軌跡和角度;反饋部分加入模型預(yù)測(cè)控制,根據(jù)每一時(shí)刻的預(yù)測(cè)值,調(diào)整小車(chē)對(duì)當(dāng)前時(shí)刻的控制輸出量,以減小跟蹤中的位姿誤差。

4 仿真分析

4.1 AGV跟蹤圓軌跡

設(shè)定AGV以初始速度vr=1.0m/s,初始角速度ωr=1.0rad/s,初始位姿p=[0,0,0]T,初始位姿誤差pe=[3,4,4.2]T,跟蹤圓形軌跡。分別采用模糊-預(yù)測(cè)控制和模糊控制對(duì)軌跡進(jìn)行跟蹤控制。

圖7為兩種控制方式分別對(duì)參考圓軌跡進(jìn)行跟蹤,圖8為橫向位置誤差,圖9為縱向位置誤差,圖10為導(dǎo)航角度誤差。由圖7可知,在存在初始位姿誤差前提下,采用模糊-預(yù)測(cè)控制的紅色軌跡線(xiàn),比僅采用模糊控制的綠色軌跡線(xiàn),跟蹤波動(dòng)幅度更小,跟蹤速度更快。為清晰評(píng)價(jià)兩種控制器的跟蹤效果,作橫向、縱向位置誤差以及角度誤差如圖8-圖10所示。

圖7 AGV跟蹤圓軌跡圖

圖8 橫向位置誤差

圖9 縱向位置誤差

圖10 角度誤差

由圖8可知,在初始橫向偏差設(shè)定為3m的前提下,對(duì)比模糊控制,模糊-預(yù)測(cè)控制跟蹤橫向位置波動(dòng)峰值下降6%。圖9為縱向位置誤差,在初始縱向偏差設(shè)定為4m的前提下,模糊-預(yù)測(cè)控制跟蹤縱向位置波動(dòng)峰值下降47%,且誤差能夠在20～22s收斂至0,而模糊控制在32～36s收斂,提高了6%～12%跟蹤速度。由此可見(jiàn),模糊-預(yù)測(cè)控制在穩(wěn)定性和快速性方面體現(xiàn)出更好的糾偏能力。

圖10代表控制器對(duì)方向的跟蹤能力,初始角度偏差設(shè)定為4.2°,可以發(fā)現(xiàn)模糊控制器的角度誤差在出現(xiàn)較大幅度波動(dòng)后,無(wú)法充分收斂至無(wú)誤,一直以較大的角度偏差跟蹤設(shè)定軌跡,而模糊-預(yù)測(cè)控制器在21～22s時(shí)角度誤差穩(wěn)定收斂至0,具有更佳的轉(zhuǎn)向控制能力。

4.2 AGV跟蹤曲線(xiàn)軌跡

設(shè)定AGV以初始速度vr=1.0m/s,初始角速度ωr=1.0rad/s,初始位姿p=[0,0,0]T,初始位姿誤差pe=[-4,-1.2,-1]T,跟蹤曲線(xiàn)軌跡。分別采用模糊-預(yù)測(cè)控制和模糊控制對(duì)軌跡進(jìn)行跟蹤控制。

由圖11可知,模糊-預(yù)測(cè)控制能夠?qū)θ我馇€(xiàn)軌跡進(jìn)行跟蹤,且對(duì)比僅采用模糊控制的綠色軌跡線(xiàn),軌跡貼合性更好,跟蹤速度更快。為清晰評(píng)價(jià)兩種控制器的跟蹤效果,作橫向、縱向位置誤差以及角度誤差如圖12-14所示。

圖11 AGV跟蹤曲線(xiàn)軌跡圖

圖12 橫向位置誤差

圖13 縱向位置誤差

由圖12-13可知,在初始位置誤差(-4,-1.2)/m的前提下,對(duì)比模糊控制,模糊-預(yù)測(cè)控制跟蹤橫坐標(biāo)位置誤差波動(dòng)峰值下降63.4%,收斂速度提高5s;跟蹤縱坐標(biāo)位置誤差波動(dòng)峰值均下降28.6%,收斂速度提高8s。

由圖14可知,在初始角度偏差設(shè)定為-1°時(shí),模糊控制下的AGV角度誤差一直增大,并將持續(xù)以較大角度偏差跟蹤設(shè)定軌跡,后期會(huì)出現(xiàn)偏離預(yù)期軌跡的問(wèn)題,但模糊-預(yù)測(cè)控制器在20s左右角度誤差收斂至0,進(jìn)一步驗(yàn)證了在初始位姿偏差較大的情況下,模糊-預(yù)測(cè)控制的轉(zhuǎn)向能力更佳。

圖14 角度誤差

從仿真結(jié)果看,雖然兩種控制器在一定仿真時(shí)間之后,都能正常跟蹤參考軌跡,但在復(fù)雜軌跡條件和初始位姿誤差較大情況下,對(duì)比單純使用模糊控制,采用模糊-預(yù)測(cè)對(duì)軌跡進(jìn)行跟蹤控制,能夠在快速性和穩(wěn)定性方面得到進(jìn)一步優(yōu)化。

5 結(jié)論

本文針對(duì)震后復(fù)雜環(huán)境下AGV小車(chē)軌跡跟蹤控制的問(wèn)題,分別建立了模糊和模型預(yù)測(cè)控制器,考慮到模糊控制在控制精度和動(dòng)態(tài)優(yōu)化方面的不足,將模型預(yù)測(cè)控制加入模糊控制的反饋中,結(jié)合兩種控制算法的優(yōu)點(diǎn),設(shè)計(jì)了模糊-預(yù)測(cè)控制器。最后,通過(guò)仿真驗(yàn)證發(fā)現(xiàn),模糊-預(yù)測(cè)控制能有效控制位置跟蹤波動(dòng)幅度,降低位置誤差峰值,提升控制精度,同時(shí)減小誤差收斂時(shí)間,提高動(dòng)態(tài)跟蹤效率,證明了模糊-預(yù)測(cè)控制算法在AGV軌跡跟蹤方面的優(yōu)越性。