基于任務參數加權的動態運動基元泛化方法

張 磊 方灶軍 王聚幸 何 晨 顧丹寧

1.中國科學院寧波材料技術與工程研究所，寧波，3152012.中國科學院大學，北京，1000493.浙江省機器人與智能制造裝備技術重點實驗室，寧波，315201

0 引言

在不久的將來，機器人將成為人類社會不可或缺的一部分，在教育、醫療、家庭、災害場景及太空探索等領域為人們提供協助。機器人將不僅在結構化環境下執行單一任務，而是會面臨非結構化復雜環境下的多變任務。為所有場景和任務都進行傳統的示教編程是非常耗時耗力的，因此，機器人需要依靠自身的學習能力完成相應任務[1]。

傳統的機器人示教編程類似于簡單的“復制粘貼”操作，對某一特定場景下的特定任務進行示教，機器人只能在該場景下完成對應任務，若場景或任務條件稍有變化，任務便不能完成，需要對機器人重新示教。而機器人示教學習(learning from demonstration，LfD)通常也稱作機器人模仿學習(imitation learning)，運用機器學習的方法使機器人學習示教者的動作技巧從而掌握新的任務技能。與傳統機器人示教編程相比，LfD具有泛化能力，在場景和任務與示教不同的情況下，機器人仍然能憑借從示教過程中學習到的動作技巧和特征完成任務。

針對單條運動軌跡的示教學習，IJSPEERT等[2]提出了一種基于動態運動基元(dynamic movement primitive，DMP)的軌跡編碼方法，能夠將示教運動軌跡的形狀特征以特征參數的形式保存，對于一個新的任務場景，可以通過學習到的特征參數重構一條包含示教者運動特征的運動軌跡。針對多條運動軌跡的示教學習，在DMP的基礎上，UDE等[3]首先使用高斯過程回歸(Gaussian process regression，GPR)求出任務參數到DMP運動軌跡目標點g與時間縮放量τ的映射，其他模型參數固定為常數；然后使用局部加權回歸(locally weighted regression，LWR)基于所有示教運動軌跡的非線性項計算出新的特征參數，最后根據新的特征參數重構新的運動軌跡。劉環等[4]首先使用LWR計算出所有示教運動軌跡對應的DMP特征參數，對于一個新的任務參數，基于計算得到所有的特征參數，使用高斯過程回歸求出相應的新的特征參數，最后重構新的運動軌跡。

以上兩種泛化方法存在計算效率不高以及在示教任務參數附近的泛化性能差等問題。本文提出了一種基于任務參數加權的動態運動基元泛化方法。首先對于每條示教運動軌跡，使用局部加權回歸計算出對應的特征參數；對于一個新的任務，由各條示教運動軌跡學習到的特征參數計算出在新的任務參數處的特征運動軌跡；基于示教任務參數與新的任務參數的相似度給予各條特征運動軌跡不同的權重，然后對各條特征運動軌跡進行加權疊加形成新的運動軌跡；對動態運動基元及常用的泛化方法進行介紹，并重點闡述所提方法。

1 動態運動基元及其泛化分析

1.1 動態運動基元

DMP模型是一個特征參數可學習的非線性吸引子系統，可以用于編碼離散運動軌跡，DMP模型表達式如下：

(1)

(2)

(3)

其中，式(1)、式(2)本質上是一個彈簧阻尼系統，式(3)是一個正則系統(canonical system)，這樣設計是為了得到一個便于分析的穩定系統，并且可以通過調制非線性項達到期望的吸引子行為。式(1)～式(3)中，τ表示時間縮放量，用于調整運動軌跡的持續時間；y和z分別表示運動軌跡某時刻的位置和速度；y0和g分別表示運動軌跡的起始點和終止點；αz、βz是正常數，其關系通常設置為βz=αz/4。通過選擇合適的值，可以使系統處于臨界阻尼狀態，從而使運動軌跡的終止位置y收斂到終止點g；αs是正則系統的常數；s是相位變量，用來替代時間變量t，使系統擺脫對時間的顯式依賴，隨著t的增加，s逐漸從1單調遞減至0，由此可以保證系統最終收斂到終止點g；f(s)是系統中的非線性項，它是關于相位變量s的一個函數，可以通過調整不同時刻的吸引子行為來改變運動軌跡的形狀。

