基于動態系統穩定估計器的機器人變剛度軌跡規劃

謝嘯，張涵，湯自林，高霄，肖曉暉

(武漢大學動力與機械學院，湖北武漢，430072)

機器人作為未來制造業提高生產效率、降低生產成本以及提升產品質量的重要工具，在近年來發展迅速。國內外對于機器人技能學習與控制方面的研究也與日俱增[1]。傳統機器人需要使用者根據實際要求進行編程，對于使用者要求較高且任務泛化性差，限制了其應用范圍。示教學習/模仿學習(learning from demonstration/imitation learning)的機器人編程方式極大地降低了技術難度，即使示教者對于機器人沒有深刻認識，也能完成編程過程[2]。

許多模仿學習方法都可以實現學習并還原示教運動軌跡，其中常見的有動態運動基元DMP(dynamic movement primitives)[3-4]、概率運動基元ProMP(probabilistic movement primitives)[5-6]和GMM/GMR[7-11]等。這些算法通過采集示教運動軌跡數據進行建模，但會出現穩定性差、泛化能力以及抗干擾能力較弱等問題。其中，基于DMP 的機器人模仿學習方法僅需1次示教便能以微分方程的形式對運動軌跡進行建模，并將運動模型區分為線性部分與非線性部分，以達到復現相似度與全局穩定性俱佳的效果[12]。但DMP 采用單變量編碼，其在將復雜的動態運動過程進行高效精確編碼的同時，也丟失了各自由度之間的信息，導致其穩定性較差。

為了克服DMP 的不足之處，KHANSARIZADEH 等[13]提出了一種基于動態系統穩定估計器(SEDS)的方法。該法首先采用高斯混合模型GMM和高斯混合回歸GMR將示教軌跡建模成動態系統模型；然后，建立動態系統全局穩定的約束方程；最后，求解包含約束的非線性優化問題，得到最優參數以完成對動態系統的建模。這種方法能很好地將從示教運動學習得到的動態系統模型軌跡平滑收斂到目標點，并且在未被示教的狀態空間中也能生成相似的軌跡至目標點，具有良好的穩定性和泛化能力。但是，基于SEDS的模仿學習方法只能得到軌跡信息，并未考慮到在實際抓取和放置過程中人的阻抗特性。許金鵬等[14]引入多個規劃點來校正物體的中間位置，以提高放置精度，但同時也帶來了機械臂運動不自然、不連貫等問題。另外，考慮到人機交互中的安全性，機械臂常使用阻抗控制，其中控制器的剛度不僅會影響機器人到達目標點的位置精度，也會改變其放置物體過程中的能量消耗，是一個不可忽視的參數。

針對剛度的學習問題，YANG等[15]結合了基于EMG(electromyographic)的變阻抗控制和DMP模型的優點，實現了人-機器人技能轉移。首先，將人的上肢肌電圖信號提取出來用以獲得其肌肉剛度特征，以此作為阻抗控制器的可變增益；然后利用DMP 同時對軌跡和剛度信息進行分析；最終根據實際情況生成更符合人類特性的機器人運動過程。

本文基于物體放置場景中的位置精度以及能量消耗這2 項要求，首先利用SEDS 對示教運動進行建模，生成參考軌跡，然后結合GMM/GMR 學習到的人放置物體的剛度信息，實現機械臂的阻抗控制，使得其在放置時的位置精度較高、抗干擾能力較強，整個過程中的能量消耗減小。

1 變剛度的SEDS軌跡規劃

變剛度的SEDS 軌跡規劃的具體流程如圖1 所示。圖1中，示教過程中的軌跡信息和肌電信號由相應傳感器采集并存儲，而后分別對采集到的數據進行對齊處理。軌跡信息采用SEDS模仿學習方法，先對動態系統建模，確定約束條件后求得動態系統的最優參數并建立基于狀態反饋的模型，從而得到新任務的軌跡。剛度學習則是先找到人上肢末端肌電信號和剛度的映射關系，將預處理后的肌電信號映射到剛度空間，通過GMM擬合得到運動過程中剛度平均值的概率分布，再通過GMR 進行回歸，最終得到機器人運動時的剛度變化曲線。軌跡和剛度協同作用可以使機器人更好地模仿人類并執行放置任務，提高物體放置的位置精度，減少能量消耗。

