沈雅瓊，葉伯生，熊 爍

(華中科技大學(xué) 機(jī)械科學(xué)與工程學(xué)院，武漢 430074)

?

基于齊次變換矩陣的機(jī)器人軌跡規(guī)劃方法*

沈雅瓊，葉伯生，熊 爍

(華中科技大學(xué) 機(jī)械科學(xué)與工程學(xué)院，武漢 430074)

基于齊次變換矩陣，提出一種笛卡爾空間內(nèi)的機(jī)器人軌跡規(guī)劃方法。該軌跡規(guī)劃方法首先引入關(guān)節(jié)屬性來(lái)預(yù)知軌跡示教點(diǎn)的位置是否合適，然后以機(jī)器人末端執(zhí)行器的軌跡特征和齊次位姿矩陣的優(yōu)勢(shì)為基礎(chǔ)，將機(jī)器人笛卡爾空間內(nèi)的軌跡規(guī)劃劃分為平動(dòng)和轉(zhuǎn)動(dòng)軌跡規(guī)劃，建立平動(dòng)和轉(zhuǎn)動(dòng)軌跡方程，進(jìn)行位置和姿態(tài)的插補(bǔ)。該軌跡規(guī)劃方法適用于機(jī)器人笛卡爾空間的直線和圓弧軌跡規(guī)劃，不僅克服了工業(yè)機(jī)器人原有軌跡規(guī)劃方法中由于機(jī)器人的奇異性和歐拉角算法所引起的缺點(diǎn)，而且具有概念直觀、規(guī)劃路徑準(zhǔn)確、可操作性強(qiáng)的特點(diǎn)。

齊次變換矩陣；軌跡規(guī)劃；關(guān)節(jié)屬性；

0 引言

機(jī)器人系統(tǒng)依據(jù)運(yùn)動(dòng)的軌跡來(lái)工作，其效率和工作質(zhì)量的優(yōu)劣與軌跡規(guī)劃的好壞密不可分[1]。為此，研究機(jī)器人軌跡規(guī)劃的方法是尤為重要的。所謂軌跡，是指操作臂在運(yùn)動(dòng)中的位移、速度和加速度。而機(jī)器人軌跡規(guī)劃就是根據(jù)機(jī)器人需要完成的任務(wù)，來(lái)設(shè)計(jì)機(jī)器人的運(yùn)動(dòng)規(guī)律[2]。軌跡規(guī)劃可以在笛卡爾空間和關(guān)節(jié)空間中進(jìn)行，但都要求所規(guī)劃的軌跡函數(shù)必須連續(xù)且平滑，以保證機(jī)器人的末端執(zhí)行器的運(yùn)動(dòng)平穩(wěn)[3]。

在關(guān)節(jié)空間的軌跡規(guī)劃是以關(guān)節(jié)角度的函數(shù)來(lái)描述機(jī)器人軌跡的，在笛卡爾空間進(jìn)行規(guī)劃是將機(jī)器人手部位姿、速度和加速度表示為時(shí)間的函數(shù)，而相應(yīng)的關(guān)節(jié)位移、速度和加速度由機(jī)器人手部信息導(dǎo)出[4]。關(guān)節(jié)空間的軌跡規(guī)劃，不必在笛卡爾坐標(biāo)系中描述兩個(gè)路徑點(diǎn)的路徑形狀，計(jì)算簡(jiǎn)單、容易，且不會(huì)發(fā)生機(jī)構(gòu)的奇異性問(wèn)題[5]。但如果要求機(jī)器人末端執(zhí)行器按照預(yù)定的路徑運(yùn)動(dòng)或要求繞過(guò)障礙物，關(guān)節(jié)空間法是無(wú)法實(shí)現(xiàn)的，而需要采用笛卡爾空間法。并且機(jī)器人的空間圓弧運(yùn)動(dòng)必須在笛卡兒空間中進(jìn)行規(guī)劃。故本文重點(diǎn)研究機(jī)器人笛卡爾空間的軌跡規(guī)劃方法，并以莫托曼SK6型機(jī)器人為實(shí)驗(yàn)對(duì)象。