為了實現更加通用的吸引子表達形式，定義如下：

(4)

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

(5)

對于具有多個自由度的動態系統，需要對每個自由度分別單獨建立變換系統，正則系統提供了各個自由度之間的時間耦合，而變換系統實現了每個自由度所需的吸引子動態特性。

1.2 動態運動基元泛化分析

在機器人示教學習領域中，泛化性能指的是當任務場景發生變化時，機器人仍能模仿示教運動的特征完成相應的任務。需要強調的是，單條示教運動軌跡學習所得的DMP模型也具有一定的泛化能力，其泛化能力在該運動軌跡任務參數附近具有較好的效果，但是當新的任務參數偏離該示教任務參數較遠時，其泛化能力不能夠令人滿意。就整個機器人工作空間而言，如果只用單個任務參數處的示教運動軌跡的特征來代表整個工作空間的特征，顯然是不合理的。因此需要在機器人工作空間采集多組示教運動軌跡，保證對應示教運動軌跡的任務參數盡可能均勻地覆蓋整個機器人工作空間。對于一個新的任務場景下的任務參數，新的運動軌跡由多條示教運動軌跡生成。每條運動軌跡的形狀特征由特征參數{ωi}(i=1，2，…，N)唯一確定，因此生成一條新的任務參數下運動軌跡可認為是生成一組新的特征參數。每一條示教運動軌跡對新的運動軌跡生成的影響是不同的，可以做出合理的假設：任務參數與新的任務參數越接近的示教運動軌跡對新的運動軌跡學習的貢獻越大，新的運動軌跡的形狀特征與同其任務參數相近的示教運動軌跡相似度更高。

2 動態運動基元泛化算法介紹

(6)

XW=f

(7)

(8)

(9)

其中，W和f分別為特征參數向量和非線性項向量，X為不同相位變量下的各個高斯基函數的值所構造的矩陣，特征參數W可以由式(7)對應的線性方程組的最小二乘解計算得到。

2.1 基于LWR的動態運動基元泛化

給定一個新的任務參數q，首先使用GPR(關于GPR的算法介紹參照2.2節)由q估計出運動軌跡終止點g和時間縮放量τ：

G(z)：q[τ，g]

(10)

對于點到點的任務，一般將時間縮放量τ設置為常數，將任務參數q設置為運動軌跡目標位置g。

對于特征參數的泛化，使用局部加權回歸，通過最小化目標函數V得到在新的任務參數下的特征參數W*：

(11)

可以認為，求出一組特征參數W*，使得新生成的運動軌跡與所有示教運動軌跡的差距最小，示教任務參數與q越接近的示教運動軌跡對特征參數W*的生成影響越大。式(11)中，Xk由式(9)計算得出，fk可由式(8)計算得出。任務參數空間中的K和距離d決定了每條示教運動軌跡對最終估計的控制策略的影響程度。K為核函數，文獻[3]使用的是三次核函數，因為該核函數具有有限的范圍和連續的一階和二階導數，并且在實驗中表現效果較好，即

(12)

其中，d是任務參數q空間中的度量，由歐氏距離定義：

(13)

(14)

由式(13)和(14)可知，參數D完全由參數c?R唯一決定，通過最小化驗證集誤差來得到參數c。

2.2 基于GPR的動態運動基元泛化

對于每條示教軌跡，使用局部加權回歸計算出對應的特征參數，對于每個ωi(i=1，2，…，N)，通過最小化如下目標函數Ji：

Ji=(Sωi-f)TΓi(Sωi-f)

