《工業(yè)機(jī)器人實(shí)時(shí)高精度路徑跟蹤與軌跡規(guī)劃》由會(huì)員分享，可在線閱讀，更多相關(guān)《工業(yè)機(jī)器人實(shí)時(shí)高精度路徑跟蹤與軌跡規(guī)劃（75頁(yè)珍藏版）》請(qǐng)?jiān)谘b配圖網(wǎng)上搜索。

1、工業(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è)，并利用

2、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ù)以及繪畫等

3、作業(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 段 路徑

4、上，增加 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)式。這樣，在首、尾兩段路徑上，

5、 關(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)算法。 徐 雄

6、（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è)方程來表示。

7、 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 所示。它們

8、分別位于時(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é)隨

9、時(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

10、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í)間分別為：

11、 (1) (1) 在第 2 ～ 第 n-1 段軌跡上 在這些軌跡段上，引入附加節(jié)點(diǎn)Bi和Ci后，同時(shí)引入下面 2 個(gè)三角函數(shù)： ，i =2 ~ n -1 (2)

12、 ，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)

13、 在[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)，而與c

14、i無關(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位于

15、 下方時(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

16、 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/i

17、hQ + 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 t

18、tt ??∈)](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

19、 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é)軌跡方程

20、。設(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

21、 (10) (11) 在該區(qū)間上，關(guān)節(jié)軌跡Q1(t)可由 (4)式與 f1 、f2 及f3疊加而成，即： (12) 其速度、加速度方程分別為：

22、 (13) (14) 代入起點(diǎn) 處的初始條件 ， ， ，以及 ， 和 處 的位置條件 ，

23、 和 ，并假定 處的關(guān)節(jié)速度 ，可得 7 個(gè)方程。聯(lián)立求解可得第 1 段軌跡方程(12) ~ (14)的各個(gè)系數(shù)為： (15) (16)

24、 (17) (18) (19)

25、 (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

26、 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

27、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 c

28、tt 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

29、 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 ???

30、?????????????= ππβ 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é)速度為：

31、 。聯(lián)立求解可得如下系數(shù)遞推關(guān)系： (23) (24)

32、 (25) (26) (27)

33、 (28) 其中， (29) (3) 在區(qū)間[tn-1 , tn]上，軌跡方程的推導(dǎo)與(1)類似，其關(guān)節(jié)位移，速度，加速度的方程分別為： (30)

34、 (31) (32) 其初始條件和連續(xù)性條件分別為： ， ， ， ， ， ， 。將這些條件代入(30) ~ (32)，解聯(lián)立方程可求得第 n 段軌跡 方程(30) ~ (32)的各系數(shù)為： nhQ nnn t β=+ 3/( nQ?? )1 nnn ht γ=+ /2( ??1 )3 )](sin[)((cos[(s

35、in[ ????????+ 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

36、 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 )3

37、34 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

38、 )]([ 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 ?????? ?????? ????????+

39、 ??????????+= 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 //

40、 (33) CDb = (34) n nnnnnn bhv 918 11 ????nnc )43(422436 πγβ ????????+??=

41、 (35) )()(43({ 2d ππππ ????????= 2})]1292[ nnn hbA (36)

42、 (37) (38) (39) 其中，

43、 (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ù)，

44、則必須采用 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)

45、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é)邊界值附近，在這兩段軌跡上也

46、不會(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 )(

47、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

48、 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的幅值受到限制。

49、因此， 在這些軌跡段上即使關(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ì)太大。

50、 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中，直線

51、的起點(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<#00

52、4699>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 ?? ??= ?? ?? ?? ??= ???? ??

53、?? 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

54、 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)不變，

55、其三個(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 所

56、示。 仿真時(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

57、 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 軸跟蹤軌跡

58、 的 法 手 獻(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)

59、 圖圖 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ī)器人手部的跟蹤路

60、徑。從該圖可看 部跟蹤直線路徑的精度明顯提高，更 [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ī)劃方法，通

61、過在 加計(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è)附加

62、節(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

63、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.

64、 [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 c

65、ubic 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] Bobo

66、w 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