机器人建模与控制课件：机器人轨迹规划

机器人建模与控制

机器人轨迹规划⚫机器人将盛满水的杯子由P移到Q⚫因存在障碍物，机器人末端不能简单地沿直线段PQ移动，而是需要绕过障碍

物，例如沿图中的一系列红点移动⚫末端位姿形成了笛卡尔空间中以

TP

=cos

asin

a00−sinacos

a000010xPyP01为首、40cos

a以

TQ

=

sin

a0−sinacos

a000010xQyQ

为尾的位形序列，即笛卡尔空间路0140⚫移动过程中，机器人末端的姿态需要保持不变(姿态角α

恒定)障

碍6.1

路径与轨迹P

yP径Q

yQ0YYQP⚫笛卡尔空间路径是运动的纯几何描述，与时间

无关⚫笛卡尔空间轨迹

[x

t

y

t

a

t]T

则是指定了时

间律的笛卡尔空间曲线⚫笛卡尔空间轨迹不仅包含机器人末端位姿信息，

还包含速度和加速度信息⚫通过逆运动学，可以将笛卡尔空间路径和笛卡

尔空间轨迹分别变换为关节空间路径和关节空

间轨迹10T

t

tt6.1

路径与轨迹tf0tt⚫路径：

机器人位形的一个特定序列，而不考虑机器人位形的时间因素⚫轨迹：

与何时到达路径中的每个部分有关，强调了时间性和连续性⚫轨迹规划问题与数值分析中的函数插值问题有密切的联系，不少插值算法

在机器人轨迹规划中得到应用⚫通常期望机器人的运动是平滑的，为此，要求关节空间轨迹是一个连续的

且具有连续一阶导数的光滑函数，有时还希望二阶导数也是连续的⚫在某些轨迹规划问题中，对路径点的速度或加速度也有给定值6.1

路径与轨迹⚫以关节角的函数来描述轨迹的轨迹生成方法⚫机械臂运动轨迹的每个路径点通常是用工具坐标系相对于工作台坐标系的期望

位姿来确定的⚫通过逆运动学方法，我们可以将每个路径点转化为一组期望的关节变量，从而

设计出一系列光滑函数，按照这些函数变化的关节变量确保机器人运动轨迹经

过各路径点并最终达到目标点⚫要求所有关节同时达到相应的关节变量期望值，以在笛卡尔空间中获得工具坐

标系的预期路径点EFEF6.2

关节空间规划方法⚫为获得一条确定的光滑运动曲线，至少需要对

t

施加四个约束条件

0=0

tf

=f

ሶ

0=0ሶ

tf

=0⚫

这些约束条件唯一确定了一个三次多项式

t

=

a0

+

a1

t

+

a2

t

2

+

a3

t

36.2.1

三次多项式⚫对某个关节，要求在t0

时刻的初始角度为

0

，在tf时刻的目标角度为

f⚫有无数条光滑曲线

t

符合上述要求6.2

关节空间规划方法6.2.1

三次多项式⚫联立求解4个方程，得到4个未知的数值，便可算出任意时刻的关节位置，控制器据此驱动关节到达所需的位置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⚫

尽管每一关节是用同样步骤分别进行轨迹规划的，但所有关节自始至终都是同步驱动a0

=

O0

a1

=

0

a2

=

Of

−

O0

a3

=

−

Of

−

O06.2

关节空间规划方法6.2.1

三次多项式⚫例6-1:考虑机器人的某一关节，要求在1秒内关节角从10°运动到45°。要求

关节从静止状态开始运动，并终止在静止状态。给出满足以上要求的三次多项

式。已知

0

=10，

f

=5，

ሶ0

=0，

ሶf

=0，tf

=1，可求得：a0

=10a1

=0a2

=105a3

=−70t=10+105t2

−70t36.2

关节空间规划方法6.2.2

五次多项式⚫三次多项式可以指定t0

=0和tf时刻关节角度和速度，如果需要同时指定t0和tf时刻关节角的加速度，则可以采用五次多项式来规划轨迹：

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

关节空间规划方法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

五次多项式⚫方程组及求解结果6.2

关节空间规划方法a6.2.2

五次多项式⚫例6-2:考虑机器人的某一关节，要求在1秒内关节角从10°运动到45°。要求

关节从静止状态开始运动，并终止在静止状态。同时要求初始加速度和终止加

速度均为0°/s2

。给出满足以上要求的五次多项式。已知

0

=10，

f

=5，

ሶ0

=0，

ሶf

=0，

0

=0，

f

=0，tf

=1，可求得：

a0

=10a1

=0a2

=0a3

=50a4

=−55a5

=

10从而求得五次多项式

t=10+50t3

−55t4

+

10t56.2

关节空间规划方法6.2.3

具有中间点的路径的三次多项式⚫通过重复使用三次多项式和五次多项式，可以生成经过关节中间点

1,2,3,…的关节角路径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⚫这时在每个终止点的速度约束条件不再为零，而是已知的速度6.2

