基于無跡卡爾曼改進(jìn)微分動(dòng)態(tài)軌跡規(guī)劃的實(shí)時(shí)算法

劉 偉, 蔡晨星, 江如海， 馬利強(qiáng), 李 昊

(1.安徽建筑大學(xué) 機(jī)械與電氣工程學(xué)院， 合肥 230601； 2.合肥中科智馳科技有限公司， 合肥 230601)

0 引 言

在移動(dòng)機(jī)器人高階次系統(tǒng)軌跡規(guī)劃控制中，軌跡規(guī)劃受硬件與軌跡規(guī)劃算法限制，導(dǎo)致規(guī)劃結(jié)果精準(zhǔn)性不高或?qū)崟r(shí)性差。鑒于移動(dòng)機(jī)器人領(lǐng)域?qū)\(yùn)動(dòng)軌跡準(zhǔn)確性與快速性[1]的要求不斷提高，因此，軌跡規(guī)劃算法依然是移動(dòng)機(jī)器人領(lǐng)域研究的熱點(diǎn)。

微分動(dòng)態(tài)規(guī)劃算法具有全局最優(yōu)，二階收斂的特點(diǎn)，是一種基于數(shù)值優(yōu)化的軌跡規(guī)劃算法[2-4]，能使用機(jī)器人系統(tǒng)[5]的狀態(tài)和控制約束對機(jī)器人進(jìn)行處理。常見的改進(jìn)微分動(dòng)態(tài)規(guī)劃算法舍棄海森矩陣中動(dòng)力學(xué)二階導(dǎo)數(shù)以提高計(jì)算效率，會導(dǎo)致運(yùn)算提前結(jié)束迭代，無法求解出最優(yōu)軌跡[6-7]。動(dòng)力學(xué)二階信息決定了最優(yōu)軌跡的求解效果，直接影響機(jī)器人系統(tǒng)執(zhí)行軌跡[8]。針對微分動(dòng)態(tài)規(guī)劃算法在移動(dòng)機(jī)器人路徑規(guī)劃上運(yùn)算實(shí)時(shí)性不強(qiáng)，動(dòng)力學(xué)導(dǎo)數(shù)計(jì)算的研究還不完善，筆者利用平方根無跡卡爾曼濾波(SRUKF)原理與拉氏定理進(jìn)行近似求解，使用倒立擺模型對算法的穩(wěn)定性與求解特性進(jìn)行驗(yàn)證，在移動(dòng)機(jī)器人實(shí)際運(yùn)行場景下選取Z型路況對比兩種算法迭代時(shí)間，驗(yàn)證算法實(shí)時(shí)性。

1 二階動(dòng)力學(xué)控制模型

對于高階非線性動(dòng)力學(xué)系統(tǒng)，通常使用運(yùn)行總成本反映系統(tǒng)的性能與輸入能量，為達(dá)到最優(yōu)控制效果，可以利用運(yùn)行總成本函數(shù)尋求一組最優(yōu)控制輸入滿足系統(tǒng)性能要求。微分動(dòng)態(tài)規(guī)劃算法[2]的運(yùn)行總成本為Jk，設(shè)Zk為微分動(dòng)態(tài)規(guī)劃算法k=N最優(yōu)總成本。

Zk(x+Δx)=minu[Lk(x+Δx,u+Δu)+

Vk+1(f(x+Δx,u+Δu))]，

Zk+1(f(x+Δx,u+Δu))，

式中：Zk——微分動(dòng)態(tài)規(guī)劃算法k=N最優(yōu)總成本；

Lk——中間過程代價(jià)總和；

Vk+1——最優(yōu)總成本；

x——系統(tǒng)狀態(tài)變量；

u——控制輸入量。

ο(ΔxTΔx)，

式中：Zx|k——最優(yōu)總成本在x處的梯度；

Zxx|k——最優(yōu)總成本在x處的海森矩陣；

Tx——狀態(tài)梯度向量；

Tu——控制梯度向量；

Txx、Txu、Tux、Tuu——總時(shí)間成本分解后的海森矩陣塊。

對海森矩陣塊與梯度向量展開，展開的二階動(dòng)力學(xué)控制模型為

(1)