目前，有很多文獻(xiàn)對(duì)機(jī)器人笛卡爾空間的軌跡規(guī)劃方法進(jìn)行了研究。文獻(xiàn)[6-7]研究了笛卡爾空間內(nèi)基本的軌跡規(guī)劃，文獻(xiàn)[8-9]對(duì)原有的軌跡規(guī)劃方法進(jìn)行改進(jìn)，從而使機(jī)器人運(yùn)行更平穩(wěn)、路徑更圓滑，文獻(xiàn)[10]針對(duì)機(jī)器人軌跡規(guī)劃的時(shí)間最短性能指標(biāo)，提出一種軌跡規(guī)劃方法。但是這些算法都沒(méi)有提出采用歐拉角或RPY法描述機(jī)器人姿態(tài)時(shí)，給機(jī)器人笛卡爾空間軌跡規(guī)劃所帶來(lái)一系列問(wèn)題的解決方法。

本文首先通過(guò)分析機(jī)器人機(jī)械結(jié)構(gòu)特征，引入關(guān)節(jié)屬性，然后結(jié)合機(jī)器人末端執(zhí)行器在笛卡爾空間的運(yùn)動(dòng)特征和齊次變換矩陣的優(yōu)勢(shì)，并且對(duì)機(jī)器人傳統(tǒng)軌跡規(guī)劃中出現(xiàn)的問(wèn)題進(jìn)行分析，提出一種基于齊次變換矩陣的軌跡規(guī)劃方法。該方法通過(guò)將機(jī)器人笛卡爾空間的軌跡規(guī)劃細(xì)分為平動(dòng)和轉(zhuǎn)動(dòng)軌跡規(guī)劃，采用齊次變換矩陣的方式來(lái)設(shè)計(jì)機(jī)器人笛卡爾空間的軌跡方程，并進(jìn)行路程和姿態(tài)的插補(bǔ)。

1 機(jī)器人的關(guān)節(jié)屬性及姿態(tài)表示方法

一般，在機(jī)器人笛卡爾空間進(jìn)行軌跡規(guī)劃時(shí)，相應(yīng)的關(guān)節(jié)位移是通過(guò)機(jī)器人運(yùn)動(dòng)學(xué)反解得到的，故機(jī)器人笛卡爾空間的軌跡規(guī)劃存在由于運(yùn)動(dòng)學(xué)反解帶來(lái)的問(wèn)題，即笛卡爾路徑上解的存在性、唯一性和奇異性問(wèn)題。

機(jī)器人的軌跡是通過(guò)示教來(lái)實(shí)現(xiàn)的，示教過(guò)程中，若所選擇的起止點(diǎn)位置不當(dāng)，而又無(wú)法預(yù)知，使得所規(guī)劃的軌跡經(jīng)過(guò)機(jī)器人的奇異點(diǎn)，不僅增加工作量、浪費(fèi)時(shí)間，而且造成機(jī)器人的速度激增。實(shí)際上，所容許的關(guān)節(jié)速度是有限的，因而將導(dǎo)致操作臂偏離預(yù)期軌跡。這是機(jī)器人運(yùn)動(dòng)學(xué)反解為機(jī)器人笛卡爾空間的軌跡規(guī)劃帶來(lái)的一種問(wèn)題。同時(shí)，在描述機(jī)器人姿態(tài)時(shí)，大多情況會(huì)采用可以簡(jiǎn)便描述機(jī)器人方位的歐拉角或RPY方法，但這些方法的計(jì)算過(guò)程中，在某些特殊值處都不可避免的存在反解退化等問(wèn)題。

1.1機(jī)器人的關(guān)節(jié)屬性

