機(jī)器人建模與控制課件：機(jī)器人軌跡規(guī)劃

機(jī)器人建模與控制

機(jī)器人軌跡規(guī)劃?機(jī)器人將盛滿水的杯子由P移到Q?因存在障礙物，機(jī)器人末端不能簡單地沿直線段PQ移動，而是需要繞過障礙

物，例如沿圖中的一系列紅點(diǎn)移動?末端位姿形成了笛卡爾空間中以

TP

=cos

asin

a00?sinacos

a000010xPyP01為首、40cos

a以

TQ

=

sin

a0?sinacos

a000010xQyQ

為尾的位形序列，即笛卡爾空間路0140?移動過程中，機(jī)器人末端的姿態(tài)需要保持不變(姿態(tài)角α

恒定)障

礙6.1

路徑與軌跡P

yP徑Q

yQ0YYQP?笛卡爾空間路徑是運(yùn)動的純幾何描述，與時間

無關(guān)?笛卡爾空間軌跡

[x

t

y

t

a

t]T

則是指定了時

間律的笛卡爾空間曲線?笛卡爾空間軌跡不僅包含機(jī)器人末端位姿信息，

還包含速度和加速度信息?通過逆運(yùn)動學(xué)，可以將笛卡爾空間路徑和笛卡

爾空間軌跡分別變換為關(guān)節(jié)空間路徑和關(guān)節(jié)空

間軌跡10T

t

tt6.1

路徑與軌跡tf0tt?路徑：

機(jī)器人位形的一個特定序列，而不考慮機(jī)器人位形的時間因素?軌跡：

與何時到達(dá)路徑中的每個部分有關(guān)，強(qiáng)調(diào)了時間性和連續(xù)性?軌跡規(guī)劃問題與數(shù)值分析中的函數(shù)插值問題有密切的聯(lián)系，不少插值算法

在機(jī)器人軌跡規(guī)劃中得到應(yīng)用?通常期望機(jī)器人的運(yùn)動是平滑的，為此，要求關(guān)節(jié)空間軌跡是一個連續(xù)的

且具有連續(xù)一階導(dǎo)數(shù)的光滑函數(shù)，有時還希望二階導(dǎo)數(shù)也是連續(xù)的?在某些軌跡規(guī)劃問題中，對路徑點(diǎn)的速度或加速度也有給定值6.1

路徑與軌跡?以關(guān)節(jié)角的函數(shù)來描述軌跡的軌跡生成方法?機(jī)械臂運(yùn)動軌跡的每個路徑點(diǎn)通常是用工具坐標(biāo)系相對于工作臺坐標(biāo)系的期望

位姿來確定的?通過逆運(yùn)動學(xué)方法，我們可以將每個路徑點(diǎn)轉(zhuǎn)化為一組期望的關(guān)節(jié)變量，從而

設(shè)計出一系列光滑函數(shù)，按照這些函數(shù)變化的關(guān)節(jié)變量確保機(jī)器人運(yùn)動軌跡經(jīng)

過各路徑點(diǎn)并最終達(dá)到目標(biāo)點(diǎn)?要求所有關(guān)節(jié)同時達(dá)到相應(yīng)的關(guān)節(jié)變量期望值，以在笛卡爾空間中獲得工具坐

標(biāo)系的預(yù)期路徑點(diǎn)EFEF6.2

關(guān)節(jié)空間規(guī)劃方法?為獲得一條確定的光滑運(yùn)動曲線，至少需要對

t

施加四個約束條件

0=0

tf

=f

?

0=0?

tf

=0?

這些約束條件唯一確定了一個三次多項(xiàng)式

t

=

a0

+

a1

t

+

a2

t

2

+

a3

t

36.2.1

三次多項(xiàng)式?對某個關(guān)節(jié)，要求在t0

時刻的初始角度為

0

，在tf時刻的目標(biāo)角度為

f?有無數(shù)條光滑曲線