式中：fu——?jiǎng)恿W(xué)控制量一階偏導(dǎo)；

fx——?jiǎng)恿W(xué)狀態(tài)量一階偏導(dǎo)；

fxx——?jiǎng)恿W(xué)狀態(tài)量二階導(dǎo)數(shù)；

fuu——?jiǎng)恿W(xué)控制量二階導(dǎo)數(shù)；

Wk、Rk——正定成本加權(quán)矩陣；

Wk、rk——?jiǎng)恿W(xué)控制量二階導(dǎo)數(shù)。

2 動(dòng)力學(xué)模型的優(yōu)化求解

為求解式(1)的海森矩陣，使用Cholesky分解生成樣本點(diǎn)為

式中：Rk——正定成本加權(quán)矩陣；

H——樣本點(diǎn)集。

對樣本點(diǎn)進(jìn)行重排列成H=[H1,H2,H3…Hm+n]代入下列矩陣[9]：

τ=ζ2(m+n+κ)-(m+n)，

式中：τ——采樣點(diǎn)集初始權(quán)值；

ζ——正值比例縮放因子；

β——高階項(xiàng)信息參數(shù)；

λ——采樣點(diǎn)集的縮放因子；

κ——自由調(diào)節(jié)因子。

通過以上計(jì)算，Zxx|k+1和Rk賦值狀態(tài)與控制向量上并不斷計(jì)算傳遞。在后向傳遞過程中，海森矩陣[9]表示為

對(1)中梯度向量進(jìn)行迭代計(jì)算。式(7)中的形成的sigma點(diǎn)集攜帶了所有一階信息[10]。利用無跡變換思想將Zx|k+1映射到用于計(jì)算海森矩陣的樣本點(diǎn)[11-12]上進(jìn)行海森矩陣計(jì)算：

(2)

由式(2)可知，根據(jù)差分定理得到線性方程組：

i=1,2…,m+n

(3)

式中：X——狀態(tài)量矩陣；

U——控制量矩陣；

E——最優(yōu)成本矩陣；

將一階動(dòng)力學(xué)導(dǎo)數(shù)求解值代入下式：

(4)

式(2)至(4)為海森矩陣與梯度求解過程。求解出的Δu的最小化方程控制軌跡的如下：

式中：ck——常數(shù)項(xiàng)；

KkΔx——線性反饋項(xiàng)。

將常數(shù)項(xiàng)與線性反饋項(xiàng)代入Zxx|k和Zx|k：

當(dāng)成本迭代運(yùn)算到k時(shí)刻時(shí)，成本預(yù)期變化為

向前遞推求解出u1后結(jié)束正向遞推。利用新狀態(tài)控制U反饋修正計(jì)算新狀態(tài)X。

在達(dá)到收斂條件[11]前不斷重復(fù)上述過程，最終達(dá)到局部最優(yōu)。

3 倒立擺系統(tǒng)的仿真

為驗(yàn)證文中提出的無跡微分動(dòng)態(tài)規(guī)劃算法的可行性，進(jìn)行倒立擺系統(tǒng)仿真實(shí)驗(yàn)[13-15]。仿真流程如下，聯(lián)立倒立擺模型的非線性動(dòng)力學(xué)方程與代價(jià)函數(shù)，在Matlab軟件中求解。計(jì)算運(yùn)行價(jià)值的海森矩陣及一階導(dǎo)數(shù)，計(jì)算最優(yōu)的控制量與狀態(tài)量之間的關(guān)系式。求解X、U及ΔJ，判斷ΔJ是否滿足誤差，當(dāng)ΔJ滿足誤差時(shí)輸出最優(yōu)控制軌跡u并結(jié)束運(yùn)行；若ΔJ不滿足條件，減小α值并返回至非線性倒立擺動(dòng)力學(xué)方程。

3.1 控制效果評估

兩種算法在倒立擺仿真系統(tǒng)的控制量變化與總成本變化如圖1和2所示。由圖1可知，兩種算法的控制量保持一致，表明新算法保留了原算法的規(guī)劃控制特性。

圖1 算法控制量變化Fig. 1 Algorithm control quantity change

圖2 規(guī)劃總成本變化Fig. 2 Total planned cost change