本文所用莫托曼SK6機(jī)器人為是一款典型的平行四邊形結(jié)構(gòu)的6軸機(jī)器人，主要用于弧焊操作。該機(jī)器人屬于關(guān)節(jié)型6軸機(jī)器人，具有3種奇異形位[11]，可以用3種奇異形位作為臨界位置把機(jī)器人的狀態(tài)劃分為8種，由此可規(guī)定機(jī)器人的3種關(guān)節(jié)屬性，即前/后、左/右和上/下，如圖1所示。根據(jù)這3種關(guān)節(jié)屬性，可以得出以下結(jié)論：當(dāng)路徑起止示教點(diǎn)的關(guān)節(jié)屬性不同時(shí)，機(jī)器人在笛卡爾坐標(biāo)系下的直線運(yùn)動(dòng)有可能經(jīng)過(guò)機(jī)器人奇異形位。而機(jī)器人在做線性運(yùn)動(dòng)的同時(shí)變換關(guān)節(jié)屬性是不可能的，故可以通過(guò)在軌跡規(guī)劃中引入關(guān)節(jié)屬性的判斷，使得機(jī)器人運(yùn)動(dòng)是否經(jīng)過(guò)奇異位置得以預(yù)知[12]，即在進(jìn)行軌跡規(guī)劃前，先根據(jù)上述3中關(guān)節(jié)屬性的劃分，對(duì)示教點(diǎn)的關(guān)節(jié)屬性進(jìn)行判斷，如果關(guān)節(jié)屬性不同，則重新示教。這樣，就有效的避免了機(jī)器人在笛卡爾空間運(yùn)行過(guò)程中經(jīng)過(guò)奇異形位。

圖1 SK6型工業(yè)機(jī)器人關(guān)節(jié)屬性界定

1.2 機(jī)器人的姿態(tài)表示方法

采用3個(gè)參數(shù)的方法可以簡(jiǎn)便地描述機(jī)器人的方位，如歐拉角和RPY方法，但無(wú)論是歐拉角還是RPY描述位姿，在進(jìn)行相應(yīng)的反解時(shí)，都會(huì)出現(xiàn)反解退化的問(wèn)題，并且邊界位置的角度值是不連續(xù)的。為了分析采用三個(gè)參數(shù)來(lái)描述機(jī)器人姿態(tài)方法中存在的問(wèn)題，以z-y-z歐拉角為例予以詳細(xì)說(shuō)明。采用z-y-z歐拉角描述機(jī)器人末端執(zhí)行器方位的方法進(jìn)行姿態(tài)規(guī)劃插補(bǔ)，所謂z-y-z歐拉角描述法，即三次轉(zhuǎn)動(dòng)的順序?yàn)橄壤@z軸旋轉(zhuǎn)角度A，再繞y軸旋轉(zhuǎn)角度B，最后繞z軸旋轉(zhuǎn)角度C。

在歐拉角的反解表達(dá)式中，A角和C角在邊界位置是不連續(xù)的，即A或C角超過(guò)360°時(shí)從0°開(kāi)始計(jì)算。在所規(guī)劃的軌跡中，當(dāng)A角或C角需要跨越邊界位置時(shí)，機(jī)器人末端執(zhí)行器不能保證按最短路徑通過(guò)規(guī)劃的連續(xù)軌跡。而在機(jī)器人運(yùn)動(dòng)過(guò)程中，合理的選擇應(yīng)該是盡量保證機(jī)器人按最短路徑行走到目標(biāo)位置[13]。并且歐拉角算法本身存在奇異點(diǎn)，即當(dāng)繞y軸旋轉(zhuǎn)的角度B為0°或180°時(shí)，其他兩個(gè)旋轉(zhuǎn)角度是人為確定的，從而造成了機(jī)器人末端執(zhí)行器不能按預(yù)期的路徑行走或中間工作點(diǎn)超出工作空間范圍。

本文中提出的軌跡規(guī)劃方法采用齊次變換矩陣來(lái)表示機(jī)器人姿態(tài)，而非歐拉角或PRY方法來(lái)對(duì)直線和圓弧進(jìn)行軌跡規(guī)劃和插補(bǔ)，這種方式本身不存在算法上的奇異點(diǎn)，從而可以有效的避免歐拉角正反解中帶來(lái)的問(wèn)題。

2 機(jī)器人的直線軌跡規(guī)劃

(1)

Li左乘，表示平動(dòng)是相對(duì)于固定坐標(biāo)系的平移；Ri右乘，表示繞某個(gè)定軸的轉(zhuǎn)動(dòng)是相對(duì)于運(yùn)動(dòng)坐標(biāo)系的旋轉(zhuǎn)。

圖2 機(jī)器人直線軌跡規(guī)劃