t

符合上述要求6.2

關(guān)節(jié)空間規(guī)劃方法6.2.1

三次多項(xiàng)式?聯(lián)立求解4個方程，得到4個未知的數(shù)值，便可算出任意時刻的關(guān)節(jié)位置，控制器據(jù)此驅(qū)動關(guān)節(jié)到達(dá)所需的位置p

t

=

a0

+

a1

t

+

a2

t2

+

a3

t

3

p?

t

=

a1

+

a2

t

+

a3

t2p0

=

a0pf

=

a0

+

a1

tf

+

a2

tf2

+

a3

tf30

=

a10

=

a1

+

a2

tf

+

a3

tf2?

盡管每一關(guān)節(jié)是用同樣步驟分別進(jìn)行軌跡規(guī)劃的，但所有關(guān)節(jié)自始至終都是同步驅(qū)動a0

=

O0

a1

=

0

a2

=

Of

?

O0

a3

=

?

Of

?

O06.2

關(guān)節(jié)空間規(guī)劃方法6.2.1

三次多項(xiàng)式?例6-1:考慮機(jī)器人的某一關(guān)節(jié)，要求在1秒內(nèi)關(guān)節(jié)角從10°運(yùn)動到45°。要求

關(guān)節(jié)從靜止?fàn)顟B(tài)開始運(yùn)動，并終止在靜止?fàn)顟B(tài)。給出滿足以上要求的三次多項(xiàng)

式。已知

0

=10，

f

=5，

?0

=0，

?f

=0，tf

=1，可求得：a0

=10a1

=0a2

=105a3

=?70t=10+105t2

?70t36.2

關(guān)節(jié)空間規(guī)劃方法6.2.2

五次多項(xiàng)式?三次多項(xiàng)式可以指定t0

=0和tf時刻關(guān)節(jié)角度和速度，如果需要同時指定t0和tf時刻關(guān)節(jié)角的加速度，則可以采用五次多項(xiàng)式來規(guī)劃軌跡：

t=a0

+a1

t+a2

t

2

+a3

t3

+a4

t4

+a5

t5

?

t=a1

+a2

t+a3

t

2

+a4

t

3

+5a5

t4 t=a2

t+6a3

t

2

+1a4

t3

+0a5

t46.2

關(guān)節(jié)空間規(guī)劃方法a0

=

0a1

=

?00a2

=3

0

f

?

0

0?

8

?f

+

1

?0tf

?

0

?

ft=a5

=f2ta4

0

0?

0

f

+

1

?f

+

16

?0tf

+

0

?

ftf2f31

f

?

1

0?

6

?f

+

6

?0tf

?

0?

fttf5f2tf4=

0

=a0

f

=a0

+a1

tf

+a2

t+a3

t+a4

t

+a5

t

?0

=a1

?f

=a1

+a2

tf

+a3

t+a4

t+5a5

t0

=a2f

=a2

+6a3

tf

+1a4

t+0a5

tf3f2f4f3f2f5f4f3f2

t=a0

+a1

t+a2

t

2

+a3

t

3

+a4

t4

+a5

t5

?

t=a1

+a2

t+a3

t

2

+a4

t

3

+5a5

t4

t=a2

t+6a3

t

2

+1a4

t

3

+0a5

t46.2.2

五次多項(xiàng)式?方程組及求解結(jié)果6.2

關(guān)節(jié)空間規(guī)劃方法a6.2.2

五次多項(xiàng)式?例6-2:考慮機(jī)器人的某一關(guān)節(jié)，要求在1秒內(nèi)關(guān)節(jié)角從10°運(yùn)動到45°。要求

關(guān)節(jié)從靜止?fàn)顟B(tài)開始運(yùn)動，并終止在靜止?fàn)顟B(tài)。同時要求初始加速度和終止加

速度均為0°/s2

。給出滿足以上要求的五次多項(xiàng)式。已知

0

