基于動(dòng)力學(xué)約束的動(dòng)態(tài)運(yùn)動(dòng)基元機(jī)械臂軌跡規(guī)劃

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

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

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

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

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

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

1 動(dòng)態(tài)運(yùn)動(dòng)基元

1.1 動(dòng)態(tài)運(yùn)動(dòng)基元原理

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

(1)

(2)

(3)

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

式(1)中強(qiáng)迫項(xiàng)函數(shù)f(s)是由徑向基核函數(shù)通過(guò)線(xiàn)性加權(quán)形成的,表示為:

(4)

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

(5)

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

為了實(shí)現(xiàn)軌跡的學(xué)習(xí)及規(guī)劃,需要建立示教軌跡與新生成目標(biāo)軌跡之間的聯(lián)系,即通過(guò)觀(guān)測(cè)學(xué)習(xí)樣本為強(qiáng)迫項(xiàng)函數(shù)提供權(quán)重值wi。參數(shù)wi的學(xué)習(xí)可以通過(guò)局部加權(quán)回歸來(lái)完成，之所以選擇局部加權(quán)回歸,是因?yàn)樗囊淮涡詫W(xué)習(xí)過(guò)程非常快,而且各個(gè)內(nèi)核的學(xué)習(xí)相互獨(dú)立。

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

(6)

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

f(s)=Tw

(7)

(8)

w=[w1…wn]T

(9)

然后通過(guò)最小二乘準(zhǔn)則來(lái)求出權(quán)重wi的值,即：

(10)

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

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

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

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

DMP算法的特點(diǎn)是可通過(guò)學(xué)習(xí)參考軌跡的運(yùn)動(dòng)信息并對(duì)新目標(biāo)進(jìn)行泛化,使新生成的軌跡具有與原軌跡相似的運(yùn)動(dòng)趨勢(shì),并保證收斂到目標(biāo)狀態(tài)，同時(shí)DMP算法還具有時(shí)間項(xiàng)和空間項(xiàng)縮放的優(yōu)點(diǎn)。

1.2 改進(jìn)的避障耦合項(xiàng)

機(jī)械臂一次完整的作業(yè)運(yùn)動(dòng)可作為一個(gè)學(xué)習(xí)樣本,機(jī)械臂對(duì)此樣本的學(xué)習(xí)可進(jìn)行不同目標(biāo)的自主泛化，但DMP的學(xué)習(xí)特性使得泛化結(jié)果會(huì)保留樣本的某些特征,例如軌跡的凹凸性,所以不同的樣本對(duì)應(yīng)各自的泛化結(jié)果，由此可見(jiàn)，DMP算法對(duì)于運(yùn)動(dòng)的目標(biāo)、起始點(diǎn)、持續(xù)時(shí)間的變化是具魯棒性的。

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

(11)

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

(12)

(13)

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

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

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

(14)

式中，m為曲線(xiàn)某一點(diǎn)的坐標(biāo)；m0是曲線(xiàn)中點(diǎn)；L代表曲線(xiàn)的峰值；b為曲線(xiàn)的陡度。

假設(shè)障礙物的速度為vo，將h(x)加入后,改進(jìn)后的耦合項(xiàng)為:

(15)

(16)

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

2 速度及加速度約束

2.1 速度約束

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

(17)

(18)

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

(19)

由于軌跡速度與時(shí)間縮放因子τk成正比,因此對(duì)系統(tǒng)軌跡速度的進(jìn)一步預(yù)測(cè)是可行的,可以將速度約束轉(zhuǎn)化為對(duì)時(shí)間縮放因子τk的約束，即：

(20)

2.2 加速度約束

對(duì)公式(2)兩邊求導(dǎo)得到:

(21)

進(jìn)一步推導(dǎo)得到:

(22)

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

(23)

(24)

2.3 改進(jìn)的基本更新律

(25)

(26)

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

針對(duì)這些缺點(diǎn),對(duì)原來(lái)的基本更新律進(jìn)行改進(jìn),改進(jìn)之后的基本更新律為:

(27)

(28)

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

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

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

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

3.1 避障前后對(duì)比仿真

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

(a)改進(jìn)前

(b)改進(jìn)后

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

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

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

(a)改進(jìn)前

3.2 速度約束前后對(duì)比仿真

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

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

(a)X1方向

3.3 加速度約束前后對(duì)比仿真

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

(a)X1方向

3.4 基本更新律改進(jìn)前后對(duì)比仿真

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

(a)X1方向

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

表2 更新律改進(jìn)前后加速度峰值

3.5 避障軌跡規(guī)劃物理實(shí)驗(yàn)

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

(a)示教運(yùn)動(dòng)開(kāi)始 (b)運(yùn)動(dòng)至中間 (c)示教運(yùn)動(dòng)結(jié)束

圖8 機(jī)械臂避障軌跡曲線(xiàn)

4 結(jié)語(yǔ)

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

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

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