劉 偉, 蔡晨星, 江如海， 馬利強, 李 昊

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

0 引 言

在移動機器人高階次系統(tǒng)軌跡規(guī)劃控制中，軌跡規(guī)劃受硬件與軌跡規(guī)劃算法限制，導致規(guī)劃結果精準性不高或實時性差。鑒于移動機器人領域對運動軌跡準確性與快速性[1]的要求不斷提高，因此，軌跡規(guī)劃算法依然是移動機器人領域研究的熱點。

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

1 二階動力學控制模型

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

Lk——中間過程代價總和；

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——總時間成本分解后的海森矩陣塊。

對海森矩陣塊與梯度向量展開，展開的二階動力學控制模型為

(1)

式中：fu——動力學控制量一階偏導；

fx——動力學狀態(tài)量一階偏導；

fxx——動力學狀態(tài)量二階導數；

fuu——動力學控制量二階導數；

Wk、Rk——正定成本加權矩陣；

Wk、rk——動力學控制量二階導數。

2 動力學模型的優(yōu)化求解

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

式中：Rk——正定成本加權矩陣；

H——樣本點集。

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

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

式中：τ——采樣點集初始權值；

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

β——高階項信息參數；

λ——采樣點集的縮放因子；

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

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

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

(2)

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

i=1,2…,m+n

(3)

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

U——控制量矩陣；

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

將一階動力學導數求解值代入下式：

(4)

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

式中：ck——常數項；

KkΔx——線性反饋項。

將常數項與線性反饋項代入Zxx|k和Zx|k：

當成本迭代運算到k時刻時，成本預期變化為

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

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

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

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

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。兩種算法初末總成本量保持一致。無跡微分動態(tài)規(guī)劃算法在單次迭代上更容易往最優(yōu)區(qū)域搜索，成本減小量高于微分動態(tài)規(guī)劃算法，但最終收斂成本與微分動態(tài)規(guī)劃算法相同，表明無跡微分動態(tài)規(guī)劃算法控制成本壓縮性一致。

3.2 實時性評估

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

表1 單次迭代數據Table 1 Single iteration data

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

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

表2 倒立擺模型仿真運行結果Table 2 Simulation results of inverted pendulum model

4 實驗驗證

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

4.1 移動機器人動力學建模

表3 移動機器人參數Table 3 Parameters of mobile robot

圖3 動力學模型Fig. 3 Dynamic model

機器人動力學模型公式為

4.2 實驗方案設計

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

圖4 道路軌跡Fig. 4 Road trajectory

4.3 Z型路徑實驗與驗證

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

圖5 道路運行結果Fig. 5 Road operation results

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

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

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

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

5 結 論

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

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