=10，

f

=5，

?0

=0，

?f

=0，

0

=0，

f

=0，tf

=1，可求得：

a0

=10a1

=0a2

=0a3

=50a4

=?55a5

=

10從而求得五次多項(xiàng)式

t=10+50t3

?55t4

+

10t56.2

關(guān)節(jié)空間規(guī)劃方法6.2.3

具有中間點(diǎn)的路徑的三次多項(xiàng)式?通過重復(fù)使用三次多項(xiàng)式和五次多項(xiàng)式，可以生成經(jīng)過關(guān)節(jié)中間點(diǎn)

1,2,3,…的關(guān)節(jié)角路徑a0

=0a1

=.0a2

=

(

f

一

0)

一

.0

一

.f

a3

=

(

f

一

0)

+

(

.0

+

.f)

0

=a0

f

=a0

+a1

tf

+a2

tf2

+a3

tf3

.0=a1

.f

=a1

+2a2

tf

+3a3tf2

(t0

)=

0

(tf)=

f

.(t0

)=.0

.(tf)=.f?這時在每個終止點(diǎn)的速度約束條件不再為零，而是已知的速度6.2

關(guān)節(jié)空間規(guī)劃方法?確定中間點(diǎn)的期望關(guān)節(jié)速度:?根據(jù)工具坐標(biāo)系的笛卡爾速度確定中間點(diǎn)的速度。通?？衫迷谥虚g點(diǎn)上計算

出的操作臂的雅克比逆矩陣，把中間點(diǎn)的笛卡爾期望速度“映射”為期望的關(guān)

節(jié)速度?如果操作臂在某個特定的中間點(diǎn)上處于奇異位形，則用戶將無法在該點(diǎn)處任意

指定速度，所以，中間點(diǎn)要盡可能避開奇異位形?用戶通常難以在中間點(diǎn)給出笛卡爾期望速度6.2

關(guān)節(jié)空間規(guī)劃方法將相鄰的關(guān)節(jié)中間點(diǎn)用直線相連，則該直線的斜率就是兩個相鄰關(guān)節(jié)中間

點(diǎn)的平均速度。如果某一關(guān)節(jié)中間點(diǎn)

前后兩段直線的斜率符號相反，則可

將該點(diǎn)的速度取為0，如

1

和

3

處如果某一關(guān)節(jié)中間點(diǎn)前后兩段直線的

斜率符號相同，則可將該點(diǎn)的速度取

為兩者的平均值，如

2

處?確定中間點(diǎn)的期望關(guān)節(jié)速度的方法之一:?6.2

關(guān)節(jié)空間規(guī)劃方法t0

t1

t2

t3

tf

t

?tf3021?確定中間點(diǎn)的期望關(guān)節(jié)速度的方法之二:

(樣條函數(shù))?不直接指定關(guān)節(jié)中間點(diǎn)處的速度，而是以保證相鄰兩段三次多項(xiàng)式加速度連續(xù)

為原則選取三次多項(xiàng)式系數(shù)?考慮三個相鄰的關(guān)節(jié)中間點(diǎn)，依次為Oi

，Oj和Ok?連接Oi和Oj的三次多項(xiàng)式為pij

t=a0

+a1

t+a2

t

2

+a3

t

3

t∈[0tf1]?連接Oj和Ok的三次多項(xiàng)式為pjk

t=b0

+b1

t+b2

t

2

+b3

t

3

t∈[0tf2]?注意：這里將第二段三次多項(xiàng)式的起始時間定為0，目的是簡化系數(shù)計算6.2

關(guān)節(jié)空間規(guī)劃方法a0

=

ia0

+a1

tf1

+a2

t

1

+a3

t

1

=jb0

=

jb0

+b1

tf2

+b2

t2

+b3

t2

=ka1

=?ib1

+b2

tf2

+

b3

t2

=?ka1

+a2

tf1

+a3

t

1

=b1

a2

