第6章 轨迹规划

和 t1 2 ：
根据上式便可求得：
机电学院机械电子工程系
1 sgn( 2 1 ) 1 2( 2 1 ) 2 t1 td 12 td 12 1 2 1 12 1 td 12 t1 2 1 t12 td 12 t1 t2 2
线性域

2
t 2 4 ( 2


f
0)
需满足条件：

当
4( f 0 ) t2 f
4 (
f
0) t
2
过渡域
时，无直线段。加速度越大，抛物线过渡段越短。
14
机电学院机械电子工程系
机电学院机械电子工程系
15
南京航空航天大学
6.2 关节轨迹的插值
ni oi 1 oi oi 1 ai oi 1 0 ni ai 1 oi ai 1 ai ai 1 0
在Pi ，t=0 ，则λ=0，D(0)是4×4的单位矩阵， 则下面的两个式子相同：
0 6 0 6
oix oiy oiz 0
aix aiy aiz 0
B
Ti 0 B T
a0 0 a1 0 2 a2 0 2 )t 2 20 f 20 0 (8f 120 )t f (3 0 f f a 3 2t 3 f 2 )t 2 30 f 30 0 (14f 160 )t f (3 0 f f a4 3 2 t f 2 a 12 f 12 0 (6 f 6 0 )t f ( 0 f )t f 3 5 2 t f
0 0
8
速度约束
a0 a1 0 a2 a3
a 0 0 a1 0 3 2 1 a2 2 ( f 0 ) 0 f tf tf tf 2 1 a3 3 ( f 0 ) 2 ( f 0 ) tf tf
关节速度和加速度：
（2）过路径点的三次多项式插值
(t ) 2 a 2 5 a 3t
关于四个系数的线性方程：


(t ) a 2a t 3a t 2 令 (t ) a0 a1t a2t 2 a3t 3 1 2 3

0
a0
f 3 a 2t 2 f a 3t f
两个结点之间的“直线”运动
两个结点之间的“直线”运动 如上图所示，从Pi到Pi+1 的运动可表示为： 从 0 0 B 1 Pi 6 6 Ti B T ET 到
0 6
从Pi到Pi+1的运动可由“驱动变换” D(λ)来表示：
0 6
T ( ) 0 B T
B
Pi D ( ) 6 E T
1
T i 1 0 B T
机电学院机械电子工程系
13
2
南京航空航天大学
6.2 关节轨迹的插值
用抛物线过渡的线性插值
南京航空航天大学
6.2 关节轨迹的插值
（5）过路径点的用抛物线过渡的线性插值

任意给定 f 、 0 和 t ，选择相应的 和 t b ，得到 路径曲线。
t tb 2
相邻路径点利用直线连接，路径点附近利用抛物线过渡。
机电学院机械电子工程系
机电学院机械电子工程系
23
南京航空航天大学
6.3 直角坐标空间规划
南京航空航天大学
6.3 直角坐标空间规划
oi 0 ai 0 nix pi n iy 1 niz 0 ni 1x pi 1 n i 1 y 1 ni 1z 0
6 t 4 2 3 f t 2 ( f 0 )t 2 ( f 0 )(1 )t 0t t2 tf tf tf tf f
6 t ( f 0 )(1 )t 20 (3t 2 4t f t ) 2f (3t 2 2t f t ) t2 t t tf f f f
B
Pi 1 6 ET
1
t T ， [ 0， 1]
D(λ) ：归一化时间λ的函数。 t ：自运动开始算起的时间。 T ：走过该轨迹的总时间。
22
6 E
T ：工具坐标系{T}相对于末端连杆系{6}的变换。
B
Pi 和 B Pi 1：分别为两个结点Pi和Pi+1相对于坐标系{B}的 齐次坐标。
n sgn( n 1 n ) n




18
机电学院机械电子工程系
19
3
南京航空航天大学
6.3 直角坐标空间规划
直角坐标空间的插值计算
南京航空航天大学
6.3 直角坐标空间规划
直角坐标空间的插值计算
物体对象的描述 在给出物体的几何图形及固接坐标后，其相对 于参考系的位姿则可用与它固接的坐标系来表示。
B
Pi 6 ET
B
1
n Pi i 0
pix piy piz 1 ai 1x ai 1 y ai 1z 0 pi 1x pi 1 y pi 1z 1
Ti ( ) 0 B T
Pi D ( ) 6 E T


由上确定了一个三次多项式：
( t ) a 0 a 1t a 2 t 2 a 3 t 3
机电学院机械电子工程系
6
机电学院机械电子工程系
7
1
南京航空航天大学
6.2 关节轨迹的插值
(t ) a1 2 a 2t 3 a 3t 2
南京航空航天大学
6.2 关节轨迹的插值
2( n n 1 ) 2 tn td ( n 1) n td ( n 1) n n1 n n 1 ( n 1) n 1 td ( n 1) n tn 2 1 t( n 1) n td ( n 1) n tn tn 1 2
机电学院机械电子工程系
3 2 ( f 0 ) tf 2 3 ( f 0 ) tf
0
3 2 1 2 1 f ]t 3[ 3 ( f 0 ) 2 ( f 0 )]t 2 ( f 0 ) 0 t2 tf tf tf tf f
9
机电学院机械电子工程系
南京航空航天大学
6.2 关节轨迹的插值
过路径点的三次多 关节轨迹的插值
（3）高阶多项式插值
(t ) a0 a1t a2t 2 a3t 3 a4t 4 a5t 5
(t ) a1 2a2t 3a3t 2 4a4t 3 5a5t 4
关节轨迹的插值计算
南京航空航天大学
6.2 关节轨迹的插值
三次多项式插值
关节轨迹的插值计算
（1）三次多项式插值 四个约束条件：
给定关节空间的起始角度和目标 角度，通过插值计算中间时刻的 关节角度
(0 ) 0 (t f ) f
(0 ) 0
(t f ) 0

