7、机器人运动规划

60T B0T BPi E6T 1 60T B0T BPi1E6T 1
60T B0T BPi D()E6T 1
D()：归一化时间 的函数， t
T
1）在节点 Pi ，t=0， 0， 则
自运动开始的实际时间 该运动轨迹的总时间
60T B0T BPi D()E6T 1 60T B0T BPi E6T 1
f
a1 2a2t f

3a3t
2
f
求解可得三次多项式的系数
a0 0
a1 0
a2

3 t2
f
( f
0)
2 tf

0
1 tf

f
a3
2 t3
f
( f
0)
1 t2
f

(0 f )
2、例：一旋转关节在3秒内从起始点 0 15o运动到终止点 f 75o ，起始和终止的速度皆为零，求关节的三次多项式插值函数。
《机器人学》
第六章、机器人轨迹规划
战强
北京航空航天大学机器人研究所
第六章、机器人轨迹规划
运动学 动力学
轨迹规划
关节 操作 空间 空间
控制
反馈 轨迹 控制 控制
力 协调 控制 控制
轨迹：操作臂在运动过程中每时每刻的位置、速度和加速度。
轨迹规划：根据作业任务要求计算出预期的运动轨迹，分为关节 空间规划和操作空间规划两类。
Z9 9 X9
185 8 Z8
X7 X8
Z7
7
140
160 X6
6
Z6
X5
135
X4
5
Z5
Z4
180
4
200
X3
X2
3
Z3
220 Z2 2
X0X1
165
Z0Z1 d1
1 0 0 0
01T

0 0
0
1 0 0
0 1 0
0

d1 1

cos2 sin2 0 0
算法简单描述： 1）、通过已知直线始末两点的位置与姿态，计算中间插补点的位置 与姿态，这中间包括确定末端速度、加速度，插补点数，确定每次 运动的步长，最后确定每个中间点的位置与姿态(即求出末端T阵)； 2）、由前后两个插补点的位姿，求出末端微分运动矢量dp，再采用 已选好的运动学逆解算法，反解出每步运动的关节变量的增量dq， 由公式 qk1 qk dqk1即可求出每次插补所需的关节变量值，循环直 到插补结束。算法的流程如下图。 3）、把每次计算出的关节位置变量值周期发送给机器人的伺服控制 器，即可控制机器人按预定轨迹运动。
0
a(i1) x a(i1) y a(i1) z
0
p(i1)x
p(i
1)
y

p(i
1)
z

1
把 Pi 和 Pi1 的齐次表达式代入下式
D(1)
BP 1B i
Pi
1
ni ni1 ni oi1 ni ai1 ni ( pi1 pi )
开始 速度曲线
步长
关节空间：已知初始和终止关节角
插补：
通常采用三次多项式作 为平滑函数
操作空间：已知初始和终止末端位姿 分直线插补和圆弧插补
末端位姿
操作空间的直线轨迹插补方法如下：
运动学反解（q:9*1向量） q(k+1)=q(k)＋dq(k＋1)
k++, N:插补点数
No
K>=N
Yes
结束
算法1流程图
1）设定末端运动的速度v，加速度a， 及插补的时间t，即机器人的伺服周期， 通常为20ms。 2）计算两点之间的直线的距离
n a
点 Pi 和 Pi1 相对于目标坐标系{B}的位姿可用齐次矩阵表示：
nix oix aix pix
pi

niy niz
oiy oiz
aiy aiz
piy

piz

0 0 0 1
n(i1) x
pi 1

n(i1) y

n(i
1)
z
0
o(i1) x o(i1) y o(i1) z
BP0 WBT 1WI T I P0
由节点 P0 运动到 P1 ，相当于 60T 从
60T W0T 1WBTBP0E6T 1B0TBP0E6T 1
变化到
60T W0T 1WBTBP1E6T 1B0TBP1E6T 1
从任意节点 Pi 到下一结点 Pi1 的运动可表示为：
c os 9
89T


0

s
in 0
9
sin 9 0
c os 9 0
0 0
1 185
0 0
0
1

nx ox ax px
正向运动学方程：
90T
(q1 ,
q2 ,,
q9
)01T
(q1 )98 T
(q8
)

n y nz
oy oz
ay az
p
y

p
z

0 0 0 1
2、确定逆解算法：分解速度法/反变换法
分解速度法

q
W
1J ET
(J EW
1J ET
) 1

p
1
H (q)
,
Wi


qi

1,
if
if H (q) 0 qi
H (q) 0 qi
H (q) (qmax [i] qmin [i])2 (2q[i] qmax [i] qmin [i])
f
速度约束：




（0）0

（t f ) f
唯一确定一个三次多项式
(t) a0 a1t a2t2 a3t3
代入四个约束条件可得到关于三次多项式系数的四个方程：
0 a0
f
a0 a1t f

a2t
2
f

a3t
3
f
0 a1
(t) a0 a1t a2t2 a3t3
θ
a0 0
a1 0
a2

3 t2
f
( f
0)
2 tf

0
1 tf

f
a3


2 t3
f
( f
0)
1 t2
f