+6a3

tf1

=b2f2f2f3f2f3f2

ij

0

=

i

ij

tf1=

j

jk

0

=

j

jk

tf2=

k

?ij

0=

?i

?jktf2

=?k

?ij

tf1

=

?jk

ij

tf1

=

jk?

兩個三次多項(xiàng)式需要滿足如下的條件:求解上述線性方程組即可得到a0

a1

a26.2

關(guān)節(jié)空間規(guī)劃方法帶入三次多項(xiàng)式可得:a3

和b0

b1

b2

b300?例:試求解兩個三次曲線的系數(shù)，使得兩線段連成的樣條曲線在中間點(diǎn)處具有

連續(xù)的加速度假設(shè)起始角為

O0

，中間點(diǎn)為Ov，目標(biāo)點(diǎn)為Og第一個三次曲線為

O

t=a10

+a11

t+a12

t2

+a13

t3第二個三次曲線為

O

t=a20

+a21

t+a22

t2

+a23

t3在一個時間段內(nèi)，每個三次曲線的起始時刻為t=0，終止時刻

t=tfi

，

其中

i=1或i=26.2

關(guān)節(jié)空間規(guī)劃方法?方程組及求解結(jié)果p0

=

a10pv

=

a10

+

a11

tf1

+

a12

tf1

2

+

a13

tf1

3pv

=

a20pg

=

a20

+

a21

tf2

+

a22

tf2

2

+

a23

tf2

30

=

a110

=

a21

+

a22

tf2

+

a23

tf2

2a11

+

a12

tf1

+

a13

tf1

2

=

a21

速度連續(xù)

a12

+

6a13tf1

=

a22

加速度連續(xù)

a10

=

O0a11

=

0a12

=a13

=a20

=

Ova21

=a22

=a23

=3Og

?

3O04tf?12Ov

+

6Og

+

6O04tf

28Ov

?

5Og

?

3O04tf

312Ov

?

3Og

?

9O04tf

2?8Ov

+

3Og

+

5O04tf

3O

t

=

a10

+

a11

t

+

a12

t2

+

a13

t3O

0

=

O0

O

tf1

=

Ov

O?

0

=

0O

t

=

a20

+

a21

t

+

a22

t2

+

a23

t3

O

0

=

Ov

O

tf2

=

Og

O?

tf2

=

06.2

關(guān)節(jié)空間規(guī)劃方法?主要的軌跡為線性函數(shù)，使機(jī)器人關(guān)節(jié)以

恒定速度在起點(diǎn)和終點(diǎn)位置之間運(yùn)動?為使起點(diǎn)和終點(diǎn)處速度連續(xù)，起點(diǎn)和終點(diǎn)

處用拋物線進(jìn)行連接，在連接區(qū)域內(nèi)加速

度恒定6.2.4

帶拋物線連接的線性函數(shù)6.2

關(guān)節(jié)空間規(guī)劃方法tf

tb

t

tbtf0tf0其中，

Ob

=

O0

+

O?tb

2是擬合段終點(diǎn)的O值，

O?是擬合段的加速度選定O?，則

上述解存在的條件是

O?足夠大，滿足:恒加速度

tb

=

tf

?

p?

≥

存在多個解，每個結(jié)果都對稱于時間中點(diǎn)th和位置中點(diǎn)Oh擬合區(qū)段終點(diǎn)的速度必須等于直線段的速度，即O?tb

=

?假設(shè)兩端的拋物線擬合區(qū)段具有相同的持續(xù)時間，在這兩個擬合區(qū)段中采用相同的

恒定加速度(符號相反)6.2

關(guān)節(jié)空間規(guī)劃方法已知tf

tbttbtf0tf06.2.4

帶拋物線連接的線性函數(shù)?例6-3:考慮機(jī)器人的某一關(guān)節(jié)，要求在5秒內(nèi)關(guān)節(jié)角從10°運(yùn)動到50°。要求關(guān)節(jié)從靜止?fàn)顟B(tài)開始運(yùn)動，并終止在靜止?fàn)顟B(tài)。過渡段的加速度幅值為10

