基于泛函梯度優化的運動規劃算法

傅來磊，易 陽，張興華

(南京工業大學 電氣工程與控制科學學院，江蘇 南京 211816)

0 引 言

復雜環境中高維機械臂規劃有效運動需要長達數秒的時間，而人體往往能夠快速生成豐富多樣的運動軌跡。原因在于，人類運動系統有效利用了動作技能的先驗知識，能夠根據不同的環境和任務直接對運動軌跡進行優化，保證了運動軌跡生成的快速性[1]。這種基于樣本經驗的軌跡生成機制成為了機械臂運動規劃的熱點研究問題[2]。難點在于如何將外界任務環境的變化映射到軌跡規劃的空間并保證計算效率。

動態運動基元模型[3]能夠將運動軌跡空間以參數的形式呈現，通過效用函數對動作特征的參數進行優化，直到找到滿意的動作。動態運動基元對機械臂各維度單獨進行優化，降低了動作空間的維度，大大簡化了搜索空間，使得策略搜索的探測過程更為有安全有效。但是在連續動作空間該方法還是很難解決實際中的問題。

近年來泛函梯度算法表現出良好的計算效率，基本思路是設計基于運動軌跡的目標泛函來評價外界環境，從而在數學上可計算求取目標泛函對運動軌跡的梯度，直接采用梯度下降的方法重新規劃平滑無碰撞的運動軌跡[4,5]。

受此啟發，本文使用泛函梯度對運動樣本進行優化，不同的是，使用動態運動基元提取運動特征，設計基于外界環境信息和內部控制輸入為動作軌跡的目標泛函，通過求取目標泛函的泛函梯度快速優化運動軌跡，使其滿足無碰撞及高能效等性能要求。與現有算法相比，該方法具備以下優點：

(1)無需大量樣本數據存儲與訓練；

(2)對運動軌跡控制輸入進行優化，提高了軌跡能量效率；

(3)簡化了搜索空間同時提高了計算效率，能夠適用于連續動作空間優化。

1 系統簡介

通常外界環境與工作任務是在低維笛卡爾空間中表示，多自由度機械臂運動軌跡是在高維關節空間中描述，而運動軌跡經驗是在高維隱空間中特征。本文使用動態運動基元模型將關節空間的機械臂運動軌跡編碼為隱空間內的軌跡特征。然后設計以該特征的目標泛函，包含當前機械臂所處的外界物理環境成本和執行當前運動軌跡所需要的控制成本作為性能指標函數。由此，目標泛函將隱空間內的運動特征與笛卡爾空間內的工作任務聯系起來。將已習得的運動特征經驗作為初值，然后采用梯度下降法更新迭代直到獲得局部最優策略。最后求取的最優策略通過動態運動基元解碼，獲得無碰撞且高能效的期望參考軌跡。系統框架如圖1所示。

圖1 系統框架概述

2 算法實現

2.1 動態運動基元編碼

動態運動基元是軌跡模仿學習的主流方式[6]。其思想是以一類動力系統來描述機器人運動軌跡，使其確保運動的穩定性與收斂性。定義(t),(t),(t) 為單個自由度的位置、速度和加速度狀態，單個動作可表示為

(1)

其中，αz(βz(g-(t))-(t)) 為彈性系統，αzβz為彈性系統，g為軌跡目標，τ為時間常數。彈性系統驅動系統狀態收斂到軌跡目標。F(x) 為強迫項，為非線性的核函數的線性組合，利用非線性特征得到豐富的軌跡形狀

(2)

其中，ω是可調節的線性權重，φi(x) 為基函數矩陣，是x的函數。x通過如下正則系統產生，對運動時間進行非線性歸一化

φi(x)=exp(-hi(x-ci)2)

(3)

(4)

由此，設置不同的強迫項F(x)， 即動作特征ω， 系統即生成收斂于目標的不同軌跡形狀。圖2表示通過動態運動基元對運動軌跡和動作特征之間進行編碼與解碼關系。

圖2 動態運動基元模型

為了將運動軌跡顯示的表達為動作特征的函數，使用有限差分矩陣D1和D2

