第六章 机器人路径规划
合集下载
相关主题
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
j k Full section, tdjk
: Acceleration of parabola section at point j j : Acceleration of parabola section at point k k
14
三个相邻的途经点 j,k,l
jk
移动机器人路径规划技术就是移动机器人在具有障碍物的 环境中按照一定的评价标准（如工作代价最小、行走路径最短、 行走时间最短等），寻找一条从给定起点到达目标终点的无碰 路径。
23
传统路径规划大多基于图论的思想，通过一定的方法建立 几何模型，进行空间路径的搜索，包括自由空间法、图搜索法、 栅格解耦法、动态规划算法等。
8
3）保证经过途经点的速度和加速度连续
EXP：以 Fig 6.3 中B—D为例 起始点 B 中间点 C 终点
D
设计两个三次多项式路径。 路径1时间经历： t 0 tf1
1
路径2时间经历： t 0 tf 2
2
t a10 a11t a12t 2 a13t 3
2—3有运动方向的变化（反向），显然有负加速度：
sgn 50 500 / s 2 2 23 12
在以上加速度的条件下，速度由13.5°/s变化到-10°/s的耗时
10 13.5 23 12 t2 0.47 s 2 50
但端点速度不为0（危害？）
设计直线+抛物线的混合路径规划
带有抛物线过渡域的线性轨迹 为简化设计，假定首末两端抛物线 的时间长度相等。 路径相对于时间中点和位置中点是对 称的。 th , h
11
抛物线段的方程： 抛物线的速度方程： 在抛物线的末端 的速度：
t 0 a t 2
P 0 P 1 P 2 P 3 P 4 P 5 P 6
以上即为夹手必须经过的直角 坐标节点。参照这些节点的位 姿，可将手部描述为一连串的 运动和动作。
21
在直角坐标中，将机器人的目标（节点）位姿通过以下 矩阵方程表示：
0
T6 6Ttool 0Cbase t base Pobj
17
起始段直线段耗时：
1 0.47 t12 td 12 t1 t2 2 0.27 1.50 s 2 2
中间段（2-3）匀速运动速度：
23
3 2
td23
25 35 100 / s 1
但是匀速运动时 间暂时无法求到！
结束段（3-4）的减速时间（由一定的速度到静止状态）：
t a t t a t b b
1 2
t 0 t tb
t0 t tb
抛物线末端速度=直线段（恒速）的速 度
t a t b b t b
h b
th tb
let t 2th
所要求的运动持续时间
t 1 1
t1 td 12 td 12 2
2 2 1
1
sgn 1 2 1 1
15
1 t12 td 12 t1 t2 2
结束段，与起始段成对应关系。
1 100
2 350 3 250 4 100 td12 2s td 23 1s td 34 3s
a 0 1 a 2a t 3a t 2 f 1 2 f 3 f
a0 0 a
1 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
t 2 t t 0 b b h b
12
在给定起始角、终止角和运行时间，以及加速度：
2t 2 4 f 0 t tb 2 2
确定抛物线与直线段的转折时间
抛物线段加速度的约束条件：
4 h b t2 tb t 2
第6 章
机器人路径规划
路径规划： 一定的作业要求；一定的评定标准； 寻找一条从起始状态到目标状态的无碰撞路径。
1
6.1 关节空间路径规划
直角坐标空间到关节坐标空间的转换 起始点 途经点
2
起始点
途经点
……
终点
为每一个关节设计一条光滑的运动曲线（运动时间相等）
6.1 .1 三次多项式路径规划
2个位置约束条件
6
6.1 .2 含途经点的三次多项式路径规划
起始点 途经点 …… 终点
在每一个途经点不停歇（速度不为0）
0 0 , t f = f
0 , t = 0 f f
0 a0 f a0 a1t f a2t f 2 a3t f 3
4 h b when t2
直线段长度=0，路径由两端抛 物线组成。 加速度增加，抛物线长度下降，极限情况抛物线长度=0.
13
6.1 .4 含途经点的直线+抛物线路径规划
三个相邻的途经点 j,k,l 相邻途经点之间以线性函数相连； 而途经点附近以抛物线过渡。
j k Straight line segment, t jk , jk
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
0 0 , t f = f
满足2个位置约束条件，且光滑（连续）的轨迹 曲线有多种。
2个速度约束条件
0 0, t =0 f
满足以上4个约束条件的至少是3次多项式
t a0 a1t a2t 2 a3t 3
t a1 2a2t 3a3t 2 t 2a 6a t
2-3，3-4两端匀速运动速度由差异，那么3点末端需要一个加速
sgn 50 500 / s 2 3 34 23
3点末端加速段耗时：
t3
5.10 10.0 0.098s 50
这样留给2-3段匀速运动的时间：
1 1 0.47 0.098 t23 td 23 t2 t3 1 0.716 s 2 2 2 2
以上等式表示为：
机器人抓取位姿 = 操作对象期望抓取位姿
6
Ttool Cbase t
0
base
Pobj T6
0
1
求解
6
Ttool
，并通过逆向运动学得到各关节变量值。
实时完成难度较大，需要预先进行路径的规划。
22
6.3 移动机器人路径规划
移动机器人是集环境感知、动态决策、行为控制与执行等 多功能于一体的综合性系统。
1—2间匀速运动
12
2 1
1 td12 t1 2
t 50*0.27 13.50 / s 1 1
中间段（2-3）匀速运动速度：
23
但是匀速运动时间暂时无法求到！ 因为末端减速段耗时未知！
来自百度文库
3 2
td23
25 35 100 / s 1
加速度约束：
1
t 2 0 2a t 6a t 2a f1 12 f 1 13 f 1 22
约束条件确定了8个线性方程，可以解得两个三次多项式中的所有 待定参数！
10
6.1 .3 直线+抛物线路径规划
起始点和终点之间直线最短路径的优化方案。
t a bt
19
3-4段匀速运动的时间（匀速速度-5.10°/s）：
1 0.098 t34 td 34 t4 t3 3 0.102 2.849 s 2 2
20
6.2 直角坐标空间路径规划
在直角坐标空间的轨迹规划系统中，作业是用机械手终端 夹手的直角坐标节点序列确定的。 直角坐标节点： 表示夹手位姿的齐次变换矩阵。
2
1 0 1 t f 0 1 0 1
1
0 tf 2 0 2t f
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 0, t =0, t 3s 0 150 , t f =750 , f f
k j
tdjk
sgn k kl jk k
kl jk tk k
1 1 t jk tdjk t j tk 2 2
起始段，抛物线末端速度等于直线段速度
12
2 1
1 td12 t1 2
途经点速度的确定
1）由直角坐标速度通过雅可比矩阵变换成为关节速度
J P q
2）采用近似方法获取
J 1 P q
0, 0, 0, 0 0 A B D
C C C 2
选取左右两端的斜率的平均值
速度约束：
1
0 0 0 a 11
2 1
t 0 0 a 2a t 3a t 2 f2 21 22 f 2 23 f 2
t 2 0 a 2a t 3a t 2 a f1 11 12 f 1 13 f 1 21
起始段：
500 s2 1
t1 td 12 td 12 2
16
起始段加速段时间：
2 2 1 2 35 10 2 4 0.27 s 50
1
静止到加速到匀速的时间
1 100
2 350 3 250 4 10 0 td12 2s td 23 1s td 34 3s
24
智能路径规划是随着近年来人工智能的深入研究而发展起 来的优化方法，其中有模糊逻辑法、神经网络法、遗传算法以 及现在非常热门的仿生算法，如蚁群算法、免疫算法、粒子群 算法、蜂群算法等。
2 3
4
0 0 , t f = f
0 0, t =0 f
t a0 a1t a2t 2 a3t 3
t a 2a t 3a t 2 1 2 3
0 a0 0 a tf3 1 f 0 a2 0 3t f 2 a3 0
t4 3 9 2 10 25 0.102 s 50
结束段（3-4）的匀速运动时间：
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
0 a0 f a0 a1t f a2t f 2 a3t f 3
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
: Acceleration of parabola section at point j j : Acceleration of parabola section at point k k
14
三个相邻的途经点 j,k,l
jk
移动机器人路径规划技术就是移动机器人在具有障碍物的 环境中按照一定的评价标准（如工作代价最小、行走路径最短、 行走时间最短等），寻找一条从给定起点到达目标终点的无碰 路径。
23
传统路径规划大多基于图论的思想，通过一定的方法建立 几何模型，进行空间路径的搜索，包括自由空间法、图搜索法、 栅格解耦法、动态规划算法等。
8
3）保证经过途经点的速度和加速度连续
EXP：以 Fig 6.3 中B—D为例 起始点 B 中间点 C 终点
D
设计两个三次多项式路径。 路径1时间经历： t 0 tf1
1
路径2时间经历： t 0 tf 2
2
t a10 a11t a12t 2 a13t 3
2—3有运动方向的变化（反向），显然有负加速度：
sgn 50 500 / s 2 2 23 12
在以上加速度的条件下，速度由13.5°/s变化到-10°/s的耗时
10 13.5 23 12 t2 0.47 s 2 50
但端点速度不为0（危害？）
设计直线+抛物线的混合路径规划
带有抛物线过渡域的线性轨迹 为简化设计，假定首末两端抛物线 的时间长度相等。 路径相对于时间中点和位置中点是对 称的。 th , h
11
抛物线段的方程： 抛物线的速度方程： 在抛物线的末端 的速度：
t 0 a t 2
P 0 P 1 P 2 P 3 P 4 P 5 P 6
以上即为夹手必须经过的直角 坐标节点。参照这些节点的位 姿，可将手部描述为一连串的 运动和动作。
21
在直角坐标中，将机器人的目标（节点）位姿通过以下 矩阵方程表示：
0
T6 6Ttool 0Cbase t base Pobj
17
起始段直线段耗时：
1 0.47 t12 td 12 t1 t2 2 0.27 1.50 s 2 2
中间段（2-3）匀速运动速度：
23
3 2
td23
25 35 100 / s 1
但是匀速运动时 间暂时无法求到！
结束段（3-4）的减速时间（由一定的速度到静止状态）：
t a t t a t b b
1 2
t 0 t tb
t0 t tb
抛物线末端速度=直线段（恒速）的速 度
t a t b b t b
h b
th tb
let t 2th
所要求的运动持续时间
t 1 1
t1 td 12 td 12 2
2 2 1
1
sgn 1 2 1 1
15
1 t12 td 12 t1 t2 2
结束段，与起始段成对应关系。
1 100
2 350 3 250 4 100 td12 2s td 23 1s td 34 3s
a 0 1 a 2a t 3a t 2 f 1 2 f 3 f
a0 0 a
1 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
t 2 t t 0 b b h b
12
在给定起始角、终止角和运行时间，以及加速度：
2t 2 4 f 0 t tb 2 2
确定抛物线与直线段的转折时间
抛物线段加速度的约束条件：
4 h b t2 tb t 2
第6 章
机器人路径规划
路径规划： 一定的作业要求；一定的评定标准； 寻找一条从起始状态到目标状态的无碰撞路径。
1
6.1 关节空间路径规划
直角坐标空间到关节坐标空间的转换 起始点 途经点
2
起始点
途经点
……
终点
为每一个关节设计一条光滑的运动曲线（运动时间相等）
6.1 .1 三次多项式路径规划
2个位置约束条件
6
6.1 .2 含途经点的三次多项式路径规划
起始点 途经点 …… 终点
在每一个途经点不停歇（速度不为0）
0 0 , t f = f
0 , t = 0 f f
0 a0 f a0 a1t f a2t f 2 a3t f 3
4 h b when t2
直线段长度=0，路径由两端抛 物线组成。 加速度增加，抛物线长度下降，极限情况抛物线长度=0.
13
6.1 .4 含途经点的直线+抛物线路径规划
三个相邻的途经点 j,k,l 相邻途经点之间以线性函数相连； 而途经点附近以抛物线过渡。
j k Straight line segment, t jk , jk
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
0 0 , t f = f
满足2个位置约束条件，且光滑（连续）的轨迹 曲线有多种。
2个速度约束条件
0 0, t =0 f
满足以上4个约束条件的至少是3次多项式
t a0 a1t a2t 2 a3t 3
t a1 2a2t 3a3t 2 t 2a 6a t
2-3，3-4两端匀速运动速度由差异，那么3点末端需要一个加速
sgn 50 500 / s 2 3 34 23
3点末端加速段耗时：
t3
5.10 10.0 0.098s 50
这样留给2-3段匀速运动的时间：
1 1 0.47 0.098 t23 td 23 t2 t3 1 0.716 s 2 2 2 2
以上等式表示为：
机器人抓取位姿 = 操作对象期望抓取位姿
6
Ttool Cbase t
0
base
Pobj T6
0
1
求解
6
Ttool
，并通过逆向运动学得到各关节变量值。
实时完成难度较大，需要预先进行路径的规划。
22
6.3 移动机器人路径规划
移动机器人是集环境感知、动态决策、行为控制与执行等 多功能于一体的综合性系统。
1—2间匀速运动
12
2 1
1 td12 t1 2
t 50*0.27 13.50 / s 1 1
中间段（2-3）匀速运动速度：
23
但是匀速运动时间暂时无法求到！ 因为末端减速段耗时未知！
来自百度文库
3 2
td23
25 35 100 / s 1
加速度约束：
1
t 2 0 2a t 6a t 2a f1 12 f 1 13 f 1 22
约束条件确定了8个线性方程，可以解得两个三次多项式中的所有 待定参数！
10
6.1 .3 直线+抛物线路径规划
起始点和终点之间直线最短路径的优化方案。
t a bt
19
3-4段匀速运动的时间（匀速速度-5.10°/s）：
1 0.098 t34 td 34 t4 t3 3 0.102 2.849 s 2 2
20
6.2 直角坐标空间路径规划
在直角坐标空间的轨迹规划系统中，作业是用机械手终端 夹手的直角坐标节点序列确定的。 直角坐标节点： 表示夹手位姿的齐次变换矩阵。
2
1 0 1 t f 0 1 0 1
1
0 tf 2 0 2t f
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 0, t =0, t 3s 0 150 , t f =750 , f f
k j
tdjk
sgn k kl jk k
kl jk tk k
1 1 t jk tdjk t j tk 2 2
起始段，抛物线末端速度等于直线段速度
12
2 1
1 td12 t1 2
途经点速度的确定
1）由直角坐标速度通过雅可比矩阵变换成为关节速度
J P q
2）采用近似方法获取
J 1 P q
0, 0, 0, 0 0 A B D
C C C 2
选取左右两端的斜率的平均值
速度约束：
1
0 0 0 a 11
2 1
t 0 0 a 2a t 3a t 2 f2 21 22 f 2 23 f 2
t 2 0 a 2a t 3a t 2 a f1 11 12 f 1 13 f 1 21
起始段：
500 s2 1
t1 td 12 td 12 2
16
起始段加速段时间：
2 2 1 2 35 10 2 4 0.27 s 50
1
静止到加速到匀速的时间
1 100
2 350 3 250 4 10 0 td12 2s td 23 1s td 34 3s
24
智能路径规划是随着近年来人工智能的深入研究而发展起 来的优化方法，其中有模糊逻辑法、神经网络法、遗传算法以 及现在非常热门的仿生算法，如蚁群算法、免疫算法、粒子群 算法、蜂群算法等。
2 3
4
0 0 , t f = f
0 0, t =0 f
t a0 a1t a2t 2 a3t 3
t a 2a t 3a t 2 1 2 3
0 a0 0 a tf3 1 f 0 a2 0 3t f 2 a3 0
t4 3 9 2 10 25 0.102 s 50
结束段（3-4）的匀速运动时间：
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
0 a0 f a0 a1t f a2t f 2 a3t f 3
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