劉漢東，吳懷宇，鄭秀娟，陳 洋

(1. 武漢科技大學冶金自動化與檢測技術教育部工程研究中心,湖北 武漢,430081 2. 武漢科技大學機器人與智能系統(tǒng)研究院,湖北 武漢,430081)

隨著機械臂技術的飛速發(fā)展,機械臂的應用領域已不僅僅局限在工業(yè)生產(chǎn)中，在醫(yī)療、服務等領域同樣有著大量的需求，伴隨而來的是機械臂逐漸向輕量化、多自由度、高冗余度的方向發(fā)展，這對機械臂的運動功能要求不斷提高，也加大了對機械臂軌跡規(guī)劃的難度[1]。由于機械臂的高維度特性，導致其構型空間的拓撲結構發(fā)生根本性變化，進而許多傳統(tǒng)的軌跡規(guī)劃方法不再適用,一定程度制約了機械臂的發(fā)展與應用。

從人類學習中發(fā)展而來的示教學習機械臂軌跡規(guī)劃的研究逐漸興起，示教學習可以指導機械臂實現(xiàn)軌跡規(guī)劃,從而有效減小了搜尋可行空間的計算量,大大降低了軌跡規(guī)劃的難度；示教學習也可以賦予機械臂一定的學習能力,機械臂通過模仿學習生成相應的運動行為,獲得運動技能,完成特定作業(yè)任務[2]。示教學習方法包括動態(tài)運動基元(dynamic movement primitives,DMP)[3]和概率運動基元[4]等，DMP算法認為復雜運動可以視為一系列運動基元的疊加，應用DMP算法的系統(tǒng)能夠從運動學習的角度將機械臂一次具體運動看作學習樣本,提取其運動基元，對于新的目標點，能夠利用學習獲得的運動基元泛化出相應的軌跡。

機械臂在實際應用中,其周圍的環(huán)境約束往往處于變動之中，比如突如其來的障礙物、客觀因素造成的目標點變動、運動過程中軌跡的偏離等，這些約束可能是由于執(zhí)行器限制或安全限制而產(chǎn)生的，當運動規(guī)劃中需要適應這些限制時，運用DMP會使規(guī)劃的軌跡不可行。Krug等[5]提出了一種模型預測控制方法,該方法能夠約束速度,可確保每個自由度的速度保持在允許的范圍內,但不能確保路徑的形狀保持不變；Karlsson等[6]提出了通過降低速度來提高控制器跟蹤性能、減小跟蹤誤差的算法,但降低速度會導致生成路徑的變形,因此不能直接用來限制速度或加速度；Weitschat等[7]為了實現(xiàn)機器人的靈活運動,提出了一種使用DMP的泛化方法,動態(tài)運動基元被擴展,以保證符合最大速度約束，此方法雖能實現(xiàn)最大速度約束,但沒有考慮機器人的加速度約束,也不能實現(xiàn)對障礙物的避障；Koutras等[8]對DMP框架進行擴展,提出了一種由于移動目標而速度增加的抵消方法,但不允許明確的速度限制,且未考慮加速度約束，不能實現(xiàn)對障礙物的避障。