°/s2

，給出滿足以上要求的帶拋物線過渡的直線段。已知0

=10，f

=50，=10，?0

=0，?f

=0，tf

=5，可求得:tf

?

2

t

?

f

?

0

10×50?102

×52

?×10× f2tb

==×

10

50

?

10=1

5段的拋物線：

t=50?55?t

2

=50?55?10t+t

2

=?75+50t?5t21段的直線方程：

t=15+10×t?1

=5+10t01段的拋物線：

t=10+5t26.2

關(guān)節(jié)空間規(guī)劃方法6.2.5

考慮關(guān)節(jié)中間點(diǎn)的帶拋物線過渡的直線段?前面的方法要求初始速度和最終速度均為零。如果路徑要求過中間點(diǎn)，則運(yùn)動

軌跡段有多個。機(jī)械臂運(yùn)動到第一個運(yùn)動段末端后，還將向下一點(diǎn)運(yùn)動，速度

并不為零，否則，機(jī)械臂將表現(xiàn)為時走時停的狀態(tài)?在各運(yùn)動段之間運(yùn)用拋物線過渡時，使用相鄰點(diǎn)的位置和時間間隔等邊界條件

計算運(yùn)動段的速度，設(shè)定加速度的大小，計算拋物線擬合的時間，從而得到運(yùn)

動段的軌跡。重復(fù)這一過程直至計算出所有運(yùn)動段并達(dá)到終點(diǎn)?注意：

需檢驗(yàn)加速度值是否超過限定6.2

關(guān)節(jié)空間規(guī)劃方法?用

j,

k

和

l

表示三個相鄰的路徑點(diǎn)?位于路徑點(diǎn)

k

處的擬合區(qū)段的時間間隔為tk?位于點(diǎn)

j

和

k

之間的直線段的時間間隔為tjk?點(diǎn)

j

和

k之間總的時間間隔為

tdjk?直線段的速度為

?jk?點(diǎn)j處擬合區(qū)段的加速度為

?j存在許多可能解，這取決于每個擬合區(qū)段的加速度值6.2

關(guān)節(jié)空間規(guī)劃方法d12djklkttjktltttji

k

?

jtdjkk

=SGN

?kl

??jk|k|

?kl

??jktk

=

ktjk

=

tdjk

?

tj

?

tk

?jk=?對于內(nèi)部路徑點(diǎn)，由加速度絕對值計算各時長的公式：6.2

關(guān)節(jié)空間規(guī)劃方法lk

td12

tdjk ttjktltji

?12

=

1

td12

?t1t12

=td12

?t1

?1t2

2

?

1t1

=td12

?t

12

?d2td12

?

1

t1

=

1

t11

=SGN

2

?1

1

?對于第一個路徑段，由

2

?

16.2

關(guān)節(jié)空間規(guī)劃方法可計算得到

t

d1djk進(jìn)而ji取2tjk11tl

ttttkl

.(n

一1)n

=t(n一1)n=td(n一1)n

一

tn

一

tn

一1tn

=

td(n

一1)n

一

t

(n

一1)n

一d22(

n

一

n

一1)

n進(jìn)而?對于最后一個路徑段，同樣由

n

=SGN(n

一1

一n)

n

=n

tn

和

td(n

一1)n

一

tn6.2

關(guān)節(jié)空間規(guī)劃方法td(n一1)n

一

tn

ntl

ln一1

n

一1一

n

n一

n

一1可計算得到tdjktd12

t

tjkei1tntkj?考慮兩直線p1

t=k1

t+p10p2

t=k2

t+p20?p1

t

和p2

t

交于t12

，有k1

t12

+p10

=k2

t12

+p20?解得小20?小10t12

=pk

?