對比兩種算法規(guī)劃成本變化，新算法與原算法的總成本都從3 780 mm降至1 643.77 mm。兩種算法初末總成本量保持一致。無跡微分動(dòng)態(tài)規(guī)劃算法在單次迭代上更容易往最優(yōu)區(qū)域搜索，成本減小量高于微分動(dòng)態(tài)規(guī)劃算法，但最終收斂成本與微分動(dòng)態(tài)規(guī)劃算法相同，表明無跡微分動(dòng)態(tài)規(guī)劃算法控制成本壓縮性一致。

3.2 實(shí)時(shí)性評估

兩種算法單次迭代時(shí)的具體數(shù)據(jù)見表1。無跡微分動(dòng)態(tài)規(guī)劃算法的總成本lw下降速度更快，選取第3次、第5次和第7次迭代結(jié)果，無跡微分的總成本量分別為2 585.2、1 960.6和1 694 mm。

表1 單次迭代數(shù)據(jù)Table 1 Single iteration data

與微分動(dòng)態(tài)規(guī)劃算法同次迭代結(jié)果相比，總成本量為微分動(dòng)態(tài)規(guī)劃算法總成本ld量的97.79%、90.76%和86.4%。三次迭代結(jié)果對比，無跡算法總成本比微分算法總成本分別壓縮2.21%、9.3%和13.6%。新算法單次迭代的壓縮效率更高，能有效減少迭代時(shí)間，提高微分動(dòng)態(tài)規(guī)劃算法的實(shí)時(shí)性。

倒立擺模型運(yùn)行結(jié)果如表2所示，兩種算法的最終成本量一致，優(yōu)化算法的單次迭代運(yùn)行時(shí)間為440 ms，單次運(yùn)算時(shí)長為微分動(dòng)態(tài)規(guī)劃算法的48.73%，單次迭代運(yùn)算效率提升51.27%。優(yōu)化算法總運(yùn)行時(shí)間為23.35 s，原算法運(yùn)行時(shí)間為135.39 s，總運(yùn)行時(shí)間效率提升了82.75%。綜上分析，優(yōu)化算法的實(shí)時(shí)性優(yōu)于原算法。

表2 倒立擺模型仿真運(yùn)行結(jié)果Table 2 Simulation results of inverted pendulum model

4 實(shí)驗(yàn)驗(yàn)證

為進(jìn)一步驗(yàn)證優(yōu)化算法在實(shí)際應(yīng)用中的實(shí)時(shí)性，將文中算法應(yīng)用到XL4301輪式移動(dòng)實(shí)驗(yàn)平臺，實(shí)驗(yàn)平臺搭載GPS系統(tǒng)，采用北科天繪激光雷達(dá)以及上位機(jī)。

4.1 移動(dòng)機(jī)器人動(dòng)力學(xué)建模

表3 移動(dòng)機(jī)器人參數(shù)Table 3 Parameters of mobile robot

圖3 動(dòng)力學(xué)模型Fig. 3 Dynamic model

機(jī)器人動(dòng)力學(xué)模型公式為

4.2 實(shí)驗(yàn)方案設(shè)計(jì)

實(shí)驗(yàn)原理：設(shè)置Z型實(shí)驗(yàn)道路，經(jīng)兩種算法求解后獲得最優(yōu)軌跡，發(fā)布虛擬軌跡節(jié)點(diǎn)。虛擬軌跡節(jié)點(diǎn)生成后經(jīng)VCU節(jié)點(diǎn)轉(zhuǎn)換為控制命令，電機(jī)接收信號運(yùn)轉(zhuǎn)，輪式機(jī)器人運(yùn)行。利用GPS與慣導(dǎo)記錄相應(yīng)的數(shù)據(jù)與信息，對比每次實(shí)驗(yàn)的算法運(yùn)行的總時(shí)間與機(jī)器人運(yùn)行軌跡，分析算法實(shí)時(shí)性與控制特性。實(shí)驗(yàn)場景如圖4所示，選取Z字型路況，Z字型彎道路況包含兩個(gè)轉(zhuǎn)角，全長40 m。

圖4 道路軌跡Fig. 4 Road trajectory

4.3 Z型路徑實(shí)驗(yàn)與驗(yàn)證