关节空间规划方法⚫确定中间点的期望关节速度:⚫根据工具坐标系的笛卡尔速度确定中间点的速度。通常可利用在中间点上计算

出的操作臂的雅克比逆矩阵，把中间点的笛卡尔期望速度“映射”为期望的关

节速度⚫如果操作臂在某个特定的中间点上处于奇异位形，则用户将无法在该点处任意

指定速度，所以，中间点要尽可能避开奇异位形⚫用户通常难以在中间点给出笛卡尔期望速度6.2

关节空间规划方法将相邻的关节中间点用直线相连，则该直线的斜率就是两个相邻关节中间

点的平均速度。如果某一关节中间点

前后两段直线的斜率符号相反，则可

将该点的速度取为0，如

1

和

3

处如果某一关节中间点前后两段直线的

斜率符号相同，则可将该点的速度取

为两者的平均值，如

2

处⚫确定中间点的期望关节速度的方法之一:⚫6.2

关节空间规划方法t0

t1

t2

t3

tf

t

⚫tf3021⚫确定中间点的期望关节速度的方法之二:

(样条函数)⚫不直接指定关节中间点处的速度，而是以保证相邻两段三次多项式加速度连续

为原则选取三次多项式系数⚫考虑三个相邻的关节中间点，依次为Oi

，Oj和Ok⚫连接Oi和Oj的三次多项式为pij

t=a0

+a1

t+a2

t

2

+a3

t

3

t∈[0tf1]⚫连接Oj和Ok的三次多项式为pjk

t=b0

+b1

t+b2

t

2

+b3

t

3

t∈[0tf2]⚫注意：这里将第二段三次多项式的起始时间定为0，目的是简化系数计算6.2

关节空间规划方法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⚫

两个三次多项式需要满足如下的条件:求解上述线性方程组即可得到a0

a1

a26.2

关节空间规划方法带入三次多项式可得:a3

和b0

b1

b2

b300⚫例:试求解两个三次曲线的系数，使得两线段连成的样条曲线在中间点处具有

连续的加速度假设起始角为

O0

，中间点为Ov，目标点为Og第一个三次曲线为

O

t=a10

+a11

t+a12

t2

+a13

t3第二个三次曲线为

O

t=a20

+a21

t+a22

t2

+a23

t3在一个时间段内，每个三次曲线的起始时刻为t=0，终止时刻

t=tfi

，

其中

i=1或i=26.2

关节空间规划方法⚫方程组及求解结果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

速度连续

a12

+

6a13tf1

=

a22

加速度连续

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

关节空间规划方法⚫主要的轨迹为线性函数，使机器人关节以

恒定速度在起点和终点位置之间运动⚫为使起点和终点处速度连续，起点和终点

处用抛物线进行连接，在连接区域内加速

度恒定6.2.4

带抛物线连接的线性函数6.2

关节空间规划方法tf

tb

t

tbtf0tf0其中，

Ob

=

O0

+

Oሷtb

2是拟合段终点的O值，

Oሷ是拟合段的加速度选定Oሷ，则

上述解存在的条件是

Oሷ足够大，满足:恒加速度

tb

=

tf

−

pሷ

≥

存在多个解，每个结果都对称于时间中点th和位置中点Oh拟合区段终点的速度必须等于直线段的速度，即Oሷtb

=

⚫假设两端的抛物线拟合区段具有相同的持续时间，在这两个拟合区段中采用相同的

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

关节空间规划方法已知tf

tbttbtf0tf06.2.4

带抛物线连接的线性函数⚫例6-3:考虑机器人的某一关节，要求在5秒内关节角从10°运动到50°。要求关节从静止状态开始运动，并终止在静止状态。过渡段的加速度幅值为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

关节空间规划方法6.2.5

考虑关节中间点的带抛物线过渡的直线段⚫前面的方法要求初始速度和最终速度均为零。如果路径要求过中间点，则运动

轨迹段有多个。机械臂运动到第一个运动段末端后，还将向下一点运动，速度

并不为零，否则，机械臂将表现为时走时停的状态⚫在各运动段之间运用抛物线过渡时，使用相邻点的位置和时间间隔等边界条件

计算运动段的速度，设定加速度的大小，计算抛物线拟合的时间，从而得到运

动段的轨迹。重复这一过程直至计算出所有运动段并达到终点⚫注意：

需检验加速度值是否超过限定6.2

关节空间规划方法⚫用

j,

k

和

l

表示三个相邻的路径点⚫位于路径点

k

处的拟合区段的时间间隔为tk⚫位于点

j

和

k

之间的直线段的时间间隔为tjk⚫点

j

和

k之间总的时间间隔为

tdjk⚫直线段的速度为

ሶjk⚫点j处拟合区段的加速度为

ሷj存在许多可能解，这取决于每个拟合区段的加速度值6.2

关节空间规划方法d12djklkttjktltttji

k

−

jtdjkk

=SGN

ሶkl

−ሶjk|k|