(0 f )
过路径点的三次多项式插值函数
t 关节角轨迹光滑
z Z0
{W}
P0 · P1·
{B}
P0 ：60T W0T 1WI T I P0E6T 1 P1 ：60T W0T 1WBTBP0E6T 1
由每一个节点可求出 一个相应的手臂变换 矩阵 60T
假设节点 P0 在坐标系{B}中的描述为 BP0 ，则
60T W0T 1WBTBP0E6T 1 由于 60T W0T 1WI TI P0E6T 1
1、三次多项式插值：
通过起始点关节角和终止点关节角 θ 的运动轨迹可用一个光滑插值函数
(t) 来表示。
f
0
为实现单关节的平稳运动，轨迹函数 (t) 至少满足四个约束条件，两个端 点的角度约束和两个端点的速度约束。
t 关节角轨迹光滑
角度约束：
（0）0 （t f )
第二个转动饶a轴线转动使方向矢量o对准；移动是使把i点的坐
标原点移动到i+1点。
这样就利用驱动变换D()来控制一个移动和两个转动生成两个 结点之间的运动轨迹。
60T ()B0TBPi D()E6T 1
虽然笛卡儿空间中的路径形状与关节空间中的路径有连续 对应的关系，但直角空间中的路径易出现与工作空间和奇异 点有关的问题。
P0
P1
P3
P2
P5 P4
P6
销钉插孔作业
如果两个点不是在同一坐标系下描述的，需变换到同一坐标系 下，如全局坐标系{W}。
假设节点 P0 是相对于局部 坐标系{I}描述的，P1 是 相对于工作台坐标系{B}描 述的，则
P0 ：W0T 60T E6T WIT IP0
P1 ：W0T 60T E6T WBT BP0
D(0)是4*4的单位阵
2）在节点Pi1 ，t=T， 1，则 BPi D(1)BPi1
D(1)
BP 1B i
Pi
1
规定抓手坐标系三个轴用n, o, a来表示， 坐标原点用P表示。
a：接近矢量，表示抓手 接近物体的方向；
o：方向矢量，表示两手 指连线的方向； o
n o a：法向矢量；
qi
4(qmax [i] q[i])2 (q[i] qmin [i])2
3、轨迹规划/控制
设置末端起始点位置和姿态坐标为（0，－110，1675，90，0，0），终点为 （1060，280，1465，45，45，90）；前三个是位置坐标，后三个是姿态坐 标，是相对于初始参考姿态的Z-Y-Z欧拉角。
c os 4 0
0 0
1 380
0 0
0
1

c os 5
45T


0

s
in 0
5
sin5 0
c os 5 0
0 0 1 0 0 0 0 1
cos6
56T

sin 6 0

0
sin 6 cos6
0
0
0 135
运动在笛卡尔坐标中是直观的，容易定义，但是它需要对机 械手的定位点进行不断的求值，把它变换成各个关节坐标的 运动。
在笛卡儿空间中，机器人末端抓手的位姿可用一系列的节点 表示，轨迹规划的首要问题是在路径起始点和终点之间如何 生成一系列的中间点。
1、一个典型的笛卡儿空间的任务
INIT P0 原位 MOVE P1 接近销钉 MOVE P2 移动到销钉的位置 GRASP 抓住销钉 MOVE P3 垂直提起销钉 MOVE P4 按一定角度接近孔眼 MOVE P5 插入销钉 RELEASE 松开销钉 MOVE P6 离开
a3

2 t3
f
( f
0)

4.44
可得三次多项式的关节插值函数为： (t) 15 20t2 4.44t3 (t) 40t 13.32t2 (t) 40 26.64t
角
ቤተ መጻሕፍቲ ባይዱ
角
度
速
度
时间
角 加 速 度
时间
时间
任何三次多项式函数的速度曲线皆为抛物线，加速度曲线为直线。
▲如起始点和终止点都在工作空间内，但路径上的某些点不在 工作空间内，如果直线运动,
A
则轨迹不可实现，但在关节 空间中规划则容易实现。
B
6-3 运动控制范例
示例：说明如何对一个实际机器人进行运动学的建模、轨 迹规划和控制机器人的运动。
一个放在移动导轨上的模块机器人（9DOF）
1、建立D-H坐标系、计算变换矩阵
如果对运动轨迹的要求更严格，约束条件更多，则三次多项式 不能满足需要，需要用更高阶多项式进行插值，如五次多项式。
▼关节空间函数的光滑性并不表示操作空间运动的光滑性
6-2 操作空间（笛卡儿空间）的轨迹规划
笛卡儿空间轨迹规划的特点：
笛卡尔运动可以非常简单地推广到圆柱坐标、球坐标以及 其他正交坐标系统。
0
0

1 0
0
1

c os 7
67T


0

s
in 0
7
sin 7 0
c os 7 0
0 0
1 300
0 0
0
1

cos8 sin 8 0 0
78T


0

s
in 0

8
0 cos8
0
1 0 0
0 0 1
a0 0
a1 0
a2

3 t2
f
( f
0)
2 tf

0
1 tf

f

2
1
a3


t3 f
( f
0 ) t2 f
(0 f )
可得三次多项式的系数为：
a0 15
a1 0
a2

3 t2
f
( f
0 ) 20
D()

oi ai

ni 1 ni 1
0
oi oi1 ai oi1
0
oi ai1 ai ai1
0
oi
( pi1

pi
)

ai

(
pi 1

pi
)
1
o n
a
则抓手坐标系从点 Pi 到点Pi1 的运动包括一个平移运动和两个 旋转运动：第一个转动使抓手坐标系轴线与预期的方向a对准，
θ
t
关节轨迹
P0 P1 P3
P2 P4 P5 P6
操作空间轨迹
6-1 关节空间轨迹规划
以关节角度（位置）函数描述机器人轨迹：计算简单、无奇异性。
θ
确定路径点
反解关节值
每个关节运 动时间相同
光滑函数拟 合每个关节
t 某关节的反解值（线性化）
▼关键要使关节轨迹满足约束条件，如各点上的位姿、速度和 加速度要求和连续性要求等，在满足约束条件下选取不同的 插值函数。
21T


0

s
in 0

2
0 cos2
0
1 0 0
385
0
1

cos3
23T


0 sin3
0
sin3 0
cos3 0
0 0
1 110
0 0
0
1

c os 4
34T


0

sin 0
4
sin4 0