Chi等[9]將動態(tài)勢場與DMP模型相結合,來實現(xiàn)服務機器人的自主避障功能；Lauretti等[10]提出了一種基于DMP的演示學習方法,使得機器人能夠避開路徑上的障礙物,在關節(jié)空間和笛卡爾空間能夠以類似于人類演示的方式進行運動；Ugur等[11]擴展了DMP的框架,增加一個新的非線性成形函數(shù)和一個新的力反饋耦合項，利用隱馬爾可夫模型參數(shù)對動作控制變量的非線性軌跡和執(zhí)行過程中測得的觸覺反饋軌跡進行編碼；Ginesi等[12]提出了一個基于超二次位勢函數(shù)來表示體積的避障框架。以上幾種方法雖能實現(xiàn)對簡單障礙物的避障,但都未考慮機器人的動力學約束，由此可知，應用動態(tài)運動基元算法的場景大都未考慮機械臂的速度和加速度限制，鮮有學者在研究機械臂避障的同時還考慮機械臂的動力學限制，為此,本文針對傳統(tǒng)機械臂軌跡規(guī)劃方法存在適用性差的問題,利用動態(tài)運動基元的學習與泛化特性規(guī)劃出機械臂的運動軌跡，在動態(tài)運動基元的基礎上,加入改進的耦合項來實現(xiàn)機械臂避障，把對速度及加速度的約束轉化為對時間縮放因子的約束,利用改進的更新律來滿足機械臂動力學約束,從而提出一種基于動力學約束的動態(tài)運動基元避障軌跡規(guī)劃方法，并通過仿真實驗和物理實驗來驗證該方法的可行性，以期為提升機械臂在不同約束下的軌跡規(guī)劃能力與環(huán)境適應性提供參考。

1 動態(tài)運動基元

1.1 動態(tài)運動基元原理

動態(tài)運動基元的基本思想是向簡單且穩(wěn)定的動態(tài)系統(tǒng)引入非線性函數(shù),由非線性函數(shù)控制系統(tǒng)的運動過程，最終使系統(tǒng)達到目標吸引子狀態(tài)。DMP的框架采用帶非線性強迫項的二階彈性系統(tǒng)來描述物體的運動[13-14]。DMP作為一個非線性動力系統(tǒng)，它主要應用于離散和連續(xù)兩種不同的運動模式，對于離散系統(tǒng)是采用點吸引子系統(tǒng)為基本系統(tǒng),而對于連續(xù)系統(tǒng)采用極限環(huán)為基本系統(tǒng)，在這個框架中,物體的運動可以用一組微分方程表示為:

(1)

(2)

(3)

式(1)為轉換系統(tǒng),其中x為系統(tǒng)的位置,g為系統(tǒng)的目標位置,z為系統(tǒng)的速度,τ為時間縮放因子,用于控制系統(tǒng)的時間縮放；β、λ為系統(tǒng)參數(shù),在選擇合適的參數(shù)值下,系統(tǒng)會變成一個臨界阻尼系統(tǒng)；式(3)為規(guī)范系統(tǒng),其中α為常數(shù),s為相位變量，s是關于時間t的函數(shù),引入相位變量弱化了系統(tǒng)對時間明確的依賴性,任意選擇初始狀態(tài)s0,可以證明s的狀態(tài)單調收斂于0。

式(1)中強迫項函數(shù)f(s)是由徑向基核函數(shù)通過線性加權形成的,表示為:

(4)

ψi(s)=exp(-hi(s-ci)2)

(5)

式中，ψi(s)為徑向基核函數(shù)；hi為核函數(shù)的帶寬；ci為核函數(shù)的中心；wi為徑向基核函數(shù)對應的權重值；i為基函數(shù)的數(shù)目,示教軌跡越復雜,需要的基函數(shù)就越多。

為了實現(xiàn)軌跡的學習及規(guī)劃,需要建立示教軌跡與新生成目標軌跡之間的聯(lián)系,即通過觀測學習樣本為強迫項函數(shù)提供權重值wi。參數(shù)wi的學習可以通過局部加權回歸來完成，之所以選擇局部加權回歸,是因為它的一次性學習過程非?？?而且各個內核的學習相互獨立。

記錄一維的示教運動,設整個示教運動的軌跡可以用一組位移、速度和加速度的連續(xù)序列表示,將示教運動作為樣本帶入公式(1)中,得到非線性項為:

(6)

要想完成學習任務,需要找到合適的權重wi。首先將公式(6)中f(s)demo逼近真實樣本模型的強迫項函數(shù)f(s),把公式(4)改寫成線性方程的形式為:

f(s)=Tw

(7)

(8)

w=[w1…wn]T

(9)

然后通過最小二乘準則來求出權重wi的值,即：

(10)