(15)

其中，f由式(8)計算得列；S為每個時刻t對應的相位變量值x(t)所構造的向量；Γi為ωi對應的高斯基函數在所有相位變量x(t)的值構造的對角矩陣。S和Γi定義如下：

(16)

由式(15)可以推導計算出對應的特征參數ωi(i=1，2，…，N)的公式：

(17)

對于M條示教運動軌跡，可由式(17)計算得到M組特征參數ωi(i=1，2，…，N)。

在一個新的任務場景下，使用GPR求出新的任務參數到該場景下的特征參數的映射。高斯過程(Gaussian process，GP)定義為

g(q)～GP(m(q)，k(q，q′))

(18)

(19)

其中，W和W*為已知特征參數矩陣和待求特征參數；Q和q*為已知任務參數矩陣和新的任務參數；K(·，·)為相應的聯合協方差矩陣，一種常用的協方差函數定義如下：

(20)

其中，σf、l和σn為常數，h(q，q′)定義為

(21)

(22)

由式(22)可知，高斯過程回歸與局部加權回歸類似，與新的任務參數q*越接近的對應任務參數的示教運動軌跡對新的特征參數生成的影響越大。

2.3 基于任務參數加權的動態運動基元泛化

在實時運行的機器人模仿學習系統中，以上兩種泛化方法存在運行速度較慢、在示教任務參數附近泛化性能較差等不足。本文提出了一種新的基于任務參數加權的動態運動基元泛化方法，相比以上兩種動態運動基元泛化方法，在計算速度和示教任務參數鄰近區域的泛化性能方面都有了較大的提高。

(23)

(24)

(25)

基于任務參數加權的動態運動基元泛化方法流程圖如圖1所示，流程如下：

圖1 方法流程圖Fig.1 Flowchart of the method

(3)相機輸入。輸入新的任務參數q*。

(4)映射。由式(10)計算q到g和τ的映射。

對于一個實時運行的機器人系統，步驟(1)、(2)可以進行離線處理，可以減少運行過程中的計算量，提高實時運行速度。

3 實驗結果與分析

為了驗證本文方法的有效性，設置了一個積木收納的任務場景，如圖2所示。實驗中使用的機器人為Kukaiiwa協作機器人，視覺檢測裝備為Kinectv2深度相機，使用筆記本電腦作為上位機接收相機拍攝的任務場景信息，然后對機器人進行控制。該任務需要機器人將桌子上工作空間內擺放的積木收納到積木桶中。首先，通過合適的示教方式獲取示教數據，以示教數據為訓練數據離線訓練示教模型；對于一個新的任務場景，使用Kinectv2相機檢測出積木顏色和形狀特征以及位姿，獲取任務參數，然后使用訓練好的示教模型規劃出運動軌跡；最后由PC機控制機器人按照指定運動軌跡完成任務，每次任務中機器人起始位置固定。

圖2 積木收納任務場景Fig.2 Building block storage task scene

3.1 數據采集與預處理

圖3 動覺示教Fig.3 Kinesthetic teaching

機器人示教方法主要有三種：動覺示教、視覺示教及遙操作示教方法，三種方法各有優劣。本實驗采用的是動覺示教方法，示教過程如圖3所示。動覺示教也稱作手把手示教，對示教者的技術要求較低，能夠使示教者與機器人進行物理交互，感受機器人的運動模式及關節極限，由機器人自身的傳感器記錄關節角度和力矩的信息，為示教模型提供可靠的訓練數據[5]。