ሶkl

−ሶjktk

=

ktjk

=

tdjk

−

tj

−

tk

ሶjk=⚫对于内部路径点，由加速度绝对值计算各时长的公式：6.2

关节空间规划方法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

关节空间规划方法可计算得到

t

d1djk进而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进而⚫对于最后一个路径段，同样由

n

=SGN(n

一1

一n)

n

=n

tn

和

td(n

一1)n

一

tn6.2

关节空间规划方法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

*关节空间规划方法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

*关节空间规划方法d1djklk

t

tjktltttji•有四个方程，四个未知数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

*关节空间规划方法d12djklkttjktltttji⚫若希望操作臂精确经过某中间点，将该点替换为位于其两侧的两个伪中间点，

该点在连接两个伪中间点的直线段上，

计算仍用前面的公式⚫多数带有抛物线连接的直线样条曲线并没有经过那些中间点⚫选取加速度足够大，则实际路径与期望的中间点非常接近伪关节中间点

原关节中间点

伪关节中间点6.2

关节空间规划方法tt⚫例6-4:考虑机器人的某一关节，指定以下关节路径点：30

，45

，55

，30

(单位：

度)。各路径段的持续时间分别为3

，1

，2

(单位：秒)。过渡段加速度幅值

为50°/s2

。计算各路径段的速度、过渡段时间间隔以及直线段时间间隔。(1)

过渡段1和直线段12：过渡段加速度：

1

=SGN5−0×50=50°/s2过渡段持续时间：

t1

=−9−=0.101s直线段速度：

ሶ12

==5.09°/s直线段时间间隔：t12

=−0.101−t2

=.899s

(这里t2

来自下面路径段2的计算)6.2

关节空间规划方法(2)

过渡段2

、直线段23

、过渡段3

直线段23速度：

ሶ23

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

2

=SGN

10−5.09×50°/s2

=50°/s2过渡段2持续时间：

t2

=

=

=

0.098

s过渡段3加速度：

3

=SGN−1.98−10×50°/s2

=−50°/s2

过渡段3持续时间：

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

关节空间规划方法直线插值线性插补：⚫位置分量(XYZ)三方向线性变化。⚫

姿态分量，如采用旋转矩阵方式描述每

个中间点，那么将无法进行线性插补。常见的轨迹包括了直线。其他还有圆弧、

正弦等形状A6.3

笛卡儿空间轨迹规划6.3.1

笛卡儿直线运动指定一系列中间点B

AB⚫t∈[01](注意这里的起止时间做了归一化)⚫机器人末端在笛卡尔空间中沿直线从P0运动P1，同时姿态从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⚫以笛卡尔空间中机器人末端沿直线运动为例，6.3

笛卡儿空间轨迹规划6.3.1

笛卡儿直线运动P

0

和

R

1

10P

t10=6.3.1

笛卡儿直线运动⚫对于末端位置轨迹P

t

，可以运用前面的多项式或带抛物线过渡直线段的

插值方法来获得⚫但这种方法不能直接用于对R

t

进行插值，因为插值得到的矩阵一般不满

足旋转矩阵的性质⚫解决这一问题的一种方法是采用等效轴角表示姿态⚫姿态的等效轴角描述方法只需要3个数⚫x

y

z

T

为等效单位转动轴⚫e为绕该轴的转动量(这里单位取为°)⚫可以对等效轴角表示的三个数运用前面的插值方法来获得其轨迹6.3

笛卡儿空间轨迹规划xyzkxkykz=

eK

=⚫表示围绕空间

123T

/14轴旋转450°获得的姿态⚫该最终姿态也等于绕同一轴旋转450+360n度的结果⚫n：任意整数6.3.1

笛卡儿直线运动⚫注意：等效轴角表示并不唯一。举例来说，6.3

笛卡儿空间轨迹规划450°900°

1350°kxkykz/14=6.3.1

笛卡儿直线运动k0x

k1x⚫对两个等效轴角表示的姿态K0=k0y

和K1=k1y

插值时，遵循如下的选取规

k0z

k1z则：1x1y

运用前面的多项式或带抛物线过渡直线段等插

1zk0x

⚫然后对

k0y

和k0z1x1y

最小的n1z6.3

笛卡儿空间轨迹规划⚫插值时，通常应该选择使得k0x

k0y

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

姿态的四元数插值⚫笛卡尔空间中对姿态还可以采用如下的四元数插值⚫前面知识：

S3中的单位四元数数刀+ie1

+je2

+ke3

与U中的欧拉参数

刀

e1

e2

e3

T

一一对应⚫考虑两个用欧拉参数(等价于用单位四元数)表示的不同姿态：

r0

=

刀

e1

e2

e3

Tr1

=新616263T⚫r0

≠r1

且r0

≠−r1⚫四元数插值目的：找出中间姿态rt

，t

e[01]，使得r0平滑过渡到r1⚫注意：这里起止时间t作了归一化6.3

笛卡儿空间轨迹规划⚫注意到

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