Z型道路實(shí)際運(yùn)行的控制參數(shù)變化如圖5所示。第一段直線加速過程前輪轉(zhuǎn)角與轉(zhuǎn)角加速度不變，速度上升到最大。第二段速度降低，加速度先減小后回落至0 m/s2，前輪轉(zhuǎn)角先上升再降低至0°；第三段速度、加速度、前輪轉(zhuǎn)角與前輪轉(zhuǎn)角導(dǎo)數(shù)均無變化；第四段路程前輪轉(zhuǎn)角先下降后上升，輪轉(zhuǎn)角導(dǎo)數(shù)先下降再上升最后回歸到0 (°)/s附近。第五段速度上升，輪轉(zhuǎn)角與輪轉(zhuǎn)角導(dǎo)數(shù)均不變。前輪轉(zhuǎn)角最大值為30°，最小值為-30°；前輪轉(zhuǎn)角導(dǎo)數(shù)最大值為11 (°)/s，最小值為-11 (°)/s。

圖5 道路運(yùn)行結(jié)果Fig. 5 Road operation results

Z型路況最為復(fù)雜，包含兩處較大轉(zhuǎn)角，Z型路徑規(guī)劃原算法總用時(shí)為3.44 s,優(yōu)化算法規(guī)劃時(shí)間為2.16 s，相較于微分動(dòng)態(tài)規(guī)劃算法新算法運(yùn)行效率提升了37.2%。

對比無跡微分動(dòng)態(tài)規(guī)劃與微分動(dòng)態(tài)規(guī)劃實(shí)際運(yùn)行結(jié)果，二者規(guī)劃結(jié)果從實(shí)際控制量來看基本保持一致，控制量算法上兩種算法運(yùn)行結(jié)果極值相同；相同時(shí)間段內(nèi)兩者控制量曲線剔除實(shí)際道路對數(shù)據(jù)的影響趨向相同。兩種算法計(jì)算控制軌跡結(jié)果相同，說明新算法可以保留原算法特性。

優(yōu)化算法在Z字型道路實(shí)驗(yàn)中求解時(shí)間較原算法縮短了37.2%。實(shí)驗(yàn)分析得新算法比微分動(dòng)態(tài)規(guī)劃算法有更高的實(shí)時(shí)性，且遇到大轉(zhuǎn)角復(fù)雜情況時(shí)優(yōu)化算法有更好的實(shí)時(shí)性。優(yōu)化算法運(yùn)行總時(shí)間相比于原算法運(yùn)行總時(shí)間更短。因此，可以得出在實(shí)際路況，優(yōu)化算法比原算法實(shí)時(shí)性更好。

從仿真實(shí)驗(yàn)得出，兩種算法從控制量到壓縮成本量上均保持一致，在實(shí)時(shí)性上無跡微分動(dòng)態(tài)規(guī)劃算法收斂性更好，能更快完成成本壓縮，具有更高的實(shí)時(shí)性；從實(shí)際道路實(shí)驗(yàn)可得兩種算法在實(shí)際控制效果基本保持一致，實(shí)際道路求解實(shí)時(shí)性均高于微分動(dòng)態(tài)規(guī)劃算法，道路實(shí)驗(yàn)驗(yàn)證了仿真結(jié)果的正確性。

5 結(jié) 論

(1) 分析了微分動(dòng)態(tài)規(guī)劃算法動(dòng)力學(xué)信息求解過程，建立了二階動(dòng)力學(xué)控制模型，引入無跡變化構(gòu)建動(dòng)力學(xué)樣本點(diǎn)與差分思想求解梯度與導(dǎo)數(shù)，優(yōu)化了動(dòng)力學(xué)模型中海森矩陣與梯度計(jì)算方法。

(2) 通過倒立擺仿真與Z型路況實(shí)驗(yàn)對比分析了兩種算法迭代時(shí)間，仿真環(huán)境下優(yōu)化算法總運(yùn)行時(shí)間效率縮短了82.75%，單次迭代運(yùn)算時(shí)間效果縮短了51.27%，在實(shí)際道路測試中改進(jìn)算法求解性能提高了37.2%，無跡微分動(dòng)態(tài)規(guī)劃算法實(shí)時(shí)性優(yōu)于算法。