工業(yè)機(jī)器人實(shí)時(shí)高精度路徑跟蹤與軌跡規(guī)劃
工業(yè)機(jī)器人實(shí)時(shí)高精度路徑跟蹤與軌跡規(guī)劃
//
工業(yè)機(jī)器人實(shí)時(shí)高精度路徑跟蹤與軌跡規(guī)劃 *
徐 雄 譚冠政
(中南大學(xué) 信息科學(xué)與工程學(xué)院，湖南 長(zhǎng)沙 410083)
摘要：手部路徑跟蹤和關(guān)節(jié)軌跡規(guī)劃是機(jī)器人應(yīng)用領(lǐng)域中一個(gè)非常重要的課題。在以往的研究中，往往是通過
在設(shè)定的路徑上增加節(jié)點(diǎn)數(shù)和路徑分段數(shù)來提高機(jī)器人手部跟蹤設(shè)定路徑的精度，但這種方法會(huì)導(dǎo)致在線計(jì)算
量的大幅增加。針對(duì)這一缺陷，本文提出了一種新的實(shí)時(shí)高精度路徑跟蹤與關(guān)節(jié)軌跡規(guī)劃方法。該方法通過在
設(shè)定的手部路徑上按一定規(guī)則額外選取多個(gè)附加節(jié)點(diǎn)，使每一軌跡段上的節(jié)點(diǎn)數(shù)由 2 個(gè)增加到 4 個(gè)，并利用 1
個(gè) 3 次多項(xiàng)式、1 個(gè)正弦函數(shù)、1 個(gè)余弦函數(shù)以及 1 個(gè)由正弦函數(shù)和 1 次多項(xiàng)式的乘積構(gòu)成的函數(shù)來構(gòu)造每一
段的關(guān)節(jié)軌跡方程，使得在路徑分段數(shù)不變、關(guān)節(jié)軌跡方程總數(shù)不變以及計(jì)算量不顯著增加的前提下，大幅提
高機(jī)器人手部跟蹤設(shè)定路徑的精度。計(jì)算機(jī)仿真結(jié)果表明，該方法是非常有效的，它對(duì)提高機(jī)器人應(yīng)用水平具
有重要意義和價(jià)值。
關(guān)鍵詞：工業(yè)機(jī)器人；實(shí)時(shí)；高精度；路徑跟蹤；關(guān)節(jié)軌跡規(guī)劃；附加節(jié)點(diǎn)
中圖分類號(hào)：TP24 文獻(xiàn)標(biāo)識(shí)碼：9><>A
手部路徑跟蹤與關(guān)節(jié)軌跡規(guī)劃是工業(yè)機(jī)器人應(yīng)用領(lǐng)域中一個(gè)非常重要的課題，它在諸于弧焊、噴涂、
裝配、切割、外科手術(shù)以及繪畫等作業(yè)中有著廣泛的應(yīng)用，有很多學(xué)者對(duì)它進(jìn)行過研究[1~ 15]。作者曾在
文獻(xiàn)[10]中提出過一種改進(jìn)的路徑跟蹤與軌跡規(guī)劃方法。該方法是在原來相鄰的兩個(gè)節(jié)點(diǎn)之間，再在笛卡
爾空間中設(shè)定的路徑上選取 2 個(gè)節(jié)點(diǎn), 即 2 個(gè)附加節(jié)點(diǎn), 并引入一個(gè)正弦函數(shù)和一個(gè)余弦函數(shù), 然后采用
一種由 3 次或 4 次多項(xiàng)式加這兩個(gè)三角函數(shù)的方程來構(gòu)造機(jī)器人的關(guān)節(jié)軌跡。這種方法是基于增加節(jié)點(diǎn)數(shù)
但不增加關(guān)節(jié)軌跡方程數(shù)量的思想，由于能確保機(jī)器人手部通過設(shè)定路徑上更多的節(jié)點(diǎn)，因此它具有更高
的路徑跟蹤精度。從文獻(xiàn)[10]的仿真實(shí)例可以看出，假如設(shè)定的路徑被分割成n段，那么在第 2 ~ 第n-1 段
路徑上，增加 2 個(gè)附加節(jié)點(diǎn)后機(jī)器人手部跟蹤設(shè)定路徑的精度確實(shí)比改進(jìn)前的方法要高。但是，在首、尾
兩段路徑上，一方面由于關(guān)節(jié)位移方程中的多項(xiàng)式次數(shù)較高 (4 次)，另一方面由于多引入了 1 個(gè)三角函數(shù)
使關(guān)節(jié)軌跡多產(chǎn)生 1 次振蕩，所以手部路徑跟蹤精度在首、尾兩段路徑中的各節(jié)點(diǎn)之間有所降低。
本文針對(duì)文獻(xiàn)[10]存在的這一缺陷，將對(duì)其首、尾兩段路徑上的關(guān)節(jié)軌跡方程進(jìn)行改進(jìn)。在這兩段路
徑上，除引入一個(gè)正弦函數(shù)和一個(gè)余弦函數(shù)外，再引入一個(gè)新的函數(shù)，該函數(shù)由一個(gè)正弦函數(shù)和一個(gè) 1 次
多項(xiàng)式的乘積構(gòu)成，并且這兩段上原來的 4 次多項(xiàng)式將降低為 3 次多項(xiàng)式。這樣，在首、尾兩段路徑上，
關(guān)節(jié)軌跡方程將由一個(gè) 3 次多項(xiàng)式與一個(gè)正弦函數(shù)、一個(gè)余弦函數(shù)以及新引入的函數(shù)相加組成。從本文的
計(jì)算機(jī)仿真結(jié)果可以看出，這種新方法能有效提高首、尾兩段路徑上的路徑跟蹤精度，較好地解決了文獻(xiàn)
[10]所存在的問題。
* 基金項(xiàng)目: 國(guó)家自然科學(xué)基金（No. 8><#004699>502751<#004699>50）和中國(guó)科學(xué)院機(jī)器人學(xué)開放研究實(shí)驗(yàn)室基金（No. RL200002）資助項(xiàng)目.
作者簡(jiǎn)介: 譚冠政(1962-)，男，湖南湘潭人，博士，教授，博士生導(dǎo)師，主要研究方向：智能機(jī)器人系統(tǒng)與控制，人工
智能及其應(yīng)用，先進(jìn)控制理論與先進(jìn)算法。
徐 雄（1983-），男，湖北黃石人，碩士研究生，主要研究方向：智能機(jī)器人系統(tǒng)與控制。
論文聯(lián)系人: 徐 com
1
//
1 含 2 個(gè)附加節(jié)點(diǎn)的路徑跟蹤與關(guān)節(jié)軌跡規(guī)劃原理
設(shè)O0-X0Y0Z0為機(jī)器人的基礎(chǔ)坐標(biāo)系。如圖 1 所示，假定沿機(jī)器人手部設(shè)定的路徑從起點(diǎn)P0 到終點(diǎn)Pn
已選取了（n+1）個(gè)節(jié)點(diǎn)，即P0，P1， …， Pi-1，Pi， … ， Pn，它們將路徑分成n段。相應(yīng)地在關(guān)節(jié)空
間中，對(duì)某一關(guān)節(jié)j (j=1 ~ N, N為機(jī)器人的自由度)，從起點(diǎn)qj0到終點(diǎn)qjn, 其軌跡也被分成n段，整段軌跡可
由n個(gè)方程來表示。
xP0
nxP
0X
nyPyP0
nP
0P 0Y
0Z
zP0
nzP
iB i
P
1??iP
iC
0O
圖 1 含 2 個(gè)附加節(jié)點(diǎn)的實(shí)時(shí)高精度路徑規(guī)劃原理
Fig.1 Principle of hand path tracking with two extra knots on each segment
現(xiàn)在，再在原來相鄰的兩個(gè)節(jié)點(diǎn)Pi-1和Pi之間，在設(shè)定的手部路徑上額外選取兩個(gè)附加節(jié)點(diǎn)Bi和Ci，如
圖 1 所示。它們分別位于時(shí)間點(diǎn)t = ti-1+hi /3 和t = ti-1+2hi /3 處，這里ti-1為節(jié)點(diǎn)Pi-1對(duì)應(yīng)的時(shí)間，hi為節(jié)點(diǎn)Pi-1與
Pi之間的時(shí)間間隔。于是，從起點(diǎn)P0到終點(diǎn)Pn，節(jié)點(diǎn)總數(shù)將由原來的（n+1）個(gè)增加到（3n+1）個(gè)。由于
節(jié)點(diǎn)總數(shù)增加了近 2 倍，所以路徑跟蹤精度也將提高近 2 倍。我們的目的就是要對(duì)關(guān)節(jié)軌跡進(jìn)行規(guī)劃，使
機(jī)器人手部不僅通過節(jié)點(diǎn)P0 ~ Pn而且還要通過附加節(jié)點(diǎn)B1 ~ Bn和C1 ~ Cn，并且滿足位置、速度連續(xù)條件。
下面介紹關(guān)節(jié)軌跡構(gòu)造方法。由于每次只涉及 1 個(gè)關(guān)節(jié)，為表述簡(jiǎn)單，將省去關(guān)節(jié)序號(hào)j。
圖 2 所示為機(jī)器人某一關(guān)節(jié)隨時(shí)間t變化的位移曲線。圖中，q0，qi-1，qi及qn分別為節(jié)點(diǎn)P0，Pi-1，Pi及
Pn對(duì)應(yīng)的關(guān)節(jié)坐標(biāo)；βi為附加節(jié)點(diǎn)Bi對(duì)應(yīng)的關(guān)節(jié)坐標(biāo)，γi為附加節(jié)點(diǎn)Ci對(duì)應(yīng)的關(guān)節(jié)坐標(biāo)。關(guān)節(jié)坐標(biāo)可由節(jié)點(diǎn)
的位置和姿態(tài)坐標(biāo)經(jīng)逆運(yùn)動(dòng)學(xué)計(jì)算求得。
圖 2 機(jī)器人某一關(guān)節(jié)軌跡構(gòu)造圖
Fig.2 Joint trajectory construction method for some joint
q
iP
1??iP
nP
ntit1??it0t
1f2
f
iB
iC
ii ht 3
1
1 +??
t
ii ht 3
2
1 +??
0
ic??
iγ
)(tQiiq
1??iq
ib
)(1 tQi
0q
nq
iβ
0P
2
//
設(shè)t0和tn分別是起點(diǎn)P0和終點(diǎn)Pn對(duì)應(yīng)的時(shí)間，隨著節(jié)點(diǎn)P0，P1，… ，Pn的選取，區(qū)間[t0 , tn]也被分
割成n段。在某一子區(qū)間[ti-1, ti]上有 4 個(gè)節(jié)點(diǎn)，即起、止節(jié)點(diǎn)Pi-1、Pi和附加節(jié)點(diǎn)Bi、Ci 。其中，Bi和Ci對(duì)應(yīng)
的時(shí)間分別為：
(1)
(1) 在第 2 ～ 第 n-1 段軌跡上
在這些軌跡段上，引入附加節(jié)點(diǎn)Bi和Ci后，同時(shí)引入下面 2 個(gè)三角函數(shù)：
，i =2 ~ n -1 (2)
，i =2 ~ n -1 (3)
為了構(gòu)造關(guān)節(jié)軌跡，我們?cè)僖胍粋€(gè)三次多項(xiàng)式，其表達(dá)式為：
，i =1 ~ n (4)
在[ti-1, ti] (i =2 ~ n –1) 上，關(guān)節(jié)軌跡 可由(4)式與 f1 、f2 疊加而成，即： (Q
(5)
在[ti-1, ti] (i =2 ~ n –1)上，關(guān)節(jié)運(yùn)動(dòng)速度 和加速度 分別為：
(6)
(7)
函數(shù)f1和f2之所以如此選取，主要是為了保證計(jì)算節(jié)點(diǎn)Bi的關(guān)節(jié)坐標(biāo) 時(shí)只與bi有關(guān)，而與ci無關(guān)；計(jì)算
節(jié)點(diǎn)Ci的關(guān)節(jié)坐標(biāo) 時(shí)只與ci有關(guān)，而與bi無關(guān)，以簡(jiǎn)化計(jì)算。 和 的計(jì)算公式為：
(8)
bi和ci滿足下列關(guān)系：
當(dāng)Bi位于 上方時(shí)； 當(dāng)Ci位于 上方時(shí)；
當(dāng)Bi位于 下方時(shí)； 當(dāng)Ci位于 下方時(shí)；
因此，不論節(jié)點(diǎn)Bi和Ci的位置是在 的上方還是下方，只要適當(dāng)選取bi和ci的符號(hào)，就能使 通過Bi和
Ci兩個(gè)附加節(jié)點(diǎn)。
(2) 在第 1 和第 n 段軌跡上
)ti
)](
2
3sin[)
2
3()](
2
3cos[)
2
3((3)(2( ??????+??+??+= tctbttaQ )) 11213121 ???????? i
ii
ii
ii
iiiiiii thh
t
hh
tatat ππππ&
)](3cos[)3((3sin[)3((62( ??????????+= tctbtQ ππππ
22
)]
22
)) 1
2
1
2
132 ?????? i
ii
ii
ii
iiiii thh
t
hh
taat&&
( 1
1
ii t ??
( 1
1
iii t ??
( 1
1
iii ht ??
( 1
1
iii ht ??
ti& ti&&
1 ti
)3/ihQ + 3/2Q +
)3/hQ + 3/2Q +
)
)
)(Q (Q )
)(Q
)](
2
cos[(
2
sin[ ????+?? tcbt 3)]3)()()(
)()(
11
3
13
2
12110
21
1
?????????? ++??+??+=
++=
i
i
ii
i
iiiiiiii
ii
t
h
tt
h
tattattaa
fftQtQ
ππ
iγ
iβ
iγ
)(Q ti
],[ 1 ii ttt ??∈)](2
3cos[ 12 ????= i
i
i tth
cf π
)](
2
3sin[ 11 ????= i
i
i tth
bf π
()() ?????? iiiiiii tttt t ??∈
],[ 1 ii ttt ??∈
3
13
2
12110
1 )()( ??+??+??+=i tatataaQ tt ],[ 1 ii
iβ
iiiii
iiiii
chtQ
bhtQ
??+=
++=
??
??
)3/2(
)3/(
1
1
1
1
γ
β
????
??
>
<
,0
,
ic ????
?? 0
??= iii
3/1 iiB htt += ??
3/21 iiC htt += ????
???? tth
???? 1??
????
??
<
>
,0
,0
ib
3
//
在這兩段軌跡上，軌跡方程的構(gòu)造與(5)式不同，將在下節(jié)的第(1)和第(3)部分作詳細(xì)介紹。
2 關(guān)節(jié)軌跡方程推導(dǎo)
本節(jié)將分三部分建立實(shí)時(shí)高精度路徑跟蹤新方法的關(guān)節(jié)軌跡方程。設(shè)起點(diǎn)處，關(guān)節(jié)坐標(biāo)為q0，速度為
v0，加速度為<>a0；終點(diǎn)處，關(guān)節(jié)坐標(biāo)為qn，速度為vn，加速度為an。下面依次建立各時(shí)間區(qū)間上的關(guān)節(jié)軌跡
方程。
(1) 在區(qū)間[t0 , t1]上，引入附加節(jié)點(diǎn)B1和C1后，同時(shí)引入下面 3 個(gè)函數(shù)：
(9) , tt
(10)
(11)
在該區(qū)間上，關(guān)節(jié)軌跡Q1(t)可由 (4)式與 f1 、f2 及f3疊加而成，即：
(12)
其速度、加速度方程分別為：
(13)
(14)
代入起點(diǎn) 處的初始條件 ， ， ，以及 ， 和 處
的位置條件 ， 和 ，并假定 處的關(guān)節(jié)速度
，可得 7 個(gè)方程。聯(lián)立求解可得第 1 段軌跡方程(12) ~ (14)的各個(gè)系數(shù)為：
(15)
(16)
(17)
(18)
(19)
(20)
(21)
其中，
][ 10t∈)](sin[ thbf ??= 0111 t
π
)](cos[2 th
cf ??= tt0
1
1 t
π ],[ 10t∈
)](sin[) t
h
df ????= tt∈( 0
1
013 ttt
π
],[ 10t
)]((( tt
h
tdt
h
ctt
h
b ????+??+??+ πππ sin[)()]cos[)]sin[
)()()()(
0
1
010
1
10
1
1
3
013
2
012011101
tt
ttattattaatQ ??+??+??+=
h(t t
)]()((() t
h
t
h
dtt
h
dtt
hh
c ??????+???? πππππ cos[)()]sin[)]sin[(
)](cos[)()(3)(2)(
0
1
0
1
10
1
10
11
1
0
11
1
2
013012111
tt
tt
hh
bttattaatQ
+
??+??+??+= ππ&
)](sin[)())](cos[)2 tttdt ????????+ ππππ ((
)](cos[)()](sin[)()(62)(
0
1
0
2
1
10
11
1
0
1
2
1
10
1
2
1
1013121
t
hh
t
hh
d
tt
hh
ctt
hh
bttaatQ ??????????+= ππππ&&
1011 /)(v ??=13/ β=+ hQ ( 001 ) 111 ) = 1tt =13/2 γ=+Q 001 )( ht
10t tt =3/2tt +=3/htt 00 h00 +=001 ) = )( vQ = aQ =(t t& t&& 001 )(001
CDb =1
11 )π439( bBc ????=
)43(Ed π??= 9 111 hh
1010 cqa ??=
1
1
011 bh
va ??= π
1
1
12
1
2
0
12
2h2
d
h
caa ππ ??+=
3
1
2
112111101113 )( haaaca ??????+= hhq
4
//
(22)
BAD )12( 2π????=
1
2
1
2
1010011 )232()16327(21652254 cbhahvE ????????????????= ππβ
2
10101110 4266 hahvhvA +++??=
100111
(2) 在區(qū)間[ti-1 , ti] ( i =2 ~ n-1)上，關(guān)節(jié)位移，速度和加速度方程見式(5) ~ (7)，其中相鄰兩段軌跡方程
的系數(shù)可以建立遞推關(guān)系。將 ， ， 以及 處的位置坐標(biāo)條件q i-1、βi 、
γi、q i 代入 ，注意到ti-1 處的速度連續(xù)條件 ，又設(shè)ti處的關(guān)節(jié)速度為：
。聯(lián)立求解可得如下系數(shù)遞推關(guān)系：
(23)
(24)
(25)
(26)
(27)
(28)
其中，
(29)
(3) 在區(qū)間[tn-1 , tn]上，軌跡方程的推導(dǎo)與(1)類似，其關(guān)節(jié)位移，速度，加速度的方程分別為：
(30)
(31)
(32)
其初始條件和連續(xù)性條件分別為： ， ， ， ， ，
， 。將這些條件代入(30) ~ (32)，解聯(lián)立方程可求得第 n 段軌跡
方程(30) ~ (32)的各系數(shù)為：
nhQ nnn t β=+ 3/( nQ?? )1 nnn ht γ=+ /2( ??1 )3
)](sin[)((cos[(sin[ ????????+ n htdthcthb )])]
)()()()(
111
3
13
2
12110
??????
??????
++
??+??+??+=
n
n
nnn
n
n
n
n
nnnnnnnn
ttttt
ttattattaatQ
πππ
)]()((sin[()( ????+??+???? t
h
tt
h
dtt
h
dt
hh
c cos[)()])]sin[
)](cos[)()(3)(2)(
1111
1
2
13121
????????
?????? ??+??+??+=
n
n
n
n
nn
n
nn
nn
n
n
nn
nnnnnnn
tt
tt
hh
bttattaatQ
πππππ
ππ&
ti
q )1??
)( 111 ?????? iiii tt &&)(Q (=
tt
iii hqv /( ??= i
)
i=3/htt 1 ii +== tt 3/2tt??i 1 ii h1?? += ??
CDb =i
)334 20(])6([ ??????= ππ ii bAc
)143)(954( ????= ππC
11 iiiiii hvvA )42(20727 ???? ??+????= β
iiiiii hvvB )24(72027 11 ???? ??+????= γ
)203()346( ??????= ππ BAD????
????
??
????
????
??
??
)2(288()[( ii qa ++??= β )]2627 2102 iiiiii hbhaa??
32
1103 iiiiiiiii hhaq
??10
)]([ haaba ++??+=
iii cqa ??=
)
2
bva ??= 3(11
i
iii h
π
??
42236B ????+??= 418 hvγβ
)439)(12(2 2 πππ ??????=C??
??
????
????
??
????
????
)](sin[)()()](cos[)(2
)](cos[)()](sin[)()(62)(
11
2
1
1
2
1
2
132
??????
??????
????????+
??????????+=
n
n
n
n
nn
nn
n
n
nn
nn
nn
nnnnn
tt
h
tt
h
dtt
hh
d
tt
hh
ctt
hh
bttaatQ
ππππ
ππππ&&
nnn t =) vQ =)( aQ =)(( ( nnn t& nnn t&&11 ) ???? = nnn t 11 ???? nnn t& )( = vQ
5
//
(33) CDb =
(34)
n
nnnnnn bhv 918 11 ????nnc )43(422436 πγβ ????????+??=
(35) )()(43({
2d ππππ ????????= 2})]1292[ nnn hbA
(36)
(37)
(38)
(39)
其中，
(40)
應(yīng)該指出，以上以 3 次多項(xiàng)式加三角函數(shù)所構(gòu)造的關(guān)節(jié)軌跡，在附加節(jié)點(diǎn)Bi和Ci (i=1~ n) 處關(guān)節(jié)加速
度是連續(xù)的，但在節(jié)點(diǎn)Pi (i =1~ n -1) 處，關(guān)節(jié)加速度不連續(xù)。如要保證在Pi (i =1~ n -1) 處關(guān)節(jié)的加速度連
續(xù)，則必須采用 4 次多項(xiàng)式加三角函數(shù)來構(gòu)造關(guān)節(jié)軌跡。
3 超調(diào)性及振蕩性
3.1 超調(diào)性
這里，超調(diào)指關(guān)節(jié)坐標(biāo)超出其上界或下界值。在圖 2 中，在區(qū)間[ti-1 , ti] ( i =2 ~ n-1 )上Q1i (t) 是一個(gè) 3
次多項(xiàng)式曲線，它僅有一個(gè)可能的拐點(diǎn)或沒有拐點(diǎn)。但是，由于增加了三角函數(shù)f1和f2，所以不論曲線Q1i (t)
在區(qū)間[ti-1 , ti]上有無拐點(diǎn)，新的關(guān)節(jié)軌跡Qi (t)在該區(qū)間上的最大值和最小值都不一定存在于qi-1、 、
以及qi四者之中。但是，由于我們特意規(guī)定正弦函數(shù)f1和余弦函數(shù)f2分別在其對(duì)應(yīng)的附加節(jié)點(diǎn)Bi和Ci處取
得幅值，在該區(qū)間內(nèi)的其它點(diǎn)處，它們的值都小于其幅值。由于這種限制，因而新的關(guān)節(jié)軌跡Qi (t) 在區(qū)
間[ti-1 , ti] ( i =2 ~ n-1 )上的最大值和最小值都不會(huì)大大超過max (qi-1， ， ，qi )或min (qi-1， ， ，qi )。因
此，只要設(shè)定的軌跡不在關(guān)節(jié)邊界值附近，在這些軌跡段上就不會(huì)發(fā)生超調(diào)問題。類似地，在區(qū)間[t0 , t1]
和[tn-1 , tn]上盡管關(guān)節(jié)軌跡方程多增加了一個(gè)函數(shù)f3，但由于函數(shù)f1、f2、f3的幅值同樣受到其上的節(jié)點(diǎn)和附
加節(jié)點(diǎn)的關(guān)節(jié)坐標(biāo)值的限制，所以只要設(shè)定的軌跡不在關(guān)節(jié)邊界值附近，在這兩段軌跡上也不會(huì)發(fā)生超調(diào)
問題。
3.2 振蕩性
首先應(yīng)指出，在區(qū)間[ti-1，ti] (i =1 ~ n)上引入函數(shù)f1、f2、f3，不僅不會(huì)造成嚴(yán)重的關(guān)節(jié)軌跡振蕩，而且
有利于關(guān)節(jié)軌跡通過附加節(jié)點(diǎn)，從而提高軌跡跟蹤精度。
在區(qū)間[ti-1 , ti] ( i =2 ~ n-1 )上，由于引入的三角函數(shù)f1和f2的周期等于區(qū)間[ti-1 , ti]長(zhǎng)度的 34 倍，f1和f2兩
者疊加后的曲線在該區(qū)間上將產(chǎn)生 1 次振蕩。這樣，視三次多項(xiàng)式函數(shù)Q1i (t)在區(qū)間[ti-1 , ti]上有無拐點(diǎn)，新
iβ
iγ
iγiβ iβ iγ
32
2103 )( hca ??????+= nnnnnnnnn hahaaq
nnn ??10 cqa ??=
n
n
nn
π
??11 bh
va ??=
2
102 333( nnnnnnnnnnnn dhcbhaahqa = )2 hv ππ ??+????????
ABπ ??D )439(2 π??=
2
1
2
1
)4<#004699>50(4
)270)54)18216)36432
nnnnnn
nnn
hahvhv
+??????
??
??
??
π
nnnnnnn hvhv 11 418181818 ????
2222 22(4((( nA ????+??????= ππγπβπ
nB 4 ??+????+= γβ
)439(2)439)(12( 22 ππππ ??+????=C
????
????
??
????
??
??
????
6
//
的關(guān)節(jié)軌跡Qi (t)在該區(qū)間上可能會(huì)發(fā)生 1 ~ 2 次振蕩。但是，由于區(qū)間[ti-1 , ti]一般比較小，區(qū)間內(nèi)關(guān)節(jié)坐
標(biāo)值的變化不會(huì)很大，加上三角函數(shù)f1和f2必須在附加節(jié)點(diǎn)處取得幅值，使f1和f2的幅值受到限制。因此，
在這些軌跡段上即使關(guān)節(jié)軌跡有振蕩，其幅度也不會(huì)太大，不會(huì)給機(jī)械或電器部件造成損傷。在區(qū)間[t0, t1]
或[tn-1, tn]上，函數(shù)f1、f2、f3的周期都等于區(qū)間長(zhǎng)度的 2 倍，但由于f1、f3是正弦函數(shù)而f2是余弦函數(shù)，所以
三者疊加后的曲線在該區(qū)間上將產(chǎn)生 1 次振蕩。這樣，視三次多項(xiàng)式函數(shù)Q1i (t)在區(qū)間[ti-1 , ti]上有無拐點(diǎn)
(i=1 或n)，新的關(guān)節(jié)軌跡Qi (t)在該區(qū)間上可能會(huì)產(chǎn)生 1 ~ 2 次振蕩。同樣，由于函數(shù)f1、f2、f3的幅值都將受
到節(jié)點(diǎn)和附加節(jié)點(diǎn)的關(guān)節(jié)坐標(biāo)值的限制，所以在這兩個(gè)軌跡段上即使關(guān)節(jié)軌跡有振蕩，其幅度也不會(huì)太大。
4 計(jì)算機(jī)仿真實(shí)例
本節(jié)將以 6 自由度的 PUMA560 機(jī)器人為例，采用本文提出的方法對(duì)含 2 個(gè)附加節(jié)點(diǎn)的路徑跟蹤情況
進(jìn)行計(jì)算機(jī)仿真，并與文獻(xiàn)[10]所采用的方法進(jìn)行比較。PUMA560 機(jī)器人各坐標(biāo)架之間的關(guān)系參數(shù)以及各
關(guān)節(jié)變量的變化范圍如表 1 所示。
Table 1 Link parameters of the PUMA560
為便于觀察路徑跟蹤的精度，仿真時(shí)取機(jī)器人手部的設(shè)定路徑為直線。在該機(jī)器人的基礎(chǔ)坐標(biāo)系
O0-X0Y0Z0中，直線的起點(diǎn)P0和終點(diǎn)Pn的坐標(biāo)分別設(shè)定為：
利用計(jì)算機(jī)動(dòng)態(tài)圖形仿真技術(shù)，可
假設(shè)將時(shí)間區(qū)間[t0 , tn]等分成n個(gè)子區(qū)
路徑的參數(shù)方程可表示為：
關(guān)節(jié) θi αi ai (mm) di (mm) θi范圍
1 900 -900 0 0 -1600~+1600
2 00 00 432 149.5 -22<#004699>50~+4<#004699>50
3 900 900 0 0 -4<#004699>50~+22<#004699>50
4 00 -900 0 432.5 -1100~+1700
5 00 900 0 0 -1000~+1000
6 00 00 0 56.5 -2660~+2660
????
????
??
??=
=
=
mP
mP
mP
oz
oy
ox
400
600
400
表 1 PUMA560 機(jī)器人的連桿參數(shù)
nz
z
y
hn
PP
P
h
PP
hn
PP
P
??
??=
??
??
??
??=
????
???? ny
nx
x
n
P =??
7
在計(jì)算機(jī)屏幕上顯示PUMA560 機(jī)器人手部直線路徑，如圖 3 所示。
間，并設(shè)每一子區(qū)間 [ti-1 , ti] (i=1,2,…,n) 的長(zhǎng)度都等于h，則該直線
(41)
m
m
m
????
????
??
??=
=
??=
mmP
mmP
mmP
nz
ny
nx
360
600
<#004699>500
z
z
y
Ptt
t
Ptt
00
0
00
0
00
0
)(
)
)(
+??
+
+??
y
x
x
Pt( ??
//
圖 3 設(shè)定的機(jī)器人手部路徑 圖 3 設(shè)定的機(jī)器人手部路徑
Fig.3 Specified hand path of the PUMA560 Fig.3 Specified hand path of the PUMA560
假定機(jī)器人手部沿該直線路徑運(yùn)動(dòng)時(shí)的姿態(tài)不變，其三個(gè)歐拉角設(shè)為： ， 。 ??=φ 185=θ ??=??假定機(jī)器人手部沿該直線路徑運(yùn)動(dòng)時(shí)的姿態(tài)不變，其三個(gè)歐拉角設(shè)為： ， 。 ??=φ 185=θ ??=?? ， 065 0 0105 ， 065 0 0105
仿真時(shí)，設(shè)關(guān)節(jié)速度矢量v0 = [0,0,0,0,0,0]，vn = [0,0,0,0,0,0]；加速度矢量 <>a 0 = [0,0,0,0,0,0]，an = [0,0,0,0,0,0]。
并設(shè)t0=0 秒，tn=6 秒，分段數(shù)n=5 ，則h=1.2 秒。仿真的采樣周期取 0.2 秒。仿真結(jié)果如圖 4 所示。
仿真時(shí)，設(shè)關(guān)節(jié)速度矢量v0 = [0,0,0,0,0,0]，vn = [0,0,0,0,0,0]；加速度矢量 <>a 0 = [0,0,0,0,0,0]，an = [0,0,0,0,0,0]。
并設(shè)t0=0 秒，tn=6 秒，分段數(shù)n=5 ，則h=1.2 秒。仿真的采樣周期取 0.2 秒。仿真結(jié)果如圖 4 所示。
Z 0 (mm)
0 1 2 3 4 5 6
-600
-<#004699>500
-400
-300
-200
-100
0
100
200
300
400
t (s)
X 0(mm)
(<>a)
有 2 個(gè)附加節(jié)點(diǎn)并引入函數(shù)f1、f2的手部跟蹤路徑 有 2 個(gè)附加節(jié)點(diǎn)并引入函數(shù)f
有 2 個(gè)附加節(jié)點(diǎn)并引入函數(shù)f1、f2、f3的手部跟蹤路徑 有 2 個(gè)附加節(jié)點(diǎn)并引入函數(shù)f
(<>a) X 軸 (b) Y 軸跟蹤軌跡 (c) Z 軸跟蹤軌跡 (<>a) X 軸 (b) Y 軸跟蹤軌跡 (c) Z 軸跟蹤軌跡
的
法
手
獻(xiàn)
55
點(diǎn)點(diǎn)
1、f2的手部跟蹤路徑
1、f2、f3的手部跟蹤路徑
的
法
手
獻(xiàn)
0 1 2 3 4 5 6
-400
-395
-390
-385
-380
-375
-370
-365
-360
t (s)
0 1 2 3 4 5 6
596
597
598
599
600
601
602
603
604
Y0 (mm)
605
606
t (s)
(b) (c)
圖圖
Fig.4 RobFig.4 Rob
圖中給出了機(jī)器人手部跟蹤笛卡爾
跟蹤情況。其中，虛線表示采用文獻(xiàn)
機(jī)器人手部的跟蹤路徑。從該圖可看
部跟蹤直線路徑的精度明顯提高，更
[10]存在的缺陷。
圖中給出了機(jī)器人手部跟蹤笛卡爾
結(jié)論 結(jié)論
<>a. 提出了一種新的機(jī)器人高精度
并引入 2～3 個(gè)三角函數(shù)，在較少增
<>a. 提出了一種新的機(jī)器人高精度
并引入 2～3 個(gè)三角函數(shù)，在較少增
b. 介紹了關(guān)節(jié)軌跡方程的構(gòu)造方b. 介紹了關(guān)節(jié)軌跡方程的構(gòu)造方
跟蹤情況。其中，虛線表示采用文獻(xiàn)
機(jī)器人手部的跟蹤路徑。從該圖可看
部跟蹤直線路徑的精度明顯提高，更
[10]存在的缺陷。
跟蹤軌跡跟蹤軌跡
4 機(jī)器人手部跟蹤直線路徑的情況 4 機(jī)器人手部跟蹤直線路徑的情況
ot’s hand tracking <>a specified straight line ot’s hand tracking <>a specified straight line
空間中設(shè)定的直線路徑時(shí)，分別在基
[10]的方法機(jī)器人手部的跟蹤路徑，
出，在首、尾兩段，即第 1、第 5 段
加接近設(shè)定的直線。從而證明了本文
空間中設(shè)定的直線路徑時(shí)，分別在基
路徑跟蹤及關(guān)節(jié)軌跡規(guī)劃方法，通過在
加計(jì)算量的情況下，可大幅度提高機(jī)器
路徑跟蹤及關(guān)節(jié)軌跡規(guī)劃方法，通過在
加計(jì)算量的情況下，可大幅度提高機(jī)器
法，并建立了相鄰軌跡段關(guān)節(jié)軌跡方程法，并建立了相鄰軌跡段關(guān)節(jié)軌跡方程
[10]的方法機(jī)器人手部的跟蹤路徑，
出，在首、尾兩段，即第 1、第 5 段
加接近設(shè)定的直線。從而證明了本文
8
path path
礎(chǔ)坐標(biāo)系的X0、Y0、Z0軸方向
實(shí)線則表示采用本文提出的方
，引入新的函數(shù)f3后，機(jī)器人
提出的方法可較有效地解決文
礎(chǔ)坐標(biāo)系的X
每一軌跡段上增加 2 個(gè)附加節(jié)
人手部跟蹤設(shè)定路徑的精度。
每一軌跡段上增加 2 個(gè)附加節(jié)
人手部跟蹤設(shè)定路徑的精度。
系數(shù)的遞推公式。 系數(shù)的遞推公式。
0、Y0、Z0軸方向
實(shí)線則表示采用本文提出的方
，引入新的函數(shù)f3后，機(jī)器人
提出的方法可較有效地解決文
//
c. 以 6 自由度的 PUMA560 機(jī)器人為例，對(duì)所提出的方法進(jìn)行了計(jì)算機(jī)動(dòng)態(tài)圖形仿真，證實(shí)了該方法
的正確性和有效性。
參考文獻(xiàn)：
[1] Tondu B, Bazaz <>A. The three-cubic method: an optimal online robot joint trajectory generator under velocity, acceleration, and wandering constraints [J],
The International Journal of Robotics Research, 1999, 18(9): 893-901.
[2] Galicki M. The planning of robotic optimal motions in the presence of obstacles [J]. The International Journal of Robotics Research, 1998, 17(3):
248-259.
[3] TAN Guan-zheng, LI Tan. Optimum motion planning of CS-I industrial robot with two manipulators [<>A]. Proceedings of IEEE 1997 International
Conference on Intelligent Processing Systems [C]. Beijing, 1997. 1287-1291.
[4] Lin C S, Chang P R, Luh J Y S. Formulation and optimization of cubic polynomial joint trajectories for mechanical manipulators [J]. IEEE Transactions
on Automatic Control, 1983, 28: 1066-1078
[5] Chand S, Doty k L. On-line polynomial trajectories for robot manipulators [J]. The International Journal of Robotics Research, 1985, 4(2): 38-48.
[6] Bobow J E, Dubowsky S, Gibson J S. Time- optimal control of robotic manipulators along specified paths [J]. The International Journal of Robotics
Research, 1985, 4(3): 3-17.
[7] Aken L V, Brussel H V. On-line robot trajectory control in joint coordinates by means of imposed acceleration profiles [J]. Robotica, 1988, 6: 185-195.
[8] Luh J Y S, Lin C S. Approximat
個(gè)人主頁(yè)