基于導(dǎo)納的機(jī)械臂實(shí)時(shí)柔順軌跡規(guī)劃

王慶輝,江 超,2

(1.沈陽化工大學(xué)信息工程學(xué)院,沈陽 110142;2.中國科學(xué)院沈陽自動(dòng)化研究所,沈陽 110016)

0 引言

機(jī)械臂軌跡規(guī)劃問題一直是機(jī)械臂運(yùn)動(dòng)控制研究的重點(diǎn)內(nèi)容,軌跡規(guī)劃的效果對(duì)機(jī)械臂工作的穩(wěn)定性、安全性和能量消耗等有重大影響,因此國內(nèi)外學(xué)者針對(duì)機(jī)械臂的使用場(chǎng)合提出了多種軌跡規(guī)劃方法。林威等[1]提出了利用拋物線速度平滑的笛卡爾空間直線和圓弧軌跡插補(bǔ)算法。為了進(jìn)一步保證軌跡連續(xù)性,減少運(yùn)動(dòng)沖擊,潘海鴻等[2]提出對(duì)機(jī)器人笛卡爾空間軌跡進(jìn)行等距粗插補(bǔ)以及加減速預(yù)處理,再使用Hermite插值法對(duì)各關(guān)節(jié)運(yùn)動(dòng)進(jìn)行軌跡規(guī)劃。SU、RIAZI等[3-5]在基本插值曲線的基礎(chǔ)上,使用優(yōu)化算法求解時(shí)間、能耗和沖擊等優(yōu)化目標(biāo)的最小值,以此優(yōu)化機(jī)械臂的運(yùn)行軌跡。

上述研究只是根據(jù)任務(wù)需求計(jì)算得到機(jī)械臂時(shí)間、連續(xù)性等方面最優(yōu)的離線式軌跡,而未考慮機(jī)械臂實(shí)際作業(yè)時(shí)控制、測(cè)量誤差和環(huán)境不確定因素的存在,機(jī)械臂末端可能會(huì)與環(huán)境發(fā)生碰撞造成損壞。離線式軌跡規(guī)劃期望的是高精度位置控制,體現(xiàn)的是剛性需求。而在機(jī)械臂與外界環(huán)境發(fā)生接觸時(shí)又期望機(jī)械臂表現(xiàn)一定的順應(yīng)性,使得接觸力不至于過大,體現(xiàn)為柔性需求,為了解決這一矛盾,大量國內(nèi)外學(xué)者開展了柔順控制研究。

目前柔順控制分為被動(dòng)柔順與主動(dòng)柔順兩種。被動(dòng)柔順是通過設(shè)計(jì)特殊的柔性機(jī)械裝置(remote center compliance,RCC)來實(shí)現(xiàn)機(jī)器人末端與環(huán)境柔順接觸[6],具有不改變?cè)熊壽E規(guī)劃的同時(shí)實(shí)現(xiàn)柔順控制的優(yōu)點(diǎn), 但被動(dòng)柔順裝置存在專用型強(qiáng),適用范圍小等問題。主動(dòng)柔順是通過設(shè)計(jì)合理的控制策略來處理好力與位置控制二者的關(guān)系以達(dá)到一定的柔順效果。REIBERT等[7]提出了力/位混合控制方法,該方法基于柔順系數(shù)矩陣將機(jī)器人任務(wù)分解為位置控制和力控制兩個(gè)正交子空間,直接控制接觸力跟隨期望值變化,但控制器的結(jié)構(gòu)依賴于機(jī)器人與環(huán)境的動(dòng)力學(xué)特征和運(yùn)動(dòng)學(xué)結(jié)構(gòu),有較大的應(yīng)用限制[8]。HOGAN、KAZEROONI等[9-10]提出阻抗控制和導(dǎo)納控制方法,基于虛擬慣量阻尼剛度系統(tǒng)建立了末端位置與接觸力之間的動(dòng)力學(xué)關(guān)系式。但文中未有研究軌跡規(guī)劃與柔順控制內(nèi)在聯(lián)系,限制了柔順控制在實(shí)際工程的應(yīng)用。金玉陽等[11]提出了通過分析接觸力誤差來動(dòng)態(tài)調(diào)整控制模型參數(shù)的阻抗控制方法,引入自適應(yīng)控制策略,提高了系統(tǒng)穩(wěn)定性。但該方法仍處于仿真階段,尚不能成熟地運(yùn)用到實(shí)際作業(yè)。

