运动轨迹生成

13
结点的速度。
利用启发式信息确定关节速度
假设用直线段把这些结点连接起来，若相邻线段的斜率在 结点处改变符号，就把速度选定为0；若斜率不改变符号 ， 则选取结点两侧线段斜率的平均值作为该结点的速度。 只要给出了期望的 途径点，系统就能 用这种方法自动地 生成相应的速度。 从概念到方法都很 简单。
14
第四章 运动轨迹生成
§4.1 概述 §4.2 关节空间轨迹生成 §4.3 直角空间轨迹生成 §4.4 轨迹的实时生成
LEC.8-07
1
§4.1 概述
研究内容
（1）如何描述机器人的运动轨迹q(t)或X(t)； （2）运动轨迹的生成方法。
三个问题
（1）描述机器人的空间运动——允许用户用比较简单的方式描述机器 人的运动，而复杂的细节问题则由计算机系统解决。用一条轨迹来 通过或逼近结点，该轨迹按一定原则优化（如加速度平滑、精确 等）。轨迹应在满足作业要求的同时尽量简化计算。如：用户可只 给出末端的目标位姿，让系统由此确定到达目标的途径点、持续时 间、速度等轨迹参数。
16
根据加速度连续原则（续2）
这些约束构成了有八个未知数的八个线性方程，可求解之。 若有 t f 1 = t f 2 = t f ，则其解为
⎧a10 = θ 0 ⎪a = 0 ⎪ 11 12θ v − 3θ g − 9θ 0 ⎪ a12 = ⎪ 4t 2 f ⎪ − 8θ v + 3θ g + 5θ 0 ⎪ a13 = ⎪ 4t 2 f ⎪ ⎪a = θ v ⎨ 20 3θ g − 3θ 0 ⎪ ⎪a 21 = 4t f ⎪ − 12θ v + 6θ g + 6θ 0 ⎪ ⎪a 22 = 4t 2 f ⎪ 8θ v − 5θ g − 3θ 0 ⎪ ⎪a 23 = 4t 3 ⎪ f ⎩
对这两个多项式的约束是：
⎧θ 0＝a10 ⎪θ = a + a t + a t 2 + a t 3 10 11 f 1 12 f 1 13 f 1 ⎪ v ⎪θ v = a 20 ⎪ θ g = a 20 + a 21t f 2 + a 22 t 2 2 + a 23t 3 2 ⎪ f f ⎨ 各式对应的条件？ 0 = a11 (即θ 0 = 0) ⎪ ⎪0 = a 21 + 2a 22 t f 2 + 3a 23 t 2 2 (即θ g = 0) f ⎪ a11 + 2a12 t f 1 + 3a13t 21 = a 21 f ⎪ ⎪2a12 + 6a13 t f 1 = 2a 22 ⎩
(
)
(
)
(
)
(
)
(
)
(
)
19
四、带抛物线混合段的线性函数
线性函数——当关节由初始点 运动到终点时只使用线性插值 这样可得到关节的等速运动， 某些作业需要如此。 尽管各关节都作线性运动，但 机器人末端在直角空间运动轨 迹则一般不是线性的。 线性函数的问题——简单线性 函数将导致关节在运动的起点 和终点的速度不连续，加速度 趋于无穷大，形成刚性冲击。
2
轨迹规划——确定轨迹参数的过程。
轨迹生成的三个问题
（2）建立轨迹的计算机内部描述 根据所确定的轨迹参数，在计算机内部描述期望的轨迹。这主要是选 择合理的软件数据结构。 （3）生成轨迹 对内部描述的轨迹进行实际计算，即根据轨迹参数（如途径点位姿、 速度、加速度、轨迹函数的系数等）生成出整个轨迹。 计算是实时进行的，每一个轨迹点的计算速度称为轨迹更新速度。在 典型的机器人系统中，轨迹更新速度为50－1000HZ.
17
三、高阶多项式（1）
对于关节段的起始点和终点都规定了关节的位置、速度和加速度， 则需要一个五次多项式
θ (t ) = a 0 + a1t + a 2 t 2 + a 3 t 3＋a 4 t 4 + a 5 t 5
要求满足如下约束
⎧θ 0＝a 0 ⎪θ = a + a t + a t 2 + a t 3 ＋a t 4 + a t 5 0 1 f 2 f 3 f 4 f 5 f ⎪ f ⎪θ 0 = a1 ⎪ ⎨ θ f = a1 + 2a 2 t f + 3a3t 2＋4a 4 t 3 + 5a5 t 4 f f f ⎪ ⎪θ 0 = 2a 2 ⎪ 2 3 ⎪θ f = 2a 2 + 6a3t f + 12a 4 t f + 20a5 t f ⎩
问题
如Fra Baidu bibliotek机器人在途径点停留，可直接应用前面的三次多项式的解。但通 常我们希望能不停地通过中间点，所以有必要推广上述方法。
方法
只要把所有的中间结点也看作是一些终止点或起始点，求解逆运动 学，得到一组关节值，然后就可以确定相应的三次多项式，从而把结点 平滑地连接起来。 设结点上关节速度为已知，则约束式变为： θ (0) = θ 0 , θ (t f ) = θ f , θ (0) = θ 0 , θ (t f ) = θ f 可求得三次多项式的四个系数为：
a 0 = θ 0 , a1 = θ 0 , a 2 =
12
3 (θ f − θ 0 )－ 2 θ 0 − 1 θ f , a3 = − 2 (θ f − θ 0 ) + 12 θ f + θ 0 tf tf t2 t3 tf f f
(
)
轨迹三次多项式（续）
由上式确定的三次多项式描述了始点和终点为任意位置和速度的运 动轨迹。 在各个结点规定关节速度，然后简单地对每个段应用上式即可求出 各段的轨迹方程。 在起点加速度有突变，柔性冲击。 确定各结点关节速度的三种方法： 1. 根据该结点直角空间速度确定关节空间速度； 需求出雅可比矩阵的逆，有奇异点问题。 2. 在直角空间或关节空间采用适当的启发式方法，由控制系统自动 地选择途径点速度。 3. 根据保证每个结点的加速度连续的原则，由控制系统自动地选择
5
轨迹描述和生成的一般考虑（2）
2 轨迹描述方法 1） PTP方法——让机器人从一个初始位置运动到某个目标位置，对中 间轨迹无要求。 用户须明确规定初始点、末点或加上若干个中间点的位姿、速度、 加速度的约束，由系统轨迹规划器选择合适的满足上述约束的轨 迹。通常在关节空间描述。 2） CP方法——用户明确规定机器人末端必须经过的连续轨迹，如直 线、圆及其它空间曲线。通常在直角空间描述。 两种方法： （1） 用直线或曲线方程描述； （2）给出一连串的期望经过点（如通过示教方法）来近似表示某一 轨迹。
( )
求：用来对关节量进行插值的光滑函数 θ (t ) 为实现单个关节的平稳运动， θ (t ) 至少需要满足四个约束条件： 考虑一种最简单的情况，有 θ (0) = θ 0 , θ t f = θ f , θ (0) = 0, θ t f = 0
( )
( )
由此可唯一确定一个三次多项式：
θ (t ) = a 0 + a1t + a 2 t 2 + a 3 t 3 关节速度和加速度为： ⎧θ (t ) = a1 + 2a 2 t + 3a3 t 2 ⎨ ⎩ θ (t ) = 2a 2 + 6a3t
θ 1 (t ) = a10 + a11t + a12 t 2 + a13 t 3
相应于从 θ v 运动到 θ g 的三次多项式为 上述两个三次多项式的时间区间分别为
θ 2 (t ) = a 20 + a 21t + a 22 t 2 + a 23 t 3
[0,
15
t f1
]
[0
tf2
]
根据加速度连续原则（续）
3
轨迹规划器
q(t ), q(t ), q(t )
X (t), X (t), X (t )
4
轨迹描述和生成的一般考虑（1）
1 运动任务描述 把机器人的运动作为{工具}标架相对于{台}/{基座}标架的 运动。 优点： 1）有利于用户的想象、说明、任务描述； 2）把运动的描述从任何特定的机器人、末端执行器或工 件的耦合中解脱出来。这导致某种标准化，将可以把这 个轨迹描述用于不同的机器人或相同的机器人而工具尺 寸不同。
根据加速度连续原则确定关节速度
思路——为保证途径点的加速度连续，可以设法用两条三次曲线在 结点处连接起来，拚成期望的轨迹。拚接的约束条件是： 连接处不但速度连续，而且要求加速度连续。 方法——设所考虑的结点处关节角度为 θ v ，与该结点相邻的前后 两点的关节角度分别为 θ 0 和 θ g ，相应于从 θ 0 运动到 θ v 的三 次多项式为
对已知的任意 θ 0 、θ f 和 t f ，可以选择满足上式的 θ 和 t b 来获得相应的轨迹。通常的作法是先选择 θ 的值，然后用上式 求出相应的 t b 。即 θ 2 t 2 − 4θ θ f − θ 0 tf f tb = − 2 2θ 4 θ f − θ0 t b 有解， θ 必须选得足够大，即须 θ ≥ 为保证 2
8
关节空间法的特点
计算过程简单； 没有奇异点问题； 可以确切反映关节的 位置、速度、加速度 极限，可实现快速启 动； 适用于大范围内不要 求路径限制及精确定 位的运动； 在结点之间的轨迹形状在关节空间虽相当简单，但在直角坐标空间 的描述是复杂的，难以想像。
9
一、三次多项式
问题
已知：起始点的关节角度 θ 0＝θ (t 0 ) ，终点关节角度 θ f ＝θ t f
四、带抛物线混合段线性函数（3）
t b 和 θ 的关系
由于过渡段的终点速度必须等于线性段的速度，所以有 θ h − θ b 问题：等式右边的物理意义？ θt b = t h − tb θ b 的值可按下式解得 1 θ b = θ 0 + θt b2 2 由
t f = 2t h
可得
θt b2 − θt f t b + θ f − θ 0 = 0
θ
θf
θ0
t0
θ
tf
t
20
t0
tf
t
四、带抛物线混合段线性函数（2）
为了生成具有连续位置和速度的 光滑轨迹，用线性插值时，从起 点开始，在每个结点的某个领域 内加一个抛物线混合段 加速度为常数。 为了构成单一轨迹，假设两个过 渡域都具有相同的持续时间，故 采用相同加速度值（符号不同） 这样设计的轨迹不是唯一的。 每一个结果都对称于时间中点 t h 21 和位置中点 θ h 。
18
三、高阶多项式（2）
解该方程组，可得
⎧a 0 = θ 0 ⎪a = θ 0 ⎪ 1 ⎪a = θ 0 ⎪ 2 2 ⎪ 20θ f − 20θ 0 − 8θ f + 12θ 0 t f − 3θ 0 − θ f t 2 f ⎪a 3 = ⎨ 2t 3 f ⎪ 30θ 0 − 30θ f + 14θ f + 16θ 0 t f + 3θ 0 − 2θ f t 2 f ⎪a = ⎪ 4 2t 3 f ⎪ 12θ f − 12θ 0 − 6θ f + 6θ 0 t f − θ 0 − θ f t 2 ⎪ f a5 = ⎪ 2t 5 f ⎩
t s 值代入上式，可求出三次多项式的系
⎧θ (t ) = 40.0t − 13.33t 2 ⎨ ⎩ θ (t ) = 40.0 − 26.66t
a1 = 0, a 2 = 20.0, a3 = −4.44
θ (t ) = 15.0 + 20.0t 2 − 4.44t 3
11
二、带中间点的轨迹三次多项式
可解得
10
a 0 = θ 0 , a1 = 0, a 2 =
3 (θ f − θ 0 ), a3 = − 2 (θ f − θ 0 ) t2 t3 f f
注：这组解只适用于关节初始速度和末速度为0的场合。
举例
设只有一个旋转关节的单杆机器人处于静止状态时， θ 0＝15度，要求它 在3s之内平稳到达 θ f＝75度，末点速度为0。 求其轨迹。 解：把 θ 0 ， θ f ，θ 0 ， θ f ， 数： a = 15.0, 0 由此可得
6
轨迹描述和生成的一般考虑（3）
3
约束条件 1） 运动约束——主要要求是运动光滑。 （1） 轨迹函数连续且其一阶导数连续； （2） 除1）以外，还要求加速度连续，以减少冲击； （3） 多余的运动如摆动、超调和停顿应最小。 2） 障碍约束 3） 力矩约束
7
§4.2 关节空间轨迹生成
关节空间法——用机器人关节角度的函数来描述运动轨迹 轨迹结点（中间点/途径点）通常用工具标架相对于台标架 的位姿来表示。在每一个结点上，通过逆运动学计算可以 得到其相应的一组关节的角度。 如果能为每一个关节找到一个平滑函数，它通过各结点， 并在CP场合能逼近理想轨迹，就可生成整个机器人从起始 点，通过中间结点，最后到达终点的满足要求的运动轨迹 只要在任何一段轨迹内每一个关节的运动时间都相同，则 所有的关节都将同时到达途径点和终止点。 工具标架在各个结点上，具有预期的直角坐标位姿。