34
18
10 25 5.100 / s 3 0.05
23
3 2
td23

25 35 100 / s 1
34
10 25 5.100 / s 3 0.05
2-3，3-4两端匀速运动速度由差异，那么3点末端需要一个加速
3 sgn 34 23 50 500 / s 2
23
传统路径规划大多基于图论的思想，通过一定的方法建立 几何模型，进行空间路径的搜索，包括自由空间法、图搜索法、 栅格解耦法、动态规划算法等。
24
智能路径规划是随着近年来人工智能的深入研究而发展起 来的优化方法，其中有模糊逻辑法、神经网络法、遗传算法以 及现在非常热门的仿生算法，如蚁群算法、免疫算法、粒子群 算法、蜂群算法等。
0 0, A 0, B 0, D 0
C C C
2
选取左右两端的斜率的平均值
8
3）保证经过途经点的速度和加速度连续
EXP：以 Fig 6.3 中B—D为例 起始点 B 中间点 C 终点
D
设计两个三次多项式路径。 路径1时间经历： t 0 tf1
11
抛物线段的方程： 抛物线的速度方程： 在抛物线的末端 的速度：
t 0 a t 2
1 2
t 0 t tb
t a t
tb a tb
t0 t tb
抛物线末端速度=直线段（恒速）的速 度
tb a tb
let t 2th
起始段：
1 500 s2
t1 td 12 td 12 2
16
2 2 1
起始段加速段时间：
1
2 4
2 35 10 0.27 s 50
静止到加速到匀速的时间
1 100
2 350 3 250 4 100 td12 2s td 23 1s td 34 3s
1
路径2时间经历： t 0 tf 2
2
t a10 a11t a12t 2 a13t 3
B a10
t a20 a21t a22t 2 a23t 3
位置约束：
C a10 a11t f 1 a12t f 12 a13t f 13 C a20 D a20 a21t f 2 a22t f 2 2 a23t f 23
9
速度约束：
1
0 0 0 a11
2 1
t f 2 0 0 a21 2a22t f 2 3a23t f 2 2
t f 1 2 0 a11 2a12t f 1 3a13t f 12 a21
加速度约束：
1
t f 1 2 0 2a12t f 1 6a13t f 1 2a22
a0 0 a1 0 2 1 a2 2 0 f tf t f f 0 t f a3
7
3
2 tf
3

f
0
1 tf
2

f
0

途经点速度的确定
1）由直角坐标速度通过雅可比矩阵变换成为关节速度
PJ q
2）采用近似方法获取
q J 1 P
25
全局路径规划，又称为静态或离线路径规划，作业的环境 信息完全已知，主要方法有栅格法、可视图法、链接图法、概 率路径图法、拓扑法等。
局部路径规划局部路径规划，
又称为动态或在线路径规划， 作业环境部分未知或完全未知， 主要方法有人工势场法、模糊 逻辑算法、遗传算法、蚁群算 法、免疫算法等。
26
无论何种路径规划方法，原始尺寸变换是必须进行的。
第6 章
机器人路径规划
路径规划： 一定的作业要求；一定的评定标准； 寻找一条从起始状态到目标状态的无碰撞路径。
1
6.1 关节空间路径规划
直角坐标空间到关节坐标空间的转换 起始点 途经点
2
起始点
途经点
……
终点
为每一个关节设计一条光滑的运动曲线（运动时间相等）
6.1 .1 三次多项式路径规划
2个位置约束条件
6
6.1 .2 含途经点的三次多项式路径规划
起始点 途经点 …… 终点
在每一个途经点不停歇（速度不为0）
0 0 , t f = f 0 0 , t f = f
0 a0 f a0 a1t f a2t f 2 a3t f 3 0 a1 f a1 2a2t f 3a3t f 2
j k Straight line segment, t jk , jk
j k Full section, tdjk
j : Acceleration of parabola section at point j k : Acceleration of parabola section at point k
3点末端加速段耗时：
t3
5.10 10.0 0.098s 50
这样留给2-3段匀速运动的时间：
1 1 0.47 0.098 t23 td 23 t2 t3 1 0.716s 2 2 2 2
19
3-4段匀速运动的时间（匀速速度-5.10°/s）：
1 0.098 t34 td 34 t4 t3 3 0.102 2.849s 2 2