jk

k
t d jk

j
k
s g n ( k l
jk
) k

tk
j ：j点过渡域的加速度。
机电学院机械电子工程系
16

kl k
jk
t jk t d jk
机电学院机械电子工程系
1 1 t j tk 2 2

路径点的关节速度的确定： 根据工具坐标系在直角坐标空间中的瞬时线速度和角速度确定 在直角坐标空间或关节空间中采用适当的启发式方法，由控制系 统自动选择 为保证每个路径点的加速度连续，由控制系统按此要求自动选择
6个约束条件
(0) 0 (t ) f f (0) 0 (t f ) f (0) 0 (t f ) f
11
机电学院机械电子工程系
10
机电学院机械电子工程系
南京航空航天大学
6.2 关节轨迹的插值
南京航空航天大学
6.2 关节轨迹的插值
用抛物线过渡的线性插值
（4）用抛物线过渡的线性插值 将线性函数与两段抛物线函数平滑地衔接在一起形成一 段轨迹。
用抛物线过渡的线性插值中间段 利用直线插值，两端利用抛物线 为已知，求 tb 过渡。一般
南京航空航天大学
目 录
南京航空航天大学
6.1 轨迹规划的一般性问题
轨迹规划
6.1 轨迹规划的一般性问题 6.2 关节轨迹的插值 6.3 直角坐标空间规划方法 6.4 轨迹空间的实时生成
轨 迹 ：机械手的位移，速度和加速度。 轨迹规划 ：由任务要求，计算出预算的运动轨迹。 轨迹规划器：简化了编程手续。 ◆机器人规划方式的分类
机电学院机械电子工程系
2
机电学院机械电子工程系
3
南京航空航天大学
6.1 轨迹规划的一般性问题
轨迹规划
南京航空航天大学
轨迹规划方法的特点与区别
机械手常用的两种轨迹规划方法： 方法一：给出插值点上一组显式约束。 方法二：给出运动路径的解析式。 轨迹规划可在关节空间或直角空间中。 规划器的任务：解变换方程，运动学反解和插值运算。
f a 0 a 1t 0 a1
0 a1 2 a 2t
解得四个系数的表达式：
f
3 a 3t
2 f

(0) 0 (t f ) f (0) 0 (t f ) f