pjtdjkp?k

=SGNp?kl

?p?jk

|p?k|

p?kl

?p?jktk

=

p?ktjk

=tdjk

?1

tj

?1

tkp?jk

=6.2

*關(guān)節(jié)空間規(guī)劃方法d1djk?lkk1

?k2?

t

tjktltttji?考慮拋物線

p

t=a

t?ta

2

+

a?p

t

和

1

t

交于t1

，有k1

t1

+10

=1

a

t1

?ta

2

+

ak1

=a

t1

?

ta

?p

t

和

2

t

交于t2

，有k2

t2

+20

=1

a

t2

?ta

2

+

ak2

=a

t2

?ta

6.2

*關(guān)節(jié)空間規(guī)劃方法d1djklk

t

tjktltttji?有四個方程，四個未知數(shù)t1

t2

ta

a?可求得

20

?10

k1

?k2t1

=

+

20

?10

k1

?k2t2

=

?

?可得

=

k1

?k2

2ak1

?k2

2ak1

t1

+10

=a

t1

?ta

2

+ak1

=a

t1

?

ta

k2

t2

+20

=a

t2

?ta

2

+

ak2

=a

t2

?

ta

t12

=6.2

*關(guān)節(jié)空間規(guī)劃方法d12djklkttjktltttji?若希望操作臂精確經(jīng)過某中間點(diǎn)，將該點(diǎn)替換為位于其兩側(cè)的兩個偽中間點(diǎn)，

該點(diǎn)在連接兩個偽中間點(diǎn)的直線段上，

計算仍用前面的公式?多數(shù)帶有拋物線連接的直線樣條曲線并沒有經(jīng)過那些中間點(diǎn)?選取加速度足夠大，則實(shí)際路徑與期望的中間點(diǎn)非常接近偽關(guān)節(jié)中間點(diǎn)

原關(guān)節(jié)中間點(diǎn)

偽關(guān)節(jié)中間點(diǎn)6.2

關(guān)節(jié)空間規(guī)劃方法tt?例6-4:考慮機(jī)器人的某一關(guān)節(jié)，指定以下關(guān)節(jié)路徑點(diǎn)：30

，45

，55

，30

(單位：

度)。各路徑段的持續(xù)時間分別為3

，1

，2

(單位：秒)。過渡段加速度幅值

為50°/s2

。計算各路徑段的速度、過渡段時間間隔以及直線段時間間隔。(1)

過渡段1和直線段12：過渡段加速度：

1

=SGN5?0×50=50°/s2過渡段持續(xù)時間：

t1

=?9?=0.101s直線段速度：

?12

==5.09°/s直線段時間間隔：t12

=?0.101?t2

=.899s

(這里t2

來自下面路徑段2的計算)6.2

關(guān)節(jié)空間規(guī)劃方法(2)

過渡段2

、直線段23

、過渡段3

直線段23速度：

?23

==10°/s過渡段2加速度：

2

=SGN

10?5.09×50°/s2

=50°/s2過渡段2持續(xù)時間：

t2

=

=

=

0.098

s過渡段3加速度：

3

=SGN?1.98?10×50°/s2

=?50°/s2

過渡段3持續(xù)時間：

t3

=

=

0.

68s直線段23間隔時間：

t23

=1?0.5×0.098?0.5×0.68=0.7169s(3)

直線段34和過渡段4過渡段4加速度：

4

=SGN0?554

°/s2

=?50°/s2過渡段4時間間隔：

t4

=??=0.68s直線段34速度：

?34

==?1.98°/s直線度34時間間隔：

t34

=?0.68?0.5×0.68=1.98s6.2

關(guān)節(jié)空間規(guī)劃方法直線插值線性插補(bǔ)：?位置分量(XYZ)三方向線性變化。?

姿態(tài)分量，如采用旋轉(zhuǎn)矩陣方式描述每

