基于動態(tài)系統(tǒng)穩(wěn)定估計(jì)器的機(jī)器人變剛度軌跡規(guī)劃

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

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

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

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

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

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

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

1 變剛度的SEDS軌跡規(guī)劃

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

在機(jī)器人模仿學(xué)習(xí)過程中，示教者帶動機(jī)器人完成任務(wù)，采集軌跡信息并建立如下數(shù)據(jù)集：

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

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

為了獲取運(yùn)動過程中伴隨人手臂的剛度信息，實(shí)驗(yàn)中采用MYO肌電手環(huán)采集肌電信號，然后將8個(gè)通道的肌電信號制作成如下數(shù)據(jù)集：

式中：η為狀態(tài)變量，用于表征8 個(gè)通道的肌電信息。

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

2 軌跡學(xué)習(xí)

對于點(diǎn)對點(diǎn)示教學(xué)習(xí)問題，采用動態(tài)系統(tǒng)穩(wěn)定估計(jì)器SEDS對機(jī)器人運(yùn)動進(jìn)行非線性動態(tài)系統(tǒng)建模，采用GMM對控制策略建模，并通過穩(wěn)定性約束確保全局漸近穩(wěn)定性[16]。

2.1 基于統(tǒng)計(jì)方法的高斯混合模型

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

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

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

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

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

得到示教運(yùn)動的動態(tài)系統(tǒng)模型的具體表達(dá)式如下：

2.2 動態(tài)系統(tǒng)全局穩(wěn)定的充分條件

為了保證模型的泛化能力，將其從不同的初始點(diǎn)到達(dá)同一目標(biāo)點(diǎn)的學(xué)習(xí)任務(wù)轉(zhuǎn)化為具有全局收斂性的學(xué)習(xí)任務(wù)，此時(shí)，需要考慮模型的穩(wěn)定性問題。SEDS 中的穩(wěn)定性理論基于二次能量函數(shù)，由Lyapunov 穩(wěn)定性定理推導(dǎo)出保證該系統(tǒng)全局漸近穩(wěn)定的充分條件，并采用二次規(guī)劃求解[17]。

根據(jù)Lyapunov 穩(wěn)定性理論，其在點(diǎn)ξ*處達(dá)到全局漸近穩(wěn)定的充分條件是存在一個(gè)連續(xù)可微的Lyapunov函數(shù)V(ξ):Rd→R，且滿足如下關(guān)系式：

構(gòu)造Lyapunov函數(shù)：

求得保證動態(tài)系統(tǒng)全局穩(wěn)定的充分條件：

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

2.3 動態(tài)系統(tǒng)參數(shù)學(xué)習(xí)

為了得到具有全局漸近穩(wěn)定性的動態(tài)系統(tǒng)，需要確定計(jì)算其未知參數(shù)的方法。動態(tài)系統(tǒng)未知參數(shù)θk={μk,Mk}，k=1，2，…，K；全部參數(shù)θ={θ1,θ2,…,θk}。通過將動態(tài)系統(tǒng)參數(shù)學(xué)習(xí)的過程轉(zhuǎn)化為求解非線性優(yōu)化問題，在保證模型具有全局漸近穩(wěn)定性的條件下，求解優(yōu)化問題來計(jì)算θ的最優(yōu)值。對似然函數(shù)求對數(shù)，然后建立如下求解模型：

約束條件為：

式中：J(θ)為非線性規(guī)劃目標(biāo)函數(shù)；為示教數(shù)據(jù)的總軌跡點(diǎn)數(shù)。

借助基于擬牛頓法的連續(xù)二次規(guī)劃方法[18]可以解決以上優(yōu)化問題，得到具有全局漸近穩(wěn)定性的動態(tài)系統(tǒng)的最優(yōu)參數(shù)θ，實(shí)現(xiàn)對于動態(tài)系統(tǒng)的完整描述。

經(jīng)過SEDS 模仿學(xué)習(xí)得到的仿真結(jié)果如圖2 所示。從圖2可以看出：新的軌跡反映了示教軌跡的趨勢，并且能從各個(gè)位置收斂到目標(biāo)點(diǎn)。

3 剛度學(xué)習(xí)

3.1 剛度映射

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

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

3.2 GMM擬合概率分布

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

式中：L為似然函數(shù)；m為模型參數(shù)的個(gè)數(shù)；e為樣本數(shù)量；SBIC為貝葉斯準(zhǔn)則中用來表征信息量的參數(shù)。

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

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

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

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

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

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

3.3 GMR回歸生成剛度曲線

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

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

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

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

4.1 實(shí)驗(yàn)平臺搭建

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

根據(jù)軌跡和剛度變化的學(xué)習(xí)結(jié)果以及新任務(wù)給出的終點(diǎn)坐標(biāo)，生成新的軌跡與相應(yīng)的剛度，并將其離散為各點(diǎn)的期望位置坐標(biāo)與x，y和z這3個(gè)方向上的剛度，以預(yù)設(shè)的時(shí)間間隔(本實(shí)驗(yàn)為0.1 s)為周期逐一發(fā)送給機(jī)械臂。

4.2 阻抗控制

一般地，串聯(lián)機(jī)械臂的剛體動力學(xué)模型可描述為

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

本實(shí)驗(yàn)機(jī)械臂的控制算法采用阻抗控制器，笛卡爾空間阻抗控制器的控制律為

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

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

4.3 實(shí)驗(yàn)結(jié)果分析

4.3.1 物體放置實(shí)驗(yàn)

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

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

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

4.3.2 抗干擾實(shí)驗(yàn)

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

5 結(jié)論

1)加入剛度控制后，機(jī)器人到達(dá)目標(biāo)點(diǎn)的位置精度較高，能量消耗較少，表明機(jī)械臂學(xué)習(xí)到了人放置物體過程中的軌跡與阻抗變化趨勢。

2)受到外界干擾后，機(jī)械臂會根據(jù)控制器發(fā)出的期望坐標(biāo)位置調(diào)整自身運(yùn)動，最終仍能收斂到目標(biāo)點(diǎn)，具有較強(qiáng)的抗干擾能力。