當J取最小值時的wi就是該系統(tǒng)所對應的最佳權重值,在此基礎上,利用計算得到的最佳權重值擬合出運動學習時的位移、速度和加速度值的序列,從而完成學習的過程。若改變物體運動的目標點,DMP能夠利用已經(jīng)學習的權重,反向擬合出新的運動特征序列,規(guī)劃出一條運動軌跡,從而實現(xiàn)泛化推廣的過程,此運動軌跡有與原學習軌跡相似的運動特征。

利用動態(tài)運動基元算法進行機械臂軌跡規(guī)劃分為三部分,包括運動分解與特征提取、軌跡學習、軌跡泛化,其流程圖如圖1所示。

圖1 基于DMP的機械臂軌跡規(guī)劃流程圖

將上述一維的動態(tài)運動基元模型通過不同維度上的組合,可以構成二維平面或三維空間中的運動,從而可以產(chǎn)生復雜的運動軌跡。對于多維度的情況,規(guī)范系統(tǒng)是共享的,而每個維度都有獨立的非線性函數(shù)和轉換系統(tǒng)。

DMP算法的特點是可通過學習參考軌跡的運動信息并對新目標進行泛化,使新生成的軌跡具有與原軌跡相似的運動趨勢,并保證收斂到目標狀態(tài)，同時DMP算法還具有時間項和空間項縮放的優(yōu)點。

1.2 改進的避障耦合項

機械臂一次完整的作業(yè)運動可作為一個學習樣本,機械臂對此樣本的學習可進行不同目標的自主泛化，但DMP的學習特性使得泛化結果會保留樣本的某些特征,例如軌跡的凹凸性,所以不同的樣本對應各自的泛化結果，由此可見，DMP算法對于運動的目標、起始點、持續(xù)時間的變化是具魯棒性的。

將DMP算法應用在機械臂的軌跡規(guī)劃上,除了到達目標點完成作業(yè)任務外,尤其重要的一點是運動過程中能否避開障礙物,即是否能夠根據(jù)先驗知識或傳感器返回信息完成對軌跡的修改。當機械臂運動的環(huán)境中存在障礙物時,原DMP算法規(guī)劃的軌跡不能完全適用。Khatib[15]將物理學中“場”的概念引入機械臂的運動規(guī)劃中,在機械臂工作環(huán)境中建立人工勢場，該勢場由目標點產(chǎn)生的吸引勢場和由障礙物產(chǎn)生的排斥勢場組成，機械臂在勢場產(chǎn)生的合力作用下朝著目標進行無碰撞的避障運動；Fajen等[16]發(fā)現(xiàn)了一種模擬人類避障的微分方程,通過數(shù)學表達式來實現(xiàn)避障,并指出目標對轉彎速度的影響隨著其與運動方向的夾角增大而指數(shù)增大,隨著距離的增大而指數(shù)減??；Hoffmann等[17]在原DMP模型的基礎上加入運動特征與障礙物的耦合項,使運動軌跡發(fā)生變化,將其拓展為具有避障功能的運動規(guī)劃系統(tǒng)：

(11)

Ct=Rμzηexp(-δη)

(12)

(13)

式中，Ct為加入的耦合項；o為障礙物位置；x和z分別為機械臂的位置和速度；η為速度向量z與障礙物中心位置和機械臂當前位置坐標差向量(o-x)的夾角；μ、δ為系統(tǒng)常數(shù)；R為旋轉矩陣,決定避障軌跡繞障礙物的旋轉方向。

利用動力系統(tǒng)對擾動的魯棒性來避障，耦合項作為到障礙物的距離矢量函數(shù),相當于增加了垂直于當前速度方向的運動,使運動軌跡遠離障礙物。式(12)中的耦合項對近距離的障礙物和遠距離的障礙物賦予同樣的地位,這可能導致運動軌跡的振蕩行為，同時，加入耦合項的動態(tài)運動基元算法雖可以避障,但學習后的軌跡與樣本軌跡有較大誤差,不利于機械臂安全地完成作業(yè)任務。