在機器人模仿學習過程中，示教者帶動機器人完成任務，采集軌跡信息并建立如下數據集：

式中：ξ為狀態變量，用于表征機械臂末端的位置信息；n為1 個示教任務的示教次數，n=1，2，…，N；t為1個示教任務內的時間戳，t=0，1，2，…，Tn。對于示教軌跡數據，一般使用一階動態系統來建模，其一階自洽常微分方程可表示為

式中：f:Rd→Rd，為1 個具有單平衡點的非線性連續可微函數；Rd為d維向量張成的空間；R為實數集；ε為高斯噪聲項，其均值為0，包含了傳感器測量中的不確定性和誤差。

為了獲取運動過程中伴隨人手臂的剛度信息，實驗中采用MYO肌電手環采集肌電信號，然后將8個通道的肌電信號制作成如下數據集：

式中：η為狀態變量，用于表征8 個通道的肌電信息。

將ξ和η這2 個數據集作為輸入，使機器人在模仿學習過程中同時具備使軌跡和剛度變化的能力。在實際場景中，對于運動軌跡和剛度變化示教，示教者在拖動機械臂完成1次放置物體過程中的運動軌跡和手臂上的肌電信號均被采集用于模仿學習。

2 軌跡學習

對于點對點示教學習問題，采用動態系統穩定估計器SEDS對機器人運動進行非線性動態系統建模，采用GMM對控制策略建模，并通過穩定性約束確保全局漸近穩定性[16]。

2.1 基于統計方法的高斯混合模型

通過基于統計方法的高斯混合模型(GMM)建立對于動態系統的概率估計模型，得到動態系統的具體表達式。對于1組示教次數為N的示教軌跡數據集GMM 的概率密度函數表達式如下：

式中：P(ξt,n,;θ)為高斯過程概率密度函數；d為數據維度；k為高斯分布的數量，k=1，2，…，K；α(k)為觀測數據屬于第k個高斯子模型的概率，且有為第k個高斯分量的待確定參數，且有θ={θ1,θ2,…,θk}，k=1，2，…，K；φ(ξt,n,∣θk)為第k個子模型的高斯分布密度函數；μk為該密度函數的均值向量；Mk為協方差矩陣。

由條件期望求得整個高斯混合模型中ξ的總期望值為

式中：i=1，2，…，K。

通過換元對式(7)進行簡化：

得到示教運動的動態系統模型的具體表達式如下：

2.2 動態系統全局穩定的充分條件

為了保證模型的泛化能力，將其從不同的初始點到達同一目標點的學習任務轉化為具有全局收斂性的學習任務，此時，需要考慮模型的穩定性問題。SEDS 中的穩定性理論基于二次能量函數，由Lyapunov 穩定性定理推導出保證該系統全局漸近穩定的充分條件，并采用二次規劃求解[17]。

根據Lyapunov 穩定性理論，其在點ξ*處達到全局漸近穩定的充分條件是存在一個連續可微的Lyapunov函數V(ξ):Rd→R，且滿足如下關系式：

構造Lyapunov函數：

求得保證動態系統全局穩定的充分條件：

式中：k= 1, 2,…,K；?指矩陣的負定性。

2.3 動態系統參數學習

為了得到具有全局漸近穩定性的動態系統，需要確定計算其未知參數的方法。動態系統未知參數θk={μk,Mk}，k=1，2，…，K；全部參數θ={θ1,θ2,…,θk}。通過將動態系統參數學習的過程轉化為求解非線性優化問題，在保證模型具有全局漸近穩定性的條件下，求解優化問題來計算θ的最優值。對似然函數求對數，然后建立如下求解模型：

約束條件為：

式中：J(θ)為非線性規劃目標函數；為示教數據的總軌跡點數。

借助基于擬牛頓法的連續二次規劃方法[18]可以解決以上優化問題，得到具有全局漸近穩定性的動態系統的最優參數θ，實現對于動態系統的完整描述。

經過SEDS 模仿學習得到的仿真結果如圖2 所示。從圖2可以看出：新的軌跡反映了示教軌跡的趨勢，并且能從各個位置收斂到目標點。

3 剛度學習

3.1 剛度映射