(2)

2.1 平動(dòng)軌跡規(guī)劃

對(duì)于平動(dòng)軌跡規(guī)劃，在建立直線運(yùn)動(dòng)的軌跡方程之后，僅需要對(duì)其路程參數(shù)進(jìn)行插補(bǔ)即可。已知軌跡示教起點(diǎn)的位置坐標(biāo)為WpS，終點(diǎn)位置坐標(biāo)WpE。設(shè)終點(diǎn)WpE到起點(diǎn)WpS的軌跡長(zhǎng)度為L(zhǎng)，有：

(3)

中間點(diǎn)Wpi到起點(diǎn)WpS的軌跡長(zhǎng)度為si，則以si為參數(shù)的直線運(yùn)動(dòng)軌跡方程為：

(4)

平動(dòng)軌跡規(guī)劃只需要對(duì)路程參數(shù)si進(jìn)行插值，便能計(jì)算出各中間點(diǎn)位置坐標(biāo)。故本文采用拋物線過(guò)渡的線性插值，也就是傳統(tǒng)的梯形加減速控制策略計(jì)算路程參數(shù)si。已知設(shè)定線速度為v，加速度為a，并設(shè)si處線速度為vi，下一插值點(diǎn)si+1處線速度為vi+1，插補(bǔ)時(shí)間間隔為ΔT，有：

si+1=si+vi+1·ΔT

(5)

插值點(diǎn)處于加速勻速段，有：

(6)

插值點(diǎn)處于減速段，機(jī)器人必需以a的加速度減速直到完成本段插補(bǔ)。故有：

vi+1=vi-aΔT

(7)

將式(5)代入(4)得：

(8)

上式(8)即為平動(dòng)軌跡規(guī)劃的基本算法，聯(lián)合式(6)、(7)便可計(jì)算出vi+1。

2.2 轉(zhuǎn)動(dòng)軌跡規(guī)劃

(9)

將RS寫成：

根據(jù)旋轉(zhuǎn)變換通式，得：

(10)

(11)

(12)

θi+1=θi+ωi+1·ΔT

(13)

式(13)中的角速度ω一般要根據(jù)平動(dòng)時(shí)間進(jìn)行調(diào)整。這是因?yàn)槭窘潭x的角速度ω可能會(huì)與線速度v相矛盾，即按ω執(zhí)行的轉(zhuǎn)動(dòng)可能與按v執(zhí)行的平動(dòng)不能同步完成。

插值點(diǎn)處于加速勻速段。有：

(14)

插值點(diǎn)處于減速段。有：

ωi+1=ωi-αΔT

(15)

將式(13)代入(12)有：

(16)

式(16)便是繞某個(gè)定軸轉(zhuǎn)動(dòng)軌跡規(guī)劃的基本算法，聯(lián)合式(14)、(15)便可計(jì)算出ωi+1。

3 機(jī)器人圓弧軌跡規(guī)劃

對(duì)于平動(dòng)軌跡規(guī)劃，已知圓弧起點(diǎn)的位置坐標(biāo)為WpS，中間點(diǎn)位置坐標(biāo)WpC，終點(diǎn)位置坐標(biāo)WpE，設(shè)路徑中間點(diǎn)Wpi到起點(diǎn)WpS的弧長(zhǎng)為si，圓心角為φi，終點(diǎn)WpE到起點(diǎn)WpS的軌跡長(zhǎng)度為L(zhǎng)，圓心角為Φ。

|WpS-WpO| =|WpC-WpO|=|WpE-WpO|= R

(17)

(WpS-WpO)×(WpC-WpS)=λ(WpC-WpS)×(WpE-WpC)

(18)

圖3 圓弧軌跡示意

由式(17)可建立兩個(gè)方程：

(19)

由式(19)可建立另一個(gè)方程：

(20)

聯(lián)立式(19)和(20)，并寫成系數(shù)矩陣的形式有：

(21)

式中，A為3×3矩陣，

B為3×3矩陣，

B2=A20xS+A21yS+A22zS

由式(21)可求出圓弧圓心WpO，并由式(17)求出圓弧半徑R。

則，過(guò)圓弧圓心的旋轉(zhuǎn)軸k為：