示教區域與示教點布置如圖4所示。示教區域的選擇由機器人關節極限以及任務場景共同決定。對于示教軌跡數量M的選取，從理論上講，有效示教軌跡數量M越多，模型的泛化性能越好，但總體來看，示教軌跡數量M并不是越多越好。首先，示教軌跡數量過多會導致模型計算效率下降，不利于運用在實時運行的機器人操作系統上；其次，示教軌跡數量過多會導致產生無效示教軌跡的概率增大，反而會對模型性能產生負面影響；最后，由于機器人示教學習方法傾向于使用小樣本學習，在保證模型有較好性能的基礎上，盡可能使用數量較少的樣本，以減輕示教者的工作負擔。示教軌跡數量M需要綜合以上幾個因素，根據具體的任務條件進行確定。本文共選取13個示教點，其中9個作為訓練點，編號為1～9，對應的示教軌跡作為模型訓練數據，為了使示教運動軌跡盡可能表示真實的工作空間特征，將9個示教點均勻布置在示教區域；另外4個示教點作為測試點，編號為a～d，對應的示教軌跡作為測試數據，用來檢驗泛化方法的泛化性能，不用于模型訓練。

圖4 示教區域與示教點布置Fig.4 Teaching area and teaching point arrangement

以示教點的積木空間位置作為任務參數，為了避免單條示教軌跡的隨機性，在每個示教點以50 Hz的采樣頻率采集8組關節角軌跡，經過正向運動學將關節角軌跡轉化為笛卡兒空間軌跡，對每個示教點的8組示教運動軌跡使用高斯混合模型(Guassian mixture model，GMM)建模，使用高斯混合回歸(Guassianmixture regression，GMR)計算出均值作為該示教點的示教運動軌跡[6]，如圖5所示。

圖5 示教運動軌跡Fig.5 Teaching trajectories

在使用三種泛化方法對訓練數據進行建模的過程中，三種泛化方法的DMP部分超參數設置相同，基函數數量N=100，時間縮放量τ=1，αz=60，αx=1。

3.2 泛化性能分析

單次示教模型只包含單個示教點示教軌跡的特征，只在該示教點附近區域具有較好的泛化性能[7-8]；多次示教模型則包含多個示教點示教軌跡的特征，可以使工作空間更大區域具有較好的泛化性能。為了比較單次與多次示教運動軌跡學習得到的DMP模型的泛化性能，以圖5所示的4個測試點的積木空間位置作為待泛化任務參數，用兩種模型分別生成相應的泛化運動軌跡，其結果如圖6所示，多次示教模型泛化出的軌跡相比單次示教模型要更順滑，變形更小，并且與圖4的對應測試運動軌跡更加相似，可知多次示教模型泛化性能優于單次示教模型。相似度為

(26)

圖6 單次與多次示教模型泛化性能比較Fig.6 Comparison of generalization performance between single and multiple teaching models

表1 遠離示教點時泛化軌跡相似度比較Tab.1 Similarity comparison of generalized trajectories far from teaching point mm

表2 示教點處泛化軌跡相似度比較Tab.2 Similarity comparison of generalized trajectories at teaching points mm

當任務參數遠離示教任務參數時，由表1的4組數據可以看出，三種泛化方法與測試運動軌跡的相似度差別很小，加權方法對應4組數據的平均值與LWR方法的差別只有2%左右，而與GPR方法的差別不足1%，由此可以推斷三種泛化方法在任務參數遠離示教任務參數時泛化性能大致相同。當任務參數接近示教任務參數時，由表2的9組數據可以看出，在極限情況下，加權泛化方法與測試運動軌跡的相似度明顯優于另外兩種方法。

為了進一步分析三種泛化算法在示教任務參數附近的泛化性能，如圖7a所示，對以第5個訓練點為圓心、半徑為20 mm的鄰近區域進行分析。對于圖中A、B、C、D四個方向，其中A、B分別與x軸和y軸平行，C、D為對角方向，以圓心為中心點，在每個方向均勻選取包括中心點在內的21個點作為泛化點，其中相鄰兩個點的距離為2 mm；對于每個方向的21個泛化點，分別用三種泛化算法生成相應的泛化運動軌跡，用式(26)計算出與示教運動軌跡的相似度，結果如圖7 b～圖7e所示。可以看出，在4個方向上加權泛化方法與示教運動軌跡的相似度都要優于另外兩種方法。經過分析可知，本文方法在示教任務參數鄰近區域的泛化性能好于另外兩種方法。這主要是由于當示教軌跡數量M確定時，對比另三種方法，本文方法會給予離新的泛化點最近的示教點對應示教軌跡更大的權重。