個中間點(diǎn)，那么將無法進(jìn)行線性插補(bǔ)。常見的軌跡包括了直線。其他還有圓弧、

正弦等形狀A(yù)6.3

笛卡兒空間軌跡規(guī)劃6.3.1

笛卡兒直線運(yùn)動指定一系列中間點(diǎn)B

AB?t∈[01](注意這里的起止時間做了歸一化)?機(jī)器人末端在笛卡爾空間中沿直線從P0運(yùn)動P1，同時姿態(tài)從R

0

平滑

變化到R

1給定末端的初始位姿T0

和終P

1

1?期望找出中間位姿T

t

R

t

止位姿T1，即R

00?初始位置P0=x0

y0

z0

T

以及終止位置P1=x1

y1

z1

T?以笛卡爾空間中機(jī)器人末端沿直線運(yùn)動為例，6.3

笛卡兒空間軌跡規(guī)劃6.3.1

笛卡兒直線運(yùn)動P

0

和

R

1

10P

t10=6.3.1

笛卡兒直線運(yùn)動?對于末端位置軌跡P

t

，可以運(yùn)用前面的多項(xiàng)式或帶拋物線過渡直線段的

插值方法來獲得?但這種方法不能直接用于對R

t

進(jìn)行插值，因?yàn)椴逯档玫降木仃囈话悴粷M

足旋轉(zhuǎn)矩陣的性質(zhì)?解決這一問題的一種方法是采用等效軸角表示姿態(tài)?姿態(tài)的等效軸角描述方法只需要3個數(shù)?x

y

z

T

為等效單位轉(zhuǎn)動軸?e為繞該軸的轉(zhuǎn)動量(這里單位取為°)?可以對等效軸角表示的三個數(shù)運(yùn)用前面的插值方法來獲得其軌跡6.3

笛卡兒空間軌跡規(guī)劃xyzkxkykz=

eK

=?表示圍繞空間

123T

/14軸旋轉(zhuǎn)450°獲得的姿態(tài)?該最終姿態(tài)也等于繞同一軸旋轉(zhuǎn)450+360n度的結(jié)果?n：任意整數(shù)6.3.1

笛卡兒直線運(yùn)動?注意：等效軸角表示并不唯一。舉例來說，6.3

笛卡兒空間軌跡規(guī)劃450°900°

1350°kxkykz/14=6.3.1

笛卡兒直線運(yùn)動k0x

k1x?對兩個等效軸角表示的姿態(tài)K0=k0y

和K1=k1y

插值時，遵循如下的選取規(guī)

k0z

k1z則：1x1y

運(yùn)用前面的多項(xiàng)式或帶拋物線過渡直線段等插

1zk0x

?然后對

k0y

和k0z1x1y

最小的n1z6.3

笛卡兒空間軌跡規(guī)劃?插值時，通常應(yīng)該選擇使得k0x

k0y

?k0z e+360n e+360n值方法6.3.2

姿態(tài)的四元數(shù)插值?笛卡爾空間中對姿態(tài)還可以采用如下的四元數(shù)插值?前面知識：

S3中的單位四元數(shù)數(shù)刀+ie1

+je2

+ke3

與U中的歐拉參數(shù)

刀

e1

e2

e3

T

一一對應(yīng)?考慮兩個用歐拉參數(shù)(等價于用單位四元數(shù))表示的不同姿態(tài)：

r0

=

刀

e1

e2

e3

Tr1

=新616263T?r0

≠r1

且r0

≠?r1?四元數(shù)插值目的：找出中間姿態(tài)rt

，t

e[01]，使得r0平滑過渡到r1?注意：這里起止時間t作了歸一化6.3

笛卡兒空間軌跡規(guī)劃?注意到

r0

=r1

=1 r0

?r1

2

=刀?新2

+E1

?612

+E2

?622

+E3

?632?=刀2

+E+E+E+新2

+6+6+6?2刀新+E161

+E262

+E363

=r0

2

+r1