4 h b t2 tb t 2
4 h b when t2
直线段长度=0，路径由两端抛 物线组成。 加速度增加，抛物线长度下降，极限情况抛物线长度=0.
13
6.1 .4 含途经点的直线+抛物线路径规划
三个相邻的途经点 j,k,l 相邻途经点之间以线性函数相连； 而途经点附近以抛物线过渡。
0 a1 0 a1 2a2t f 3a3t f 2
a0 0 1 0 a 1 t f 1 f a2 0 0 1 a3 0 0 1 0 tf
2
0 tf 2 0 2t f
0 a0 0 a tf3 1 f 0 a2 0 3t f 2 a3 0
27
6.4 遗传算法简介
遗传算法基于自然选择的生物进化，是一种模仿生物进化 过程的随机方法。 自适应、全局化优化、概率化搜索。
21
在直角坐标中，将机器人的目标（节点）位姿通过以下 矩阵方程表示：
0
T6 6Ttool 0Cbase t base Pobj
以上等式表示为：
机器人抓取位姿 = 操作对象期望抓取位姿
6
Ttool C base t
0
base
Pobj T6
0
1
求解
6
Ttool
，并通过逆向运动学得到各关节变量值。
4
0 0 , t f = f
0 0, t f =0
t a0 a1t a2t 2 a3t 3
t a1 2a2t 3a3t 2
1 0 1 t f 0 1 0 1
1
0 a0 f a0 a1t f a2t f 2 a3t f 3
0 2t f
0 tf 3 0 3t f 2
a0 0 a1 0 a2 t f 2 f 0 2 tf
3
3
a3
5

f
0
Exp:6-1
0 150 , t f =750 , 0 0, t f =0, t f 3s
2 sgn 23 12 50 500 / s 2
在以上加速度的条件下，速度由13.5°/s变化到-10°/s的耗时
t2
23 12 10 13.5 0.47 s 2 50
17
起始段直线段耗时：
1 0.47 t12 td 12 t1 t2 2 0.27 1.50s 2 2
约束条件确定了8个线性方程，可以解得两个三次多项式中的所有 待定参数！
10
6.1 .3 直线+抛物线路径规划
起始点和终点之间直线最短路径的优化方案。
t a bt
但端点速度不为0（危害？）
设计直线+抛物线的混合路径规划
带有抛物线过渡域的线性轨迹 为简化设计，假定首末两端抛物线 的时间长度相等。 路径相对于时间中点和位置中点是对 称的。 th , h
20
6.2 直角坐标空间路径规划
在直角坐标空间的轨迹规划系统中，作业是用机械手终端 夹手的直角坐标节点序列确定的。 直角坐标节点： 表示夹手位姿的齐次变换矩阵。
P 0 P 1 P 2 P 3 P 4 P 5 P 6
以上即为夹手必须经过的直角 坐标节点。参照这些节点的位 姿，可将手部描述为一连串的 运动和动作。
1—2间匀速运动
12
2 1
1 td12 t1 2
1 t1 50*0.27 13.50 / s
中间段（2-3）匀速运动速度：
23 3 2
td23
但是匀速运动时间暂时无法求到！ 因为末端减速段耗时未知！
25 35 100 / s 1
2—3有运动方向的变化（反向），显然有负加速度：
实时完成难度较大，需要预先进行路径的规划。
22
6.3 移动机器人路径规划
移动机器人是集环境感知、动态决策、行为控制与执行等 多功能于一体的综合性系统。