(a)示教任務參數鄰近區域 (b)方向A (c)方向B

(d)方向C (e)方向D圖7 示教任務參數附近泛化性能比較Fig.7 Performance comparison in the vicinity of teaching task parameters

3.3 計算效率分析

圖8 時間復雜度增長趨勢Fig.8 The growth trends of time complexity

就計算效率而言，主要考慮三種算法在線處理時的計算時間。從理論上分析三種方法的計算效率，相對于示教軌跡數量M，本文方法時間復雜度為O(M)；基于GPR的泛化方法在求解式(22)時，需要對矩陣(K(Q，Q)+σn2I)求逆，時間復雜度為O(M3)；基于LWR的泛化方法的計算時間集中在對式(11)目標函數的優化上，單次迭代時間復雜度為O(M2)，總體迭代時間復雜度為O(lM2)，其中，l為迭代次數。三種方法的計算時間都隨著示教軌跡數量M的增加而增加，三種時間復雜度的增長趨勢如圖8所示，可以看出，本文方法的計算效率在理論上要優于另外兩種方法。為了進一步比較三種方法在實驗中的計算效率，對于圖3所示的13個示教點，在使用三種泛化方法計算其泛化運動軌跡時記錄了對應的計算時間，結果見表3。可見每種泛化算法在計算每條運動軌跡的時間大致相等，而且數值都在其平均值附近波動；加權和GPR泛化方法的計算時間遠遠小于LWR泛化方法的計算時間，這是因為LWR泛化方法在優化式(11)的目標函數時，迭代次數過多，平均約為432次，花費了大量計算時間；同時，加權泛化方法計算時間明顯少于GPR泛化方法計算時間。分析可知，本文方法的計算效率相比另外兩種泛化方法有了較大的提高。

3.4 積木收納任務實驗驗證

為了驗證所提方法的有效性，設置了積木收納任務進行實驗驗證。在示教區域隨意設置20個泛化點，如圖9所示。在泛化點放置積木，以積木的空間位置作為待泛化任務參數，代入訓練好的模型得到泛化運動軌跡，由PC控制機器人按照該運動軌跡完成積木收納任務。單次積木收納任務流程如圖10所示，分別表示各種場景。共進行了20次積木收納實驗，其中成功了17次，成功率為85%，證明了本文方法的有效性。

表3 計算時間比較Tab.3 Calculation time comparisons s

圖9 積木收納任務泛化點布置Fig.9 Layout of generalization points for building block storage task

(a)起始位置 (b)接近積木

(c)夾取積木 (d)靠近積木桶

(e)到達對應積木孔上方 (f)放置積木圖10 積木收納任務流程Fig.10 Process of building block storage task

4 結論與展望

針對機器人示教學習在新的任務場景下的泛化問題，本文提出了一種基于任務參數加權的動態運動基元泛化方法，與已有的基于LWR和基于GPR的泛化方法相比，在計算效率和示教任務參數附近的泛化性能方面有了明顯的提升，使用Kukaiiwa協作機器人就積木收納任務進行了機器人示教學習實驗，證明了該方法的有效性。

本文方法主要針對的是機器人示教學習的軌跡層次的泛化問題，對于軌跡層次結合語義層次的泛化問題，該方法并不能很好地解決，這是目前機器人示教學習研究領域的一個難點和熱點。一種可能的解決方法是將本文方法進行拓展，使用兩種示教方式同時進行示教：一種采用拖動示教方式進行軌跡層次的泛化，另一種采用視覺示教方式進行語義層次的泛化，后續將繼續開展所提方法的研究。