針對(duì)以上問題,本文在軌跡規(guī)劃的基礎(chǔ)上,構(gòu)建導(dǎo)納控制器以感知接觸力并計(jì)算得到位姿偏差,以此來實(shí)時(shí)調(diào)整軌跡規(guī)劃的預(yù)期軌跡,將二者納入統(tǒng)一位置控制體系,從而達(dá)到實(shí)時(shí)柔順的軌跡規(guī)劃效果。

1 實(shí)時(shí)柔順軌跡規(guī)劃原理

1.1 軌跡規(guī)劃

軌跡規(guī)劃可以拆解為幾何路徑規(guī)劃與速度規(guī)劃:速度規(guī)劃是由一個(gè)時(shí)間標(biāo)度參數(shù)s(t)表示,其為t∈[0,T]區(qū)間內(nèi)每個(gè)時(shí)刻對(duì)應(yīng)的值s,既s:[0,T]→[0,1]。另外為使得機(jī)器人運(yùn)動(dòng)流暢連續(xù),時(shí)間標(biāo)度s(t)需要適當(dāng)平滑,即二階導(dǎo)連續(xù),常見的方法有三次多項(xiàng)式法、五次多項(xiàng)式、梯形速度曲線、S型速度曲線[12];幾何路徑規(guī)劃由路徑參數(shù)θ(s)表示,其是將標(biāo)量參數(shù)s(s在路徑開始處為0,在路徑結(jié)束處為1)映射到機(jī)器人位形空間Θ中的一點(diǎn):θ:[0,1]→Θ。隨著s從0變化到1,機(jī)器人沿著路徑移動(dòng)[13]。幾何路徑軌跡主要研究笛卡爾空間位姿規(guī)劃,其包括直線插補(bǔ)運(yùn)動(dòng)和圓弧插補(bǔ)運(yùn)動(dòng)的位置規(guī)劃與姿態(tài)規(guī)劃,下面將分別討論。

1.1.1 位置規(guī)劃

(1)直線運(yùn)動(dòng)。假設(shè)直線運(yùn)動(dòng)的起點(diǎn)和終點(diǎn)分別為P1(x1,y1,z1)、P2(x2,y2,z2),則各插補(bǔ)點(diǎn)的表達(dá)式為:

(1)

式中:s(t)為兩階可微的時(shí)間標(biāo)度參數(shù)曲線s:[0,T]→[0,1],以保證直線運(yùn)動(dòng)過程連續(xù)且平滑。

(2)圓弧運(yùn)動(dòng)。由笛卡爾空間任意不共線的3點(diǎn)P1(x1,y1,z1)、P2(x2,y2,z2)、P3(x3,y3,z3)可確定一個(gè)圓弧軌跡,求解過程如下:

建立空間圓弧所在平面的新坐標(biāo)系P1-UVW,從而將三維空間問題變化為二維空間問題,其中以P1為坐標(biāo)原點(diǎn),P1P2為U軸,圓弧所在平面的法向量為W軸,再通過U軸與W軸叉乘得到的向量為V軸,如圖1所示,具體計(jì)算如下:

圖1 P1-UVW坐標(biāo)系示意圖圖2 P1-UVW坐標(biāo)系下的圓心位置

(2)

其中兩向量叉乘計(jì)算如下:

假設(shè)P1、P2、P3在新坐標(biāo)系P1-UVW中表示為A、B、C點(diǎn),則可以通過向量點(diǎn)積分別求得A、B、C點(diǎn)坐標(biāo):

(3)

根據(jù)P1P2是圓弧上的弦可知,圓弧圓心一定位于x=bx/2上,如圖2所示,因此設(shè)圓弧圓心坐標(biāo)為(bx/2,h,0)。根據(jù)平面圓的標(biāo)準(zhǔn)方程可得:

(4)

計(jì)算出P1-UVW坐標(biāo)系下表示的P0圓心坐標(biāo)后,即可算出點(diǎn)P0圓心在空間基座標(biāo)系下的位置表示:

P0=[x1,y1,z1]T+(bx/2)*u+h*v

(5)

同時(shí)可計(jì)算出圓弧半徑R:

(6)

以圓心P0為坐標(biāo)系原點(diǎn),以P0P1為U軸,圓弧所在平面的法向量為W軸,再通過U軸與W軸叉乘得到的向量為V軸建立新坐標(biāo)系P0-UVW,如圖3所示,則方向余弦計(jì)算如下:

圖3 P0-UVW坐標(biāo)系示意圖

(7)

令p0=[x0,y0,z0]T表示為空間基座標(biāo)系{B}下的P0位置,則空間基座標(biāo)系{B}到局部坐標(biāo)系{P0}的齊次變換矩陣T為:

(8)

