基于運動預(yù)測的多步控制多步優(yōu)化模型

基于運動預(yù)測的多步控制多步優(yōu)化模型

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