(k)=A(k)ω+Bg

其中

(5)

(6)

將軌跡用K個離散點表示，即

=[(1),…(k),…(K)]

離散軌跡特征為

(ω)=Aω+Bg

(7)

其中，A=[A(1),…A(k),…A(K)]T，B=IKB。

2.2 目標泛函設計

圖3 目標泛函設計

將目標泛函C() 設計為關節限制約束下無碰撞項和節能項的加權和

C[]=λ1Clim+λ2Cobs[]+Ceff[]

(8)

第一個必須滿足的條件是關節角度的限制Clim，設計為

(9)

第二項對于無碰撞目標泛函Cobs， 由于障礙物則位于世界坐標系下的笛卡爾空間，關節軌跡位于機械臂內部關節空間，使用正向運動學計算機械臂本體在給某一姿態時刻的世界坐標位置：K((i),b)， 其中b∈Brob為機械臂本體上某一點。則在運動軌跡某一時刻，設計機械臂本體與障礙物距離之間的懲罰函數c((k))∶R3→R可設計為

c(

(10)

其中，d() 是機器人運動軌跡與最近的障礙物邊界的距離，0指障礙物的最小安全緩沖距離。考慮整體運動軌跡和機械臂本體所有離散點，τ()={(k,b),k∈[1,K],b∈Brob} 使用最值約化算子，無障礙的目標泛函可設計為

(11)

(12)

這里M為機械臂系統慣性矩陣，C為科氏矩陣，G為重力項。使用線性反饋控制器對目標參考軌跡(t),(t),(t) 進行追蹤，控制u為

u=diag(Kpos)(-y)+diag(Kvel)(

(13)

能量損耗為控制輸入的總和，則

(14)

綜上所定義的目標泛函，則理想的無碰撞高能效的運動軌跡為該目標泛函的最優值

(15)

2.3 運動軌跡優化

使用泛函梯度對運動軌跡進行優化，是將運動軌跡整體看作一個幾何對象，在無限維空間中即為一個點[7]。泛函梯度沿著泛函C[] 下降速度最快的方向移動軌跡∈Ξ。 這里泛函梯度是針對整體軌跡而言，并非某個參數的梯度而言。因此，泛函梯度可能沿著對參數變化較小，某些參數變化較大的方向進行，與一般的參數梯度有所區別。定義為泛函梯度。為了說明軌跡空間Ξ的幾何結構，在運動軌跡中加入微小的擾動量φ后，看軌跡如何變化

(16)

本文使用統計學中Kullback-Leibler(KL)散度距離進行表示軌跡的變化

DKL(+εφ)=Ε[log()-log(+εφ)]

DKL(+εφ)≈-εTΕ
TΕ

作為一階項

Ε
DKL(+εφ)≈-εTGε

定義二次型的費雪信息矩陣評估擾動程度

F=-Ε

(17)

可見費雪信息矩陣有助于描述基于KL距離變化的最陡下降方向，而與具體的參數無關。也就是說，軌跡空間有著自身的幾何形狀，泛函梯度指向幾何變換最快的方向，并不是某個參數的變化方向。實踐中，費雪信息矩陣可以通過數值計算。

給定式(8)目標泛函，可由迭代接近法求取式(15)中的優化值。在當前軌跡i附近的目標泛函處線性化，近似得基于泛函梯度的目標泛函迭代更新值

C(i)(-i)+ο((-i)2)

(18)

每次迭代i中，對目標泛函進行一階泰勒近似，并取最大值

(19)

(20)

則迭代更新規則變為

(21)

由此，更新規則式(21)將局部梯度變化擴展至軌跡的其余部分。解決該優化問題后，帶入式(7)可得動作特征的迭代式

(22)

最后根據動態運動基元模型解碼為期望參考軌跡并實現機械臂的實時控制。

3 實驗設置和結果

首先在matlab中驗證在平面軌跡優化中泛函梯度優化性能。接著在v-rep中使用六自由度機械臂(kinova Jacob2)來驗證算法的可行性，采用matlab和v-rep通訊的方式進行實驗。目的是在有障礙環境和高能效要求等約束條件下快速生成可行的參考軌跡。如第2節所述，初始動作特征用ω0和常數φ來表示。高斯基函數的數量為Nbasi=10， 中心點為ci=e-0.9ni,ni=1,2,…Nbasi， 寬度為hi=0.03，τ=1， 時間步數為Ntimestep=10。 所有常數φ是一個10×10的矩陣，而ω0為10×1的向量。動態運動基元的參數設置如下：αz=2,βz=0.5,αt=0.9。 實驗中設置目標的位置g后，通過簡單計算即可得到初始軌跡 ((t),(t),(t))。

3.1 平面軌跡優化

在matlab中設計一條平面軌跡，通過DMP模型，將兩條軌跡參數化，并設計兩個障礙物，分別通過普通的梯度優化方式和泛函梯度優化的方式對軌跡進行優化，結果如圖4所示。

圖4 平面避障軌跡對比

相比與泛函梯度優化，普通的梯度優化方法，雖然也能夠完成避障的任務，但是通常是對于避障點局部優化，沒有考慮軌跡的整體性，軌跡整體的變化較大。而泛函梯度優化能夠考慮軌跡的整體性，在滿足避障要求的情況下，軌跡的變化較小，保留了軌跡原有的經驗特征，在以后的任務中，仍然可以當作經驗學習。

3.2 機械臂快速避障

在桌面工作區隨機擺放物體，創建隨機工作場景。目標位置為(0,0,0)m，而機器人初始末端執行器位置為(0.1，0.4,0.1)m。在圖5中，下方穿越障礙物方塊的為原始軌跡，上方機械臂的末端抓手運行的軌跡為優化以后的軌跡。獲取目標g的初始軌跡會撞到障礙物。這種情況下，末端執行器應該向上移動才能避開障礙物，然后再向下移動獲取目標。運用式(8)的目標泛函優化運動軌跡避開碰撞同時滿足關節限制約束。如圖6所示，以變化較大的關節-3為例，初始軌跡幾乎是直線，以固定步長η=0.4對動作特征ωi進行4次迭代，各個關節的軌跡就能在圖5中表現為無碰撞的期望運動軌跡。

圖5 機械臂kinova Jacob2抓取了桌上設有障礙的杯子

圖6 關節-3 關節角度迭代

3.3 機械臂高能效運動

本實驗中更新的機械臂的運動目標，機械臂需更新運動軌跡實現高能效的性能指標。如式(13)所示，設置控制器參數為Kpos=diag-10,-500,-100,-80,-50和Kvel=diag-1,-20,-20,-10,-5。 在能效約束的目標泛函下對初始軌跡進行重新規劃，如圖7所示，左側軌跡為原始軌跡，右側機械臂末端抓手的運行軌跡，為能耗優化以后的軌跡。圖8以能耗最大的關節-2為例，關節-2的能耗占整體50%左右，可以看出，優化以后的關節-2能耗下降十分明顯。

圖7 通過動作特征優化形成高能效的運動軌跡

圖8 關節-2力矩迭代

另外，對該算法的泛化能力進行了測試。在整個工作區間隨機生成25個目標點實驗，記錄能效降低程度和迭代次數。如表1所示，大部分優化的軌跡能夠在小于6次迭代級情況下，能耗下降在20%以上。

表1 迭代次數與能耗減少

4 結束語

本文提出了基于泛函梯度優化的運動規劃算法，結合了動態運動基元的軌跡特征提取方法和費雪信息矩陣的泛函梯度描述，通過對初始經驗軌跡的優化，使得多自由度機械臂能夠快速生成無碰撞且高能效的運動軌跡，體現出了良好的性能。但是在本次實驗中，所有的經驗軌跡都是通過人為選取，機械臂系統仍然無法通過所執行的任務和外部環境來自我判斷選擇經驗軌跡，沒有達到真正意義上的智能化。所以如何建立機械臂系統對于運動軌跡和外部環境之間的學習機制將會是一個重要的研究方向。