《机器人学》
第六章、机器人轨迹规划
战强
北京航空航天大学机器人研究所
第六章、机器人轨迹规划
控制
运动学
动力学
轨迹规划 关节 操作 空间 空间
反馈 轨迹 控制 控制 力 协调 控制 控制
轨迹：操作臂在运动过程中每时每刻的位置、速度和加速度。 轨迹规划：根据作业任务要求计算出预期的运动轨迹，分为关节 空间规划和操作空间规划两类。
1、三次多项式插值：
通过起始点关节角和终止点关节角 的运动轨迹可用一个光滑插值函数 (t ) 来表示。
为实现单关节的平稳运动，轨迹函数
(t ) 至少满足四个约束条件，两个端
θ
0
f
t 关节角轨迹光滑
点的角度约束和两个端点的速度约束。
0 ）0 （ 角度约束： （t f ) f
由每一个节点可求出 一个相应的手臂变换 矩阵 6 0T
假设节点 P0 在坐标系{B}中的描述为 B P0 ，则
6 0 0 1 B 1 T W T W T B P0 E T 6 B B 1 I P0 W T W T I P0
0 1 I I E 1 由于 6 T T T P 0 W W 0 6T
由节点 P0 运动到 P1 ，相当于 6 0T 从
6 0 0 1 B 1 B B E 1 T W T W T B P0 E T T P 6 0 0 6T 0 1 B E 1 B B E 1 T W T W T B P T T P 1 6 0 1 6T
求解可得三次多项式的系数
a0 0 a1 0 1 a 3 ( ) 2 f 0 0 2 t2 f tf tf f 2 1 a3 3 ( f 0 ) 2 (0 f ) tf tf
2、例：一旋转关节在3秒内从起始点 0 15 运动到终止点 f 75 ，起始和终止的速度皆为零，求关节的三次多项式插值函数。
o
o
可得三次多项式的系数为：
a0 0 a1 0 1 a 3 ( ) 2 f 0 0 2 t2 f tf tf f 2 1 a3 3 ( f 0 ) 2 (0 f ) tf tf
a0 15 a1 0 a 3 ( ) 20 f 0 2 t2 f 2 a3 t 3 ( f 0 ) 4.44 f
P0 P1 P2 P5 P3
MOVE P3 垂直提起销钉
MOVE P4 按一定角度接近孔眼 MOVE P5 插入销钉
RELEASE 松开销钉
MOVE P6 离开
P4 P6
销钉插孔作业
如果两个点不是在同一坐标系下描述的，需变换到同一坐标系 下，如全局坐标系{W}。
假设节点 P0 是相对于局部 坐标系{I}描述的， P1 是 相对于工作台坐标系{B}描 述的，则
θ
t 关节角轨迹光滑
过路径点的三次多项式插值函数 如果对运动轨迹的要求更严格，约束条件更多，则三次多项式 不能满足需要，需要用更高阶多项式进行插值，如五次多项式。 ▼关节空间函数的光滑性并不表示操作空间运动的光滑性
6-2 操作空间（笛卡儿空间）的轨迹规划
笛卡儿空间轨迹规划的特点：
笛卡尔运动可以非常简单地推广到圆柱坐标、球坐标以及 其他正交坐标系统。
（ 0 ）0 速度约束： （t f ) f
唯一确定一个三次多项式
(t ) a0 a1t a2t a3t
2
3
代入四个约束条件可得到关于三次多项式系数的四个方程：
0 a0 2 3 a a t a t a t 0 1 f 2 f 3 f f 0 a1 2 a 2 a t 3 a t f 1 2 f 3 f
θ
P0
P1
P3 P5
P2
t
P4 P6
关节轨迹
操作空间轨迹
6-1 关节空间轨迹规划
以关节角度（位置）函数描述机器人轨迹：计算简单、无奇异性。
θ
确定路径点
反解关节值
每个关节运 动时间相同
光滑函数拟 合每个关节
t 某关节的反解值（线性化）
▼关键要使关节轨迹满足约束条件，如各点上的位姿、速度和 加速度要求和连续性要求等，在满足约束条件下选取不同的 插值函数。
0 6 E I I P0 ：W T 0T 6T W T P0
z Z0 {W} P0 · P1· {B}
P1 ： T 6T W T WT 0Байду номын сангаас
0 6 E B
B
P0
0 1 I I E 1 P0 ：6 T T T P 0 W W 0 6T
P1 ：6T 0T 1 B T B P E T 1 0 W W 0 6
运动在笛卡尔坐标中是直观的，容易定义，但是它需要对机 械手的定位点进行不断的求值，把它变换成各个关节坐标的 运动。
在笛卡儿空间中，机器人末端抓手的位姿可用一系列的节点 表示，轨迹规划的首要问题是在路径起始点和终点之间如何 生成一系列的中间点。
1、一个典型的笛卡儿空间的任务
INIT
P0
原位
MOVE P1 接近销钉 MOVE P2 移动到销钉的位置 GRASP 抓住销钉
(t ) a0 a1t a2t 2 a3t 3
a0 0 a1 0 1 a 3 ( ) 2 f 0 0 2 t2 f tf tf f 2 1 a3 3 ( f 0 ) 2 (0 f ) tf tf
可得三次多项式的关节插值函数为： (t ) 15 20t 2 4.44t 3 2 ( t ) 40 t 13 . 32 t (t ) 40 26.64t
角 度
角 速 度
时间 角 加 速 度
时间
时间
任何三次多项式函数的速度曲线皆为抛物线，加速度曲线为直线。