人體手臂的關節剛度與肌肉收縮活動、人體手臂姿勢、肌肉拉伸反射等因素有關。本文采用一種經過驗證的、簡化的人體手臂關節剛度估計模型[19-20]對示教者放置物體過程中的肌電信號進行剛度映射，并參考彭濤[21]提出的方法，計算得到肌電信號和剛度之間的映射矩陣為

具體的映射對應關系如圖3 所示。圖3(a)所示為8個通道的肌電信號值，圖3(b)所示為通過映射矩陣求出的x，y和z方向的剛度變化曲線，可以看出3 個方向的剛度Kx，Ky和Kz具有相同的變化趨勢，且x和y方向的剛度較為接近，z方向剛度較小，也符合人手臂的實際情況。

3.2 GMM擬合概率分布

在高斯混合模型中，混合高斯分量個數K的選取對數據擬合精度具有重要的影響。一般采用貝葉斯信息準則BIC(Bayesian information criterion)估計K的最佳值：

式中：L為似然函數；m為模型參數的個數；e為樣本數量；SBIC為貝葉斯準則中用來表征信息量的參數。

在x方向上，不同K對應的SBIC如圖4所示。由圖4可見：當3＜K≤5時，SBIC下降趨勢變緩；當K＞5時，SBIC呈明顯的上升趨勢，不符合要求。結合實驗測試結果，本文選取K= 5。在y和z方向上，SBIC的變化趨勢與其在x方向上的一致，故均取K= 5。

GMM 的未知參數包括μk和Mk，最大期望算法EM(expectation-maximum)可以用來計算參數的解。

EM算法通過不斷建立最大似然函數的下界(E步驟)并優化下界(M 步驟)，通過迭代來估計函數的最大值。首先需要初始化模型參數，然后逐步進行E步驟和M步驟的迭代計算。

1)E 步驟，求最大似然函數期望值，以Kx為例進行說明。由當前參數計算x方向上第j個剛度=1，2，…，D)來自第k個子模型的可能性γjk，

2)M 步驟，求最大似然函數極大值。更新下一輪迭代的模型參數：

由EMG 信號與剛度的對應關系，通過轉換矩陣可以將示教者放置物體過程中采集到的肌電信號映射成剛度變化曲線，繼而通過GMM擬合得到圖5(a)，(c)和(e)。

3.3 GMR回歸生成剛度曲線

GMR 方法首先需要計算數據集中樣本之間的聯合概率分布f～θ(μk,Mk)，再根據需要預測的f*的先驗概率分布f*～θ(μk*,Mk*)來計算f*的后驗概率分布。與其他回歸算法相比，GMR 不需要直接建立回歸函數，計算速度快且輸入和輸出都可以是多維的。

對所得的GMM 模型采用GMR 回歸擬合得到的最大概率曲線如圖5(b)，(d)和(f)所示。由圖5可見：0～2 s 為夾取物體階段，剛度較穩定；2～10 s為放置物體階段，為保證到達目標點的位置精度，剛度逐漸增大；在10～12 s 時，為了減小物體與平面接觸的沖擊力，剛度迅速減小。

4 實驗

為了驗證變剛度的SEDS算法的有效性，本文設計基于模仿學習的機械臂執行物體放置的實驗。首先，加入變剛度阻抗控制，并將機械臂的位置精度和能量消耗與定剛度條件下的相應參數進行對比，之后加入位置擾動以驗證SEDS 軌跡的收斂性。

4.1 實驗平臺搭建

物體放置實驗平臺如圖6 所示。機械臂選用Franka Emika 公司的Panda 協作機械臂，為基于力矩控制的7 自由度機械臂。操作員將MYO 肌電手環佩戴在大臂上，手環內圈的感應器緊貼大臂肌肉，拖動機械臂末端示教物體放置過程，采集肌電信號，采用GMR 回歸生成該過程的剛度曲線，作為機械臂阻抗控制的參數。以2 個質量為500 g的砝碼作為機械臂末端額外負載，用于能量消耗對比實驗。將世界坐標系原點固定在實驗臺桌面上，建立如圖6所示的右手笛卡爾坐標系。

根據軌跡和剛度變化的學習結果以及新任務給出的終點坐標，生成新的軌跡與相應的剛度，并將其離散為各點的期望位置坐標與x，y和z這3個方向上的剛度，以預設的時間間隔(本實驗為0.1 s)為周期逐一發送給機械臂。

4.2 阻抗控制