(22)

圓弧圓心角需要根據(jù)劣弧或是優(yōu)弧分別計(jì)算。設(shè)：

如果k與n方向相同，則軌跡圓弧為劣弧，則：

φ=arccos{[(xE-xS)2+(yE-yS)2+(zE-zS)2-2R2]/(2R2)}

(23)

如果k與n方向相反，則軌跡圓弧為優(yōu)弧，則：

(24)

(25)

以si為參數(shù)的姿態(tài)旋轉(zhuǎn)軌跡方程為：

(26)

與直線運(yùn)動(dòng)軌跡規(guī)劃一樣，通過(guò)對(duì)路程參數(shù)si的插值，生成圓弧運(yùn)動(dòng)路徑中間點(diǎn)Wpi。且采用拋物線過(guò)渡的線性插值，即梯形加減速控制策略計(jì)算路程參數(shù)si，并分為加速勻速段和減速段兩部分對(duì)路程參數(shù)si插值。參照3.1節(jié)的插值過(guò)程，計(jì)算出si后，帶入式(26)，再與繞某個(gè)定軸轉(zhuǎn)動(dòng)的軌跡規(guī)劃結(jié)果聯(lián)合，便構(gòu)成了完整的圓弧軌跡規(guī)劃。

4 結(jié)束語(yǔ)

本文提出的基于齊次變換矩陣的機(jī)器人軌跡規(guī)劃方法對(duì)機(jī)器人笛卡爾空間的直線和圓弧軌跡規(guī)劃均適用。該方法具有以下優(yōu)點(diǎn)：1，充分發(fā)揮齊次變換矩陣的優(yōu)勢(shì)，避免了采用歐拉角進(jìn)行軌跡規(guī)劃時(shí)存在的問(wèn)題；2，在軌跡規(guī)劃中引入關(guān)節(jié)屬性的概念，使得示教路徑點(diǎn)的合適與否得以預(yù)知，從而有效的避免了機(jī)器人在笛卡爾空間運(yùn)行過(guò)程中經(jīng)過(guò)奇異形位；3，將機(jī)器人末端執(zhí)行器的在笛卡爾空間的運(yùn)動(dòng)分為平動(dòng)和轉(zhuǎn)動(dòng)，使得軌跡規(guī)劃方法概念清晰，步驟明確，且可操作性強(qiáng)。

本文研究的算法已經(jīng)在華中科技大學(xué)國(guó)家數(shù)控系統(tǒng)工程技術(shù)研究中心實(shí)施項(xiàng)目中的莫托曼SK6型機(jī)器人上得以驗(yàn)證，證明該方法是可行的并且滿足實(shí)用要求。

[1] 王曉麗，侯媛彬，王濤. 基于VC++的工業(yè)機(jī)器人軌跡規(guī)劃研究[J]. 工況自動(dòng)化，2009(5)：34-37.

[2] 秦鋒, 徐向榮. 基于樣條函數(shù)法的機(jī)器人運(yùn)動(dòng)軌跡規(guī)劃[J].機(jī)械科學(xué)與技術(shù)，1996，15(4)：561-565.

[3] 王衛(wèi)忠，趙杰，高永生，等. 機(jī)器人的平面曲線軌跡規(guī)劃方法[J]. 哈爾濱工業(yè)大學(xué)學(xué)報(bào)，2008，40(3)：389-392.

[4] 熊有倫. 機(jī)器人技術(shù)基礎(chǔ)[M] . 武漢：華中理工大學(xué)出版社，1996.

[5] 吳瑞祥. 機(jī)器人技術(shù)及應(yīng)用[M]. 北京：北京航空航天大學(xué)出版社，1994.

[6] 鄧勇.樣條曲線的實(shí)時(shí)插補(bǔ)[J]. 機(jī)械與電子，2001(4)：23-24.

[7] 任敬軼，孫漢旭. 一種新穎的笛卡爾空間軌跡規(guī)劃方法[J]. 機(jī)器人，2002，24(3):217-221.

[8] 周苑，基于改進(jìn)性遺傳算法的Bezier曲線笛卡爾空間軌跡規(guī)劃[J]. 上海電機(jī)學(xué)院學(xué)報(bào)，2012，15(4)：237-240.