設(shè)空間任意一點(diǎn)P在空間基座標(biāo)下的齊次坐標(biāo)為[x,y,z,1]T、在局部坐標(biāo)系P0-UVW下的齊次坐標(biāo)為[x′,y′,z′,1]T,則存在以下關(guān)系:

(9)

(10)

則圓弧運(yùn)動(dòng)各插補(bǔ)點(diǎn)表達(dá)式為:

(11)

式中:s(t)為時(shí)間標(biāo)度參數(shù),對(duì)其進(jìn)行從0～1的速度規(guī)劃即可完成圓弧插補(bǔ)。

1.1.2 姿態(tài)規(guī)劃

單位四元數(shù)具有計(jì)算量小,無萬向鎖問題等優(yōu)點(diǎn),因此廣泛用于姿態(tài)插值。設(shè)四元數(shù)的表達(dá)式為:

q=[q1,q2,q3,q4]

(12)

旋轉(zhuǎn)矩陣表達(dá)式為:

(13)

則存在以下關(guān)系式:

(14)

假設(shè)軌跡起始姿態(tài)與終止姿態(tài)分別為旋轉(zhuǎn)矩陣Rs和Re,根據(jù)式(14)可求出其對(duì)應(yīng)的單位四元數(shù)qs和qe。定義兩個(gè)單位四元數(shù)矢量的夾角為θ,則:

θ=cos(qs*qe)-1

(15)

對(duì)于起始姿態(tài)到終止姿態(tài)的插值可由球面線性插值公式Slerp[14]求出:

(16)

式中:s(t)為時(shí)間標(biāo)度參數(shù),對(duì)其進(jìn)行從0～1的速度規(guī)劃即可完成姿態(tài)插補(bǔ)。

1.2 導(dǎo)納控制

導(dǎo)納控制本質(zhì)是內(nèi)環(huán)位置控制和外環(huán)力控制的一種控制策略。根據(jù)當(dāng)前力傳感器檢測(cè)的環(huán)境接觸力信息以及設(shè)定的導(dǎo)納模型參數(shù),計(jì)算位姿附加量,此位姿附加量再去修正預(yù)先設(shè)定的軌跡位姿,最終將調(diào)整后的目標(biāo)位姿進(jìn)行逆運(yùn)動(dòng)學(xué)解算得到機(jī)械臂各關(guān)節(jié)值,并將此值送入位置控制內(nèi)環(huán),完成最終的位置控制,以實(shí)現(xiàn)機(jī)器人末端位置對(duì)外力的主動(dòng)柔順。其中導(dǎo)納控制模型可等效為一個(gè)彈簧阻尼剛度系統(tǒng),該系統(tǒng)的動(dòng)力學(xué)方程可以表示為:

(17)

由于導(dǎo)納控制時(shí)需要實(shí)時(shí)根據(jù)環(huán)境接觸力調(diào)整預(yù)期軌跡,因此可將(17)式寫為如下形式:

(18)

進(jìn)一步通過積分可以得到期望軌跡位姿的附加量Xe:

(19)

(20)

另外可以通過調(diào)節(jié)式(18)中M、B、K參數(shù)來改變導(dǎo)納系統(tǒng)特性,使之得到最優(yōu)的控制性能。因此不妨將式(18)進(jìn)行拉氏變換以獲得導(dǎo)納控制系統(tǒng)的傳遞函數(shù):

(21)

由式(21)可以看出導(dǎo)納控制器等價(jià)于一個(gè)二階系統(tǒng),因此可以把式(21)進(jìn)一步變化為典型二階系統(tǒng)表達(dá)式:

(22)

1.3 實(shí)時(shí)柔順軌跡規(guī)劃

由1.1節(jié)軌跡規(guī)劃部分可以得到以足夠光滑的速度曲線從起始位姿到目標(biāo)位姿的預(yù)期軌跡,由1.2節(jié)導(dǎo)納控制部分可以得到由機(jī)械臂末端與環(huán)境接觸產(chǎn)生的接觸力/力矩計(jì)算得到的位姿修正量,因此系統(tǒng)由位置控制內(nèi)環(huán)和導(dǎo)納控制力外環(huán)兩部分組成,將力信號(hào)轉(zhuǎn)換為位置信號(hào)融入統(tǒng)一位置控制中,從而實(shí)現(xiàn)實(shí)時(shí)柔順軌跡規(guī)劃效果,總控制框圖如圖4所示。

圖4 實(shí)時(shí)柔順控制框圖