一般地，串聯機械臂的剛體動力學模型可描述為

式中：q，和∈Rn，分別為關節位置、速度和加速度矢量；G(q)∈Rn×n，為質量矩陣；c(q,)∈Rn，為科里奧利力(Coriolis)矩陣；g(q)∈Rn，為重力項；J(q)為正運動學的雅克比矩陣；fext∈R6，為笛卡爾空間的接觸外力/力矩；τ∈Rn，為電機控制力矩。

本實驗機械臂的控制算法采用阻抗控制器，笛卡爾空間阻抗控制器的控制律為

由式(22)可知機械臂與外界的交互關系可用彈簧-阻尼系統模型表征，若無外力，則軌跡將收斂至期望軌跡。本文考慮參考姿態和當前姿態的最短距離(四元數的測地線距離)，該方法相較于由歐拉角直接求差所得的姿態計算精度更高。

阻抗控制框架如圖7所示。采集多組人工示教數據，其中軌跡信息用于SEDS軌跡學習，肌電信號用于剛度學習。軌跡規劃階段基于當前位置反饋更新參考信號，結合剛度信息作為阻抗控制器參數，發送給機械臂，以完成期望的操作任務。

4.3 實驗結果分析

4.3.1 物體放置實驗

由于世界坐標系中的z方向為物體放置方向，位置精度要求相對較低，故本文主要研究x和y方向上阻抗控制器剛度參數對物體放置點位置精度的影響。由3.3節可知2個方向剛度變化趨勢一致，故以y方向為例進行分析。圖8(a)所示為當軌跡相同時，定剛度和變剛度控制下機械臂在y方向上到達目標終點的位置誤差對比。在定剛度控制條件下，以100 N/m為增幅，設定剛度梯度，模型剛度變化范圍為100～1 000 N/m。而變剛度模型則根據示教數據擬合出的剛度變化曲線進行離散處理，將離散后的剛度作為阻抗控制器參數進行實驗。在每種剛度下進行20次實驗，誤差取平均值。從圖8可以看出在變剛度條件下機械臂末端到達目標點時的位置誤差為0.007 6 m，介于700 N/m和800 N/m定剛度下所得位置誤差之間，相較于定剛度100 N/m下的位置誤差(0.034 7 m)，減小了78.1%。

對機械臂末端施加不同的負載(無負載，500 g，1 000 g，1 500 g)，對比定剛度和變剛度阻抗控制時機械臂所產生的能耗，實驗結果表明其變化趨勢一致，故以500 g 負載條件為例進行分析，每種剛度條件下進行20 次實驗，能耗取平均值，結果如圖8(b)所示。由圖8(b)可以看出：變剛度條件下機械臂能耗為0.036 J，介于定剛度600 N/m 和700 N/m 下機械臂的能耗之間，相較于定剛度下(1 000 N/m)的0.042 9 J，能耗減小16.1%。

綜合上述2 個實驗結果，假設存在1 個定剛度，若該條件下的位置誤差比變剛度條件下的小，其剛度應大于700 N/m；若該條件下的能耗比變剛度條件下的小，其剛度應小于700 N/m。這兩者相矛盾，故不存在這樣一種定剛度。由此可以看出相較于定剛度運動，變剛度運動在位置精度和能量消耗之間可取得較好的平衡。

4.3.2 抗干擾實驗

在原實驗的基礎上，對機械臂添加位置上的干擾以測試模型的魯棒性。在無干擾和引入干擾條件下，機械臂在x，y和z這3個方向上的軌跡曲線如圖9 所示。在t≈2.5 s 時(圖9 所示紅色線段)，操作員沿y軸正方向推動機械臂，以改變其末端位置，約1.5 s后停止推動，如圖9(b)所示。從圖9中灰色區域可以看出：機械臂末端y方向坐標值發生了較大變化，x和z方向坐標值無明顯變化；當外力撤去后，在基于SEDS 的運動規劃算法的作用下，機械臂末端重新收斂到期望終點。

5 結論

1)加入剛度控制后，機器人到達目標點的位置精度較高，能量消耗較少，表明機械臂學習到了人放置物體過程中的軌跡與阻抗變化趨勢。

2)受到外界干擾后，機械臂會根據控制器發出的期望坐標位置調整自身運動，最終仍能收斂到目標點，具有較強的抗干擾能力。