[9] 張小江，高秀華. 三次樣條插值在機(jī)器人軌跡規(guī)劃應(yīng)用中的改進(jìn)研究[J]. 機(jī)械設(shè)計(jì)與制造，2008(9)：170-171.

[10] 張紅強(qiáng). 工業(yè)機(jī)器人時(shí)間最優(yōu)軌跡規(guī)劃[D]. 長(zhǎng)沙：湖南大學(xué)，2004：28-61.

[11] 畢諸明，蔡鶴皋. 六自由度操作手的逆運(yùn)動(dòng)學(xué)問(wèn)題[J]. 機(jī)器人，1994，16(2)：68-71.

[12] 葉伯生，郭顯金，熊爍. 計(jì)及關(guān)節(jié)屬性的6軸工業(yè)機(jī)器人反解算法[J]. 華中科技大學(xué)學(xué)報(bào)(自然科學(xué)版)，2013，41(3)：68-72.

[13] 丁學(xué)恭. 機(jī)器人控制研究[M]. 杭州：浙江大學(xué)出版社，2006.

[14] 李天友，孟正大. 基于焊接機(jī)器人的實(shí)時(shí)軌跡插補(bǔ)算法[J]. 工業(yè)控制計(jì)算機(jī)，2009，22(6)：34-36.

[15] 葉伯生. 機(jī)器人空間三點(diǎn)圓弧功能的實(shí)現(xiàn)[J]. 華中科技大學(xué)學(xué)報(bào)(自然科學(xué)版)，2007，35(8)：5-8.

(編輯 李秀敏)

A Method of Robot’s Trajectory Planning Based on Homogeneous Transformation Matrix

SHEN Ya-qiong,YE Bo-sheng,XIONG Shuo

(School of Mechanical Science and Engineering,HuaZhong University of Science and Technology, Wuhan 430074,China)

Based on homogeneous transformation matrix, a method of trajectory planning for robot in cartesian space is put forward. In the method, joint attribute is first introduced to predict whether the position of demonstration points is appropriate. Then, based on the trajectory characteristics of the robot’s end effector and the advantage of homogeneous transformation matrix, the trajectory planning in the robot’s cartesian space is subdivided into translation and rotation trajectory planning, based on which the equation of translation and rotation trajectory can be founded, and the interpolation of position and posture can be proceeded. The method is available for both arc and line trajectory planning for robot in cartesian space. It not only overcomes the original shortcomings of trajectory planning method caused by the singularity of robot and Euler angular algorithm, but also has many characteristics, such as visualized concept, accurate path planning and strong maneuverability.

homogeneous transformation matrix; joint attribute; euler angle; trajectory planning;

1001-2265(2014)01-0005-05

10.13462/j.cnki.mmtamt.2014.01.002

2013-05-24

“高檔數(shù)控機(jī)床與基礎(chǔ)制造裝備”科技重大專項(xiàng)“開(kāi)放式高檔數(shù)控系統(tǒng)、伺服裝置和電機(jī)成套產(chǎn)品開(kāi)發(fā)與綜合驗(yàn)證”(2012ZX04001012)；“高檔數(shù)控機(jī)床與基礎(chǔ)制造裝備”科技重大專項(xiàng)“高精度、高分辨力絕對(duì)式光柵旋轉(zhuǎn)編碼器研制”(2012ZX04001041)；高等學(xué)校博士學(xué)科點(diǎn)專項(xiàng)科研基金(新教師基金課題)：基于機(jī)床動(dòng)力學(xué)得復(fù)雜曲面軌跡優(yōu)化技術(shù)研究 (20090142120035)。

沈雅瓊(1988—)，女，甘肅景泰人，華中科技大學(xué)碩士研究生，研究方向?yàn)楣I(yè)機(jī)器人控制，(E-mail)yaqionghappy@126.com；通訊作者：葉伯生(1966—)，男，湖北黃岡人，華中科技大學(xué)副教授，博士，研究領(lǐng)域?yàn)閿?shù)控技術(shù)，機(jī)器人控制等，(E-mail) yebosh@mail.com。

TH132;TG659

A