基于運(yùn)動(dòng)預(yù)測(cè)的多步控制多步優(yōu)化模型

基于運(yùn)動(dòng)預(yù)測(cè)的多步控制多步優(yōu)化模型

1小角度偏差跟蹤控制策略自動(dòng)駕駛（agv）是一種廣泛應(yīng)用于各種行業(yè)的自動(dòng)移動(dòng)機(jī)器。路徑跟蹤是實(shí)現(xiàn)獨(dú)立移動(dòng)的關(guān)鍵，而導(dǎo)航模式提供了路徑跟蹤的指導(dǎo)信息。近年來，基于對(duì)標(biāo)準(zhǔn)圖像的識(shí)別，視覺導(dǎo)航業(yè)務(wù)的靈活性強(qiáng)，信息識(shí)別速度快，導(dǎo)航信息能力強(qiáng)，已成為agv導(dǎo)航的主要發(fā)展方向。該導(dǎo)航通常通過車載攝像機(jī)獲取本地圖像中的索引信息，即agv和輸入線之間的距離和角度之間的差異。因此，算法難以應(yīng)用于全球運(yùn)動(dòng)模型。文獻(xiàn)通過機(jī)器視覺獲得距離和角度偏差,利用線性二次型調(diào)節(jié)器(LQR)理論,求出預(yù)定速度下的固定反饋增益來實(shí)現(xiàn)路徑跟蹤.對(duì)模型的線性化處理將該方法限制于小角度偏差情況.文獻(xiàn)在小角度偏差時(shí)采用基于LQR理論的最優(yōu)控制以獲得較高的跟蹤精度,在大角度偏差時(shí)使用模糊控制以快速減小路徑偏差.LQR優(yōu)化理論在小偏差時(shí)獲得了良好的跟蹤性能,但也存在以下難題:1)目標(biāo)函數(shù)中加權(quán)矩陣Q與R的選擇;2)代數(shù)Raccati方程求解的復(fù)雜度;3)優(yōu)化過程缺乏考慮速度約束.對(duì)于加權(quán)矩陣Q與R的選取,目前普遍使用的是仿真試湊法.由于兩者之間只存在相對(duì)大小關(guān)系,仿真時(shí)一般設(shè)定R為單位陣,然后根據(jù)結(jié)果試湊Q,很多學(xué)者對(duì)此做了大量研究.文獻(xiàn)認(rèn)為Q的取值不宜太大,否則控制器輸出趨向飽和,系統(tǒng)極點(diǎn)遠(yuǎn)離虛軸,造成系統(tǒng)抗干擾能力下降,對(duì)噪聲敏感.認(rèn)為Q的取值較大,R的取值較小時(shí),系統(tǒng)的魯棒性較好.認(rèn)為距離偏差的q值不宜太大,如果迫使系統(tǒng)過分地追求快速消除距離偏差,結(jié)果會(huì)造成角度偏差過大,系統(tǒng)超調(diào)嚴(yán)重,且前輪轉(zhuǎn)角超過轉(zhuǎn)向機(jī)構(gòu)的物理極限.預(yù)測(cè)控制通過運(yùn)動(dòng)學(xué)模型估計(jì)機(jī)器人的未來位姿,能有效消除系統(tǒng)時(shí)延,并從全局角度考慮控制的長期優(yōu)化.文獻(xiàn)通過預(yù)測(cè)機(jī)器人位姿以修正視覺反饋信息,補(bǔ)償圖像處理中時(shí)延導(dǎo)致的偏差改變量,取得了較好效果.采用擴(kuò)展Kalman濾波的信息融合,進(jìn)行路徑跟蹤的廣義預(yù)測(cè)控制.在每個(gè)控制周期先預(yù)測(cè)N步,求出最優(yōu)控制序列,但只取該序列的第1項(xiàng)作為本周期的輸出量,并進(jìn)行滾動(dòng)優(yōu)化.然而該方法計(jì)算量較大,實(shí)時(shí)滾動(dòng)優(yōu)化較為困難.在全局坐標(biāo)下,采用運(yùn)動(dòng)學(xué)模型進(jìn)行單步和多步預(yù)測(cè)控制,并根據(jù)給定的加權(quán)矩陣和速度優(yōu)化控制量.該方法的計(jì)算也較復(fù)雜,且沒有研究最優(yōu)控制步數(shù)的確定方法.本文提出一種基于運(yùn)動(dòng)預(yù)測(cè)的LQR最優(yōu)控制方法.根據(jù)局部坐標(biāo)系中的運(yùn)動(dòng)學(xué)模型,預(yù)測(cè)機(jī)器人的未來位姿,并在速度約束下,尋求一個(gè)協(xié)調(diào)性最優(yōu)的控制序列,通過物理可實(shí)現(xiàn)的多步控制,將角度偏差和距離偏差同時(shí)消除到零.在目標(biāo)函數(shù)中只包含速度控制量,解決了加權(quán)矩陣選擇的難題,算法的快速性由控制步數(shù)的最小化來保證.而且,對(duì)該算法通過理論推導(dǎo)獲得了速度控制量的解析表達(dá)式,避免了Raccati方程的迭代求解,計(jì)算量很小,可滿足嵌入式控制系統(tǒng)實(shí)時(shí)滾動(dòng)優(yōu)化的需求.2agv位姿的運(yùn)動(dòng)學(xué)模型本文的研究平臺(tái)采用自主研發(fā)的差速式視覺導(dǎo)航AGV,如圖1所示.中間兩驅(qū)動(dòng)輪分別由一臺(tái)直流伺服電機(jī)驅(qū)動(dòng),前后兩輪為僅起支撐作用的萬向隨動(dòng)輪.該AGV具有直線和圓弧兩種運(yùn)動(dòng)軌跡.視覺傳感器為單目彩色CCD攝像機(jī),安裝在AGV中心線的前方位置,獲取局部視野中AGV與標(biāo)示線的距離偏差ed和角度偏差eθ.xoy是跟隨AGV運(yùn)動(dòng)的局部視覺坐標(biāo)系,標(biāo)示線與x軸交點(diǎn)的橫坐標(biāo)為ed,y軸到標(biāo)示線切線方向的夾角為eθ,該夾角逆時(shí)針為正,順時(shí)針為負(fù).左、右驅(qū)動(dòng)輪的線速度為vl和vr,兩驅(qū)動(dòng)輪間距為W,AGV中心線速度為v,角速度為ω.路徑跟蹤的實(shí)質(zhì)就是消除這兩種偏差.當(dāng)出現(xiàn)路徑偏差時(shí),在兩驅(qū)動(dòng)輪之間產(chǎn)生一個(gè)速度差Δv,則AGV沿圓弧軌跡調(diào)整其位姿,兩驅(qū)動(dòng)輪速度為vl=v+Δv,vr=v-Δv.(1)vl=v+Δv,vr=v?Δv.(1)位姿調(diào)整的角速度為ω=(vl-vr)W=2ΔvW.(2)ω=(vl?vr)W=2ΔvW.(2)從狀態(tài)k經(jīng)過時(shí)間Ts到狀態(tài)k+1時(shí),小車?yán)@瞬心A轉(zhuǎn)過的角度為ωTs,狀態(tài)k+1的角度偏差為eθ(k+1)=eθ(k)+2Δv(k)ΤsW.(3)eθ(k+1)=eθ(k)+2Δv(k)TsW.(3)AGV是具有非完整約束的不可積系統(tǒng),其距離偏差的變化采用幾何方法分析獲得.假設(shè)狀態(tài)k時(shí)的路徑偏差如圖2所示,在速度控制量Δv的作用下,AGV繞圓心C沿圓弧oA行駛以減小偏差,在狀態(tài)k+1時(shí)達(dá)到A點(diǎn).其中,oF為狀態(tài)k的距離偏差ed(k),AE為k+1時(shí)的距離偏差ed(k+1).當(dāng)時(shí)間Ts很短且角速度ω為有限值時(shí),圓弧oA近似為直線段,長度為AGV的行程,圓心角θ為角度偏差的變化量.則k+1時(shí)的距離偏差為ed(k+1)=ed(k)+vΤscosθ2?taneθ-vΤssinθ2.(4)ed(k+1)=ed(k)+vTscosθ2?taneθ?vTssinθ2.(4)當(dāng)eθ和θ很小時(shí),將式(4)線性化處理得ed(k+1)=ed(k)-veθ(k)Τs-vΔv(k)Τ2sW.(5)ed(k+1)=ed(k)?veθ(k)Ts?vΔv(k)T2sW.(5)小角度偏差條件下的線性運(yùn)動(dòng)學(xué)模型為{eθ(k+1)=eθ(k)+2ΤsWΔv(k),ed(k+1)=ed(k)-veθ(k)Τs-vΤ2sWΔv(k).(6)?????????eθ(k+1)=eθ(k)+2TsWΔv(k),ed(k+1)=ed(k)?veθ(k)Ts?vT2sWΔv(k).(6)3基于運(yùn)動(dòng)預(yù)測(cè)的lqr控制最佳3.1基于運(yùn)動(dòng)學(xué)模型的多步預(yù)測(cè)在小偏差情況下,文獻(xiàn)采用LQR優(yōu)化方法,取狀態(tài)變量X與控制變量u的二次型函數(shù)的積分作為目標(biāo)函數(shù),即J=12∫∞0[XΤQX+uΤRu]dt.(7)J=12∫∞0[XTQX+uTRu]dt.(7)本文采用一種新的思路考慮Q與R的選取問題.在式(7)中,第1項(xiàng)反映了路徑偏差消除的快速性,Q越大,目標(biāo)函數(shù)要求偏差消除越快;第2項(xiàng)反映了路徑偏差消除的協(xié)調(diào)性,其值越小,系統(tǒng)為糾正超調(diào)所做的往返控制越少.然而,由于系統(tǒng)物理結(jié)構(gòu)的限制,消除偏差所需的速度控制量(兩輪速度差)Δv在一個(gè)有限范圍內(nèi),即|Δv|≤vmax.(8)|Δv|≤vmax.(8)在速度約束下,當(dāng)前狀態(tài)的路徑偏差可能無法在一個(gè)控制周期完全消除,因此將基于運(yùn)動(dòng)學(xué)模型的多步預(yù)測(cè)引入最優(yōu)控制.從全局角度出發(fā),以糾偏協(xié)調(diào)性的優(yōu)化為目標(biāo),通過一個(gè)N步最優(yōu)控制序列Δv(k)(k=1,2,…,N)將兩種路徑偏差同步地消除到零,而控制步數(shù)N實(shí)質(zhì)上反映了偏差消除的快速性.在條件(8)下,N的最小化可實(shí)現(xiàn)滿足速度約束的最快偏差消除過程.因此,它可代替式(7)中第1項(xiàng)對(duì)偏差消除快速性的要求,既避免了Q與R相對(duì)大小的選取,又考慮了實(shí)際系統(tǒng)的速度約束.目標(biāo)函數(shù)(7)可簡化為只包含控制變量u的形式.由于Q和R之間只存在相對(duì)大小關(guān)系,一般設(shè)定R為單位陣.本文算法實(shí)現(xiàn)的物理平臺(tái)是嵌入式控制系統(tǒng),則目標(biāo)函數(shù)的離散形式為J=12Ν-1∑k=0Δv2(k).(9)J=12∑k=0N?1Δv2(k).(9)將本文的路徑跟蹤問題按照LQR優(yōu)化方法進(jìn)行描述:對(duì)于狀態(tài)方程為(6)的系統(tǒng),在條件(8)的約束下,通過最小化目標(biāo)函數(shù)(9)和控制步數(shù)N,求解一個(gè)N步最優(yōu)控制序列Δv(k)(k=1,2,…,N),使兩種路徑偏差同步、快速和平穩(wěn)地消除到零.3.2速度控制量的解析法根據(jù)Lagrange待定數(shù)列法,對(duì)狀態(tài)方程(6)引入待定數(shù)列,即Hamilton函數(shù)為Η(k)=12Δv2(k)+λΤ(k+1)×{[e0(k)ed(k)-ve0(k)Τs〗+[2Τs/W-vΤ2s/W〗Δv(k)}.(11)能使目標(biāo)函數(shù)(9)取得極小值的最優(yōu)控制序列Δv(k)滿足以下條件:λ(k)=?Η(k)?X(k),(12)?Η(k)?u(k)=0.(13)根據(jù)矩陣分析,由式(12)得λ1(k)=λ1(k+1)=c(常數(shù))?(14)λ2(k+1)=λ2(k)+cvΤs.(15)令λ2(0)=λ2,對(duì)式(15)進(jìn)行迭代變換得λ2(k)=λ2+kcvΤs.(16)根據(jù)式(13)得Δv(k)+[λ1(k+1)λ2(k+1)][2ΤsW-vΤ2sW〗=0.(17)將式(14)和(16)代入(17)得Δv(k)=ΤsW{λ2vΤs+[(k+1)(vΤs)2-2]c}.(18)將式(18)代入運(yùn)動(dòng)學(xué)模型(6),通過迭代變換得eθ(k)=eθ(0)+2(Τs/W)2{kλ2vΤs+[k(k+1)2(vΤs)2-2k]c},(19)ed(k)=ed(0)-kvΤse0(0)-(Τ/W)2vΤs{k2λ2vΤs+{[k-1∑i=1i(i+1)+k(k+1)/2](vΤs)2-2k2}c}.(20)根據(jù)控制目標(biāo),系統(tǒng)經(jīng)過N步最優(yōu)控制,兩種路徑偏差同時(shí)消除到零,即{eθ(Ν)=0,ed(Ν)=0.(21)將式(19)和(20)代入方程組(21),求出線性無關(guān)的待定常數(shù)c和λ2,再代入式(18)求出速度控制量的解析表達(dá)形式,即Δv(k)=-eθ(0)2ΝΤs/W+3[(k+1)-Ν+12]×[ΝvΤseθ(0)-2ed(0)(Ν3-Ν)ΤsWvΤs]].(22)在式(22)中,總控制步數(shù)N≥2,當(dāng)前控制步數(shù)k=0,1,…,N-1.3.3基于流變學(xué)模型的速度控制文獻(xiàn)尋求最優(yōu)控制量時(shí),都是在給定速度v下,根據(jù)仿真試湊的加權(quán)矩陣Q與R,求得固定不變的最優(yōu)反饋增益矩陣.當(dāng)小車速度改變時(shí),原先得到的加權(quán)矩陣和反饋增益矩陣未必是最優(yōu)的,最優(yōu)控制的優(yōu)化效果和魯棒性能較差.重新迭代求解Raccati方程獲取新的反饋增益矩陣,也因?yàn)橛?jì)算量大而難以在線實(shí)時(shí)完成.本文采用獨(dú)特的控制目標(biāo)優(yōu)化方式,將偏差消除的快速性用速度約束下的控制步數(shù)最小化來反映,簡化了目標(biāo)函數(shù)形式,并根據(jù)線性運(yùn)動(dòng)學(xué)模型,經(jīng)過數(shù)學(xué)推導(dǎo)完成迭代變換,求得速度控制量的解析表達(dá)式,極大地簡化了控制過程.在式(22)中,速度控制量序列Δv(k)與以下因素相關(guān):1)結(jié)構(gòu)參數(shù)W;2)控制周期Ts;3)路徑偏差eθ和ed;4)運(yùn)動(dòng)速度v;5)最優(yōu)控制總步數(shù)N和當(dāng)前控制步數(shù)k.一般地,系統(tǒng)的結(jié)構(gòu)參數(shù)W和控制周期Ts是固定不變的.在路徑偏差eθ和ed一定的條件下,小車速度v限制了速度控制量Δv(k)的取值范圍,從而決定了最優(yōu)控制總步數(shù)N.當(dāng)確定了最優(yōu)控制總步數(shù)N時(shí),每一步的控制量Δv(k)也隨k值線性變化.與以前文獻(xiàn)中的固定反饋增益矩陣相比,本文的速度控制量序列Δv(k)顯然考慮了更全面的因素,具有更好的優(yōu)化效果和魯棒性能.3.4考慮實(shí)際系統(tǒng)執(zhí)行能力的最小控制步數(shù)n最優(yōu)控制步數(shù)N反映了路徑偏差消除的快慢,但其取值受到條件(8)的制約.為了盡快消除兩種偏差并考慮實(shí)際系統(tǒng)的執(zhí)行能力,需要在條件(8)下最小化控制步數(shù)N.分析式(22)可知,速度控制量Δv(k)隨k值線性變化,則存在以下關(guān)系:|Δv(k)|max=|Δv(0)|or|Δv(N-1)|.(23)將式(23)代入條件(8),求出滿足條件的最小控制步數(shù)N.4高速工況下仿真為驗(yàn)證上述最優(yōu)控制模型,本文在不同速度v和控制量極值vmax下進(jìn)行大量數(shù)值仿真.針對(duì)線性運(yùn)動(dòng)學(xué)模型的小偏差假設(shè),本文將角度偏差的范圍定為|eθ(k)|≤5°.根據(jù)自主研發(fā)的AGV結(jié)構(gòu),系統(tǒng)參數(shù)設(shè)置如下:W=400mm,Ts=0.1s.在初始路徑偏差為eθ=3°,ed=-10mm時(shí),數(shù)值仿真結(jié)果如圖3所示.圖3(a)的速度與控制量極值為:v=200mm/s,vmax=0.25v.在糾偏前期,eθ減小卻無法阻止ed的增大,所以eθ減小到零后反向增大,加快ed的消除,最后兩種偏差同步減小到零.對(duì)于圖3(b),v=800mm/s,vmax=0.1v.兩種偏差糾正得更快,但偏差的超調(diào)也更大,ed反向增大的數(shù)值大于圖3(a)的相應(yīng)情形.可見,高速條件下的糾偏一定要及時(shí)和協(xié)調(diào),否則將產(chǎn)生更大的偏差,甚至路徑跟蹤失敗.5agv實(shí)際路徑糾偏為驗(yàn)證本文的最優(yōu)控制模型在AGV路徑跟蹤中的實(shí)際效果,作者采用嵌入式技術(shù),自主研發(fā)了差速式視覺導(dǎo)航AGV,并在實(shí)驗(yàn)室環(huán)境進(jìn)行了大量路徑跟蹤實(shí)驗(yàn),如圖4所示.車載控制器是基于ARM和DSP的多核嵌入式控制系統(tǒng).DSP通過圖像處理,從地面標(biāo)示線圖像中獲取兩種路徑偏差.ARM在嵌入式實(shí)時(shí)操作系統(tǒng)uC/OS-II的支持下,實(shí)現(xiàn)本文提出的最優(yōu)控制算法.在操作系統(tǒng)的管理下,路徑跟蹤任務(wù)與串口通信任務(wù)、小車行走任務(wù)和電機(jī)控制任務(wù)等共同完成路徑偏差的接收、速度控制量的計(jì)算和控制電壓的轉(zhuǎn)化等,控制伺服驅(qū)動(dòng)器完成直流電機(jī)調(diào)速.上位機(jī)監(jiān)控軟件以10Hz的頻率,通過無線方式實(shí)時(shí)采集AGV的路徑偏差并存入數(shù)據(jù)庫.根據(jù)該數(shù)據(jù)繪制AGV的實(shí)際糾偏過程,如圖5所示.在初始大角度偏差下,糾偏過程分兩個(gè)階段:從第1步到第15步,采用基于偏差分析的智能預(yù)測(cè)控制,將大偏差狀態(tài)快速轉(zhuǎn)化為小偏差狀態(tài);從第16步到第22步,采用本文的多步預(yù)測(cè)最優(yōu)控制,同步和平穩(wěn)地消除兩種路徑偏差.實(shí)驗(yàn)結(jié)果表明了該最優(yōu)控制算法在AGV實(shí)際路徑跟蹤中的可行性和有效性.與仿真結(jié)果相比,實(shí)際實(shí)驗(yàn)的糾偏趨勢(shì)是相同的,只是在糾偏過程中存在一些波動(dòng).這一方面是由于圖像處理和偏差獲取存在一定時(shí)延,另一方面是由于電機(jī)驅(qū)動(dòng)系統(tǒng)的響應(yīng)存在一定滯后,此外,路徑曲率的變化也對(duì)跟蹤效果產(chǎn)生影響.一般地,AGV低速行駛的跟蹤誤差為:eθ≤1°,ed≤1m