具體來說,通過式(18)～式(20),以及根據(jù)機(jī)械臂末端與環(huán)境的接觸力/力矩Fe和導(dǎo)納模型參數(shù)(M、B和K)系統(tǒng),導(dǎo)納控制外環(huán)可產(chǎn)生位姿修正量Xe,與軌跡規(guī)劃計(jì)算的期望機(jī)械臂末端位姿Xd比較得到參考末端位姿:

Xr=Xd+Xe

(23)

其中當(dāng)機(jī)械臂末端未與環(huán)境發(fā)生接觸時(shí),末端作用力Fe=0,對(duì)應(yīng)計(jì)算的末端位姿修正量Xe=0,根據(jù)式(23)得Xr=Xd,此時(shí)機(jī)械臂的控制狀態(tài)等效于常規(guī)的軌跡規(guī)劃位置控制;當(dāng)機(jī)械臂末端與環(huán)境發(fā)生接觸時(shí),末端作用力Fe≠0,同時(shí)對(duì)應(yīng)的末端位姿修正量Xe≠0,Xr=Xd+Xe,此時(shí)機(jī)械臂處于導(dǎo)納控制狀態(tài)。

2 實(shí)驗(yàn)設(shè)計(jì)與結(jié)果

本實(shí)驗(yàn)采用末端自帶六維力傳感器的UR5e六自由度協(xié)作機(jī)械臂作為實(shí)驗(yàn)主體;控制器采用搭載Linux實(shí)時(shí)操作系統(tǒng)與本文對(duì)應(yīng)算法程序的工控機(jī);控制器接收機(jī)械臂上傳的各關(guān)節(jié)位置與力傳感器信息,計(jì)算出各關(guān)節(jié)下一伺服周期的位置,并將此關(guān)節(jié)值發(fā)送回機(jī)械臂伺服,以達(dá)到實(shí)時(shí)柔順軌跡規(guī)劃效果,其中二者信息交互周期為2 ms。整體實(shí)驗(yàn)結(jié)構(gòu)如圖5所示。選取機(jī)械臂各關(guān)節(jié)角為(0°,-133°,-110°,-27°,90°,0°)作為實(shí)驗(yàn)起始狀態(tài),其對(duì)應(yīng)的上位機(jī)軟件坐標(biāo)系可視化示意圖如圖6所示,并設(shè)定導(dǎo)納模型各自由度參數(shù)分別為M=30,B=173,K=250。

圖5 實(shí)驗(yàn)設(shè)備 圖6 機(jī)械臂坐標(biāo)系示意圖

2.1 實(shí)時(shí)柔順的直線運(yùn)動(dòng)實(shí)驗(yàn)

末端執(zhí)行器的運(yùn)動(dòng)軌跡為直線,其中直線的起始點(diǎn)和終止點(diǎn)在起始狀態(tài)的末端工具坐標(biāo)系下位置分別為[0,0,0]T、[0,-0.3,0]T,姿態(tài)保持不變。同時(shí)為了驗(yàn)證本文控制策略有效性,在直線運(yùn)動(dòng)過程中,人為地在軌跡前進(jìn)方向的左側(cè)方放置球形障礙物,使之與機(jī)械臂末端發(fā)生接觸碰撞,通過檢測(cè)接觸力大小與軌跡運(yùn)行狀態(tài),評(píng)估控制方法表現(xiàn)。

圖7和圖8為直線運(yùn)動(dòng)過程中所受的接觸力與力矩曲線。圖9和圖10為機(jī)械臂末端位姿曲線。從圖7機(jī)械臂末端在x軸與y軸方向的接觸力曲線中可以清晰地看出,機(jī)械臂在1.9 s時(shí)與障礙物開始發(fā)生碰撞接觸,但由于導(dǎo)納控制器的作用,機(jī)械臂末端軌跡順應(yīng)著接觸力移動(dòng),其中碰撞過程中除x軸與y軸接觸力的其他自由度受到的力較小,位姿修正量也很小,x軸與y軸方向的最大接觸力絕對(duì)值分別為6.4 N和8.84 N,在允許范圍內(nèi),由此可見在本文所提出的控制方法作用下避免了由軌跡誤差過大而造成的關(guān)節(jié)輸出力矩激增問題。8.29 s后機(jī)械臂開始與障礙物脫離,接觸力與力矩緩慢減小直至為0,機(jī)械臂末端重新回到期望直線軌跡上。

圖7 直線過程所受接觸力 圖8 直線過程所受接觸力矩

圖9 直線過程中的位置 圖10 直線過程中的姿態(tài)

圖11為機(jī)械臂直線運(yùn)動(dòng)開始時(shí)狀態(tài)和結(jié)束時(shí)狀態(tài)。