針對以上的不足之處,對避障耦合項進行改進。為了確保改進模型的全局穩(wěn)定性,引入S形狀的logistic函數(shù)為:

(14)

式中，m為曲線某一點的坐標；m0是曲線中點；L代表曲線的峰值；b為曲線的陡度。

假設障礙物的速度為vo，將h(x)加入后,改進后的耦合項為:

(15)

(16)

由此可見，針對動態(tài)障礙物,避障耦合項需要考慮的因素不再是機械臂本身的運動速度,而是機械臂和障礙物之間的相對速度(z-vo)。

2 速度及加速度約束

2.1 速度約束

不考慮動力學限制會使移動機械臂或機器人在執(zhí)行任務時違反安全限制或電機的轉矩限制,可能造成安全事故、損壞移動機械臂或機器人,所以有必要考慮系統(tǒng)軌跡的速度和加速度約束。要確保機械臂在與人類交互時能夠安全地執(zhí)行任務,則要在機械臂的位置和速度隨時間不斷變化的環(huán)境中提供安全交互的框架,約束DMP系統(tǒng)軌跡的速度和加速度分別為:

(17)

(18)

由公式(2)和公式(17)推導得到:

(19)

由于軌跡速度與時間縮放因子τk成正比,因此對系統(tǒng)軌跡速度的進一步預測是可行的,可以將速度約束轉化為對時間縮放因子τk的約束，即：

(20)

2.2 加速度約束

對公式(2)兩邊求導得到:

(21)

進一步推導得到:

(22)

加速度由當前的時間縮放因子及其導數(shù)來決定,要使加速度約束能夠滿足,要求公式(18)成立,經(jīng)過推導得到:

(23)

(24)

2.3 改進的基本更新律

(25)

(26)

上述基本更新律雖然能夠使速度和加速度在連續(xù)時間情況下保持在規(guī)定范圍內,但是生成的軌跡曲線不夠平滑,加速度在某些地方有突變，這不利于實際的機械臂運動軌跡控制，甚至有時τk的極端變化會導致物體的運動軌跡不可行。

針對這些缺點,對原來的基本更新律進行改進,改進之后的基本更新律為:

(27)

(28)

改進的基本更新律包括兩個部分，一部分作為勢函數(shù),其目的是在加速度接近極限時,通過增加τk將加速度保持在約束之內；另一個部分的作用是驅動τk回到標稱的時間縮放因子τ*。當加速度都離約束足夠遠時,公式(27)中的勢函數(shù)項可以忽略不計,基本更新律可以解釋為一階濾波器。

使用改進的基本更新律,軌跡加速度在達到極限之前被減慢,因此可能導致略慢于所需的軌跡加速度，然而,這種處理加速度限制的保守方法為飽和控制器中的反饋項提供了更多容量,可以提高控制器的跟蹤性能。改進的基本更新律可以使所有維度的軌跡以及相位以同步的方式被延遲,路徑保持不變。

3 實驗與結果分析

本文主要研究的是機械臂在二維平面下的運動場景，其運動可以分解為X1和X2兩個方向的運動。采用高階多項式生成訓練軌跡樣本，高階多項式所生成的軌跡及其一階、二階導數(shù)分別對應機械臂末端執(zhí)行器做運動所需要的位移、速度及加速度。根據(jù)DMP算法進行實驗設計,在Matlab中設計仿真實驗,再進行物理實驗。

3.1 避障前后對比仿真

在Matlab的機器人工具箱搭建一個三自由度機械臂的平臺。首先在沒有障礙物的情況下,利用DMP的學習與泛化特點規(guī)劃出一條運動軌跡,然后在機械臂運動的平面中加入兩個靜態(tài)障礙物,驗證其自主避障功能，然后利用改進的耦合項來進行避障實驗。靜態(tài)障礙物避障運動軌跡曲線對比如圖2所示。圖2中黑色圓代表大小不同的兩個靜態(tài)障礙物，從圖2中可以看出,在有障礙物的情況下,機械臂通過加入耦合項的動態(tài)運動基元算法能夠規(guī)劃出一條可行的避障軌跡,且避障軌跡平滑，同時還可以看出，避障耦合項改進前后的機械臂均可進行避障,表明DMP系統(tǒng)具有良好的學習能力與泛化能力。

