基于模糊預(yù)測控制的救援AGV軌跡跟蹤研究

舒靜青,時 偉,汪宇陽

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

1 引言

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

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

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

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

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

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

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

圖1 四輪差速AGV小車

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

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

(1)

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

(2)

小車的誤差方程為

(3)

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

(4)

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

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

3.1.1 控制率

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

(5)

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

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

圖3 模糊控制原理圖

3.1.2 模糊化規(guī)則

模糊規(guī)則[16]包含兩個重點:

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

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

ke=n/a

(6)

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

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

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

模糊控制器的規(guī)則語句[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)化為清晰值的過程。本文采用重心法[18]解模糊化。

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

(7)

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

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

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

3.2.1 預(yù)測模型

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

(8)

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

(9)

則得到

(10)

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

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

(11)

其中

3.2.2 滾動優(yōu)化

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

(12)

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

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

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

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

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

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

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

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ù)測控制和模糊控制對軌跡進(jìn)行跟蹤控制。

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

圖7 AGV跟蹤圓軌跡圖

圖8 橫向位置誤差

圖9 縱向位置誤差

圖10 角度誤差

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

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

4.2 AGV跟蹤曲線軌跡

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

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

圖11 AGV跟蹤曲線軌跡圖

圖12 橫向位置誤差

圖13 縱向位置誤差

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

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

圖14 角度誤差

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

5 結(jié)論

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