7机器人运动规划

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