(a)改進前

(b)改進后

通過改變靜態(tài)障礙物的大小,做20組不同的避障實驗，分別計算每一組避障耦合項改進前后機械臂規(guī)劃的避障軌跡與樣本軌跡的誤差,求取每一組軌跡誤差的平均值，并把計算的20組平均值求和之后再計算均值,并根據(jù)此均值計算軌跡誤差的方差及標準差,避障耦合項改進前后靜態(tài)障礙物避障軌跡誤差如表1所示。從表1中可以看出,與利用原避障耦合項規(guī)劃的軌跡相比,避障耦合項改進后的避障軌跡與樣本軌跡的誤差均值更小,軌跡誤差的方差及標準差均較小,由此表明，避障耦合項改進后的總體軌跡誤差較小,利用改進的避障耦合項在完成避障的同時能夠盡量保持與原始軌跡的一致性。

表1 避障耦合項改進前后靜態(tài)障礙物避障軌跡誤差

將機械臂運動的平面中兩個靜態(tài)障礙物換成一個動態(tài)障礙物后再進行實驗，避障耦合項改進前后動態(tài)障礙物避障運動軌跡曲線對比如圖3所示。圖3中黑色箭頭表示動態(tài)障礙物的運動方向,此動態(tài)障礙物的運動速度設定為0.19 m/s，實線圓表示動態(tài)障礙物開始運動的位置,虛線圓表示動態(tài)障礙物運動過程中的位置。從圖3中可以看出，改進后的避障耦合項生成的運動軌跡可以避開動態(tài)障礙物(見圖3(b)),而利用原始避障耦合項生成的運動軌跡與動態(tài)障礙物會相交,不能避開動態(tài)障礙物(見圖3(a))，由此可見，利用改進的避障耦合項可以完成對動態(tài)障礙物的避障,表明避障耦合項改進后的系統(tǒng)具有良好的避障規(guī)劃能力與環(huán)境適應性。

(a)改進前

3.2 速度約束前后對比仿真

機械臂末端執(zhí)行器在二維平面下的運動分解為X1和X2兩個方向上的運動,相位變量s是關于時間t的函數(shù),為了簡化比較,在橫坐標軸上使用的是相位變量,而不是時間變量。在機械臂規(guī)劃的運動軌跡能夠實現(xiàn)靜態(tài)障礙物避障的基礎上,考慮加入速度限制,X1和X2兩個方向的速度約束絕對值分別設為0.15、0.18 m/s。

速度約束前后系統(tǒng)軌跡速度曲線對比如圖4所示。在實驗的開始,并沒有設置速度約束,在相位變量s=0.8時,加入了速度約束。從圖4中可以看出,未使用基本更新律的DMP系統(tǒng)軌跡速度會超過限制；當使用所提出的基本更新律時,系統(tǒng)在兩個方向上的速度能夠滿足約束,能夠適應在線修改的速度限制,并且在其余部分會遵守新的速度限制。

(a)X1方向

3.3 加速度約束前后對比仿真

在機械臂規(guī)劃的運動軌跡能夠實現(xiàn)靜態(tài)障礙物避障的基礎上,考慮加入加速度限制,對X1和X2兩個方向的加速度限制的絕對值分別設置為0.27、0.26 m/s2，加速度約束前后系統(tǒng)軌跡的加速度曲線對比如圖5所示。從圖5中可以看出,施加運動軌跡的加速度約束后,利用基本更新律的DMP系統(tǒng)在達到加速度限制之前主動縮放軌跡,加速度保持在整個界限內,而未使用基本更新律的DMP系統(tǒng)會違反加速度的限制,加速度超越了上下界線。

(a)X1方向

3.4 基本更新律改進前后對比仿真