圖11 直線運(yùn)動(dòng)開始與結(jié)束狀態(tài)

2.2 實(shí)時(shí)柔順的圓弧運(yùn)動(dòng)實(shí)驗(yàn)

末端執(zhí)行器的運(yùn)動(dòng)軌跡為圓弧,其中圓弧的起始點(diǎn)、中間點(diǎn)和終止點(diǎn)在起始狀態(tài)的末端工具坐標(biāo)系下位置分別為[0,0,0]T、[-0.1,-0.1,0]T、[0,-0.2,0]T,姿態(tài)以歐拉角形式表示為[0,0,0]T、[0,0,0]T、[0,0,90°]T。與直線實(shí)驗(yàn)類似,在圓弧軌跡中間位置處放置球形障礙物,使之與機(jī)械臂末端發(fā)生碰撞接觸,然后檢測(cè)接觸力大小與軌跡運(yùn)行狀態(tài),以評(píng)判控制算法有效性。

實(shí)驗(yàn)結(jié)果如圖12～圖15所示。由圖12可以看出,在2.3 s時(shí)刻機(jī)械臂末端與障礙物發(fā)生接觸,此時(shí)導(dǎo)納控制器根據(jù)接觸力信號(hào)來修正機(jī)械臂末端軌跡以表現(xiàn)對(duì)環(huán)境接觸力的順應(yīng)性,如圖14所示。碰撞過程中,接觸力最大絕對(duì)值為7.4 N,在允許范圍內(nèi)。之后在6.1 s時(shí)刻,機(jī)械臂末端開始與障礙物脫離,接觸力與力矩經(jīng)振蕩后趨于平穩(wěn)并逐漸減小,同時(shí)機(jī)械臂末端軌跡開始逐步回到期望圓弧軌跡上。

圖12 圓弧過程所受接觸力 圖13 圓弧過程所受接觸力矩

圖14 圓弧過程的位置 圖15 圓弧過程中的姿態(tài)

圖16為機(jī)械臂圓弧運(yùn)動(dòng)開始狀態(tài)與結(jié)束狀態(tài)。

圖16 圓弧運(yùn)動(dòng)開始與結(jié)束狀態(tài)

3 結(jié)論

本文針對(duì)機(jī)械臂進(jìn)行了基于導(dǎo)納的實(shí)時(shí)柔順軌跡規(guī)劃研究,可以得出以下結(jié)論:①將機(jī)械臂笛卡爾空間軌跡規(guī)劃分為了位置規(guī)劃與姿態(tài)軌跡,并通過時(shí)間標(biāo)度參數(shù)s(t),s:[0,T]→[0,1]將位置規(guī)劃結(jié)果與姿態(tài)規(guī)劃結(jié)果歸一化,這樣可以通過設(shè)計(jì)時(shí)間標(biāo)度參數(shù)s(t)為二階導(dǎo)連續(xù)的速度曲線,就可以實(shí)現(xiàn)機(jī)械臂笛卡爾空間路勁規(guī)劃和速度規(guī)劃解耦的同時(shí),笛卡爾空間軌跡的位置、速度、加速度連續(xù);②以機(jī)械臂最常用的直線軌跡規(guī)劃和圓弧規(guī)劃為例闡述了機(jī)械臂笛卡爾空間位置規(guī)劃與姿態(tài)軌跡的原理;③為了解決傳統(tǒng)笛卡爾空間軌跡離線式規(guī)劃可能導(dǎo)致的過大環(huán)境接觸力問題,闡述了導(dǎo)納控制方法的基本原理,并將此方法與軌跡規(guī)劃相結(jié)合,以實(shí)現(xiàn)機(jī)械臂末端實(shí)時(shí)主動(dòng)順從外界接觸力;④本文最后給出了實(shí)時(shí)柔順的直線運(yùn)動(dòng)實(shí)驗(yàn)和圓弧運(yùn)動(dòng)實(shí)驗(yàn),實(shí)驗(yàn)結(jié)果表明本文提出的研究方法可以保證機(jī)械臂實(shí)現(xiàn)軌跡運(yùn)行高精度的同時(shí)也能避免因位置誤差導(dǎo)致的破壞性接觸力,驗(yàn)證了方法的正確性。

本文在導(dǎo)納控制器參數(shù)選取方面仍存在一定不足,比如控制器參數(shù)目前仍是人為選取定值而不是自動(dòng)計(jì)算得到,因此如何根據(jù)機(jī)器人作業(yè)對(duì)象類型與環(huán)境自適應(yīng)選取更合適參數(shù)是下一步研究的重點(diǎn)。