在機械臂規(guī)劃的運動軌跡能夠實現(xiàn)靜態(tài)障礙物避障的基礎上,考慮加入加速度限制,對X1和X2兩個方向的加速度約束不同，X1和X2兩個方向的加速度限制的絕對值分別設置為1.05、0.88 m/s2。改進的基本更新律并沒有在極限處大幅度改變τk,而是隨著接近極限而逐漸增加τk，也就是說,當接近極限時,通過選擇τk,運動軌跡速度都會以一種預防的方式減慢，該方法可以通過主動縮放而不是單純的反應性縮放來保持對更大范圍軌跡的微分約束的可行性,從而能夠更大程度地滿足約束。更新律改進前后系統(tǒng)軌跡的加速度曲線對比如圖6所示。從圖6中可以看出，與原來的基本更新律相比,改進的基本更新律在約束系統(tǒng)加速度的同時,加速度曲線更加平緩,波動較小。

(a)X1方向

更新律改進前后加速度峰值如表2所示。從表2中可以看出,利用改進后的基本更新律得到的DMP系統(tǒng)的加速度峰值比改進前的加速度峰值小,其加速度曲線更加平緩。由圖6和表2可以看出,基本更新律改進后不僅使加速度約束更容易滿足,也能獲得更平穩(wěn)的加速度,同時保持生成的軌跡曲線與參考軌跡具有良好的軌跡跟蹤特性。

表2 更新律改進前后加速度峰值

3.5 避障軌跡規(guī)劃物理實驗

將以上仿真分析的系統(tǒng)集成在一個三自由度的移動機械臂平臺上,在移動機械臂上進行真實環(huán)境下的實驗。移動機械臂的上位機使用機器人操作系統(tǒng)(robot operating system,ROS)通信,下位機運動控制卡通過PWM控制機械臂運動。機械臂示教學習及避障過程如圖7所示。首先在沒有障礙物的情況下,利用DMP的學習與泛化特點,使移動機械臂規(guī)劃出一條運動軌跡,如圖7(a)～圖7(c)所示；隨后在環(huán)境中加入靜態(tài)障礙物后重新進行自主軌跡規(guī)劃測試，在機械臂的正下方放置一個圓形凳子作為靜態(tài)障礙物,如圖7(d)～圖7(f)所示。在實驗中加入速度限制,X1和X2兩個方向的速度約束絕對值分別設為0.15、0.18 m/s,利用改進的耦合項來進行避障試驗。移動機械臂進行自主避障規(guī)劃的軌跡曲線如圖8所示,圖中圓表示障礙物。從圖7和圖8中可以看出,在沒有障礙物的情況下,移動機械臂在起始位置開始,從左邊運動至右邊,到達目標位置；在有靜態(tài)障礙物的情況下,移動機械臂的運動軌跡平滑且能避開障礙物完成實驗任務,驗證了所提出的軌跡規(guī)劃方法的有效性,表明了該系統(tǒng)具有一定的避障規(guī)劃能力與環(huán)境適應性。

(a)示教運動開始 (b)運動至中間 (c)示教運動結束

圖8 機械臂避障軌跡曲線

4 結語

(1)利用動態(tài)運動基元的學習與泛化特性規(guī)劃出機械臂的運動軌跡，用改進的耦合項來實現(xiàn)機械臂避障，把對速度及加速度的約束轉化為對時間縮放因子的約束,利用改進的更新律來滿足機械臂動力學約束，提出一種基于動力學約束的動態(tài)運動基元機械臂軌跡規(guī)劃方法。

(2)基于動力學約束的動態(tài)運動基元機械臂軌跡規(guī)劃方法能夠使機械臂在避障的同時,遵守速度或加速度約束,并且速度約束可以在線修改。

(3)與傳統(tǒng)機械臂軌跡規(guī)劃方法相比,基于動力學約束的動態(tài)運動基元機械臂軌跡規(guī)劃方法可以提升機械臂在不同約束下的軌跡規(guī)劃能力與環(huán)境適應性。