运动控制算法轨迹规划

解上面四个方程得： 解上面四个方程得：
注意：这组解只适用于关节起点、终点速度为零的运动情况。 注意：这组解只适用于关节起点、终点速度为零的运动情况。
例：设只有一个自由度的旋转关节机械手处于静止状态时， 设只有一个自由度的旋转关节机械手处于静止状态时， =150，要在 内平稳运动到达终止位置： 要在3s内平稳运动到达终止位置 内平稳运动到达终止位置： 止点的速度为零。 止点的速度为零。 将上式的已知条件代入以下四个方程得四个系数： 解： 将上式的已知条件代入以下四个方程得四个系数： =750，并且在终
•点位作业（PTP=point-to-point motion） 点位作业（ ） •连续路径作业（continuous-path motion），或者称为轮廓运动 连续路径作业（ ），或者称为轮廓运动 ），
（contour motion）。 ）。
操作臂最常用的轨迹规划方法有两种： 操作臂最常用的轨迹规划方法有两种： 第一种是要求对于选定的轨迹结点（插值点）上的位姿、 第一种是要求对于选定的轨迹结点（插值点）上的位姿、速 是要求对于选定的轨迹结点 度和加速度给出一组显式约束（例如连续性和光滑程度等），轨 度和加速度给出一组显式约束（例如连续性Fra Baidu bibliotek光滑程度等），轨 ）， 迹规划器从一类函数（例如 次多项式 选取参数化轨迹， 次多项式） 迹规划器从一类函数（例如n次多项式）选取参数化轨迹，对结 点进行插值，并满足约束条件。 点进行插值，并满足约束条件。 第二种方法要求给出运动路径的解析式。 第二种方法要求给出运动路径的解析式。 方法要求给出运动路径的解析式
►
运动规划通常又称运动插补。插补就是按给定曲线生成相 应逼近轨迹的方法，其实质是对给定曲线进行“ 应逼近轨迹的方法，其实质是对给定曲线进行“数据点的 密化” 密化”。数控加工零件的刀具路径一般由直线、圆弧、椭 圆等简单曲线或B样条、NURBS等复杂曲线组成。对于由 圆等简单曲线或B样条、NURBS等复杂曲线组成。对于由 简单曲线组成的刀具路径，可以由相应的简单插补算法进 行插补。如对于直线路径有直线插补算法，对于圆弧路径 则有缘故插补算法。而对于复杂曲线组成的刀具路径目前 一般有两种做法：第一种做法是预先将这些复杂曲线按照 给定的精度要求分成大量的直线或圆弧段，再由数控系统 对这些直线或圆弧段进行插补运算；另外一种做法是直接 讲这些复杂曲线的参数传递到数控系统中，由数控系统对 这些复杂曲线进行实时插补运算
运动控制系统中的算法
► 运动控制设计两个基本问题：一是运动规划；二是
控制算法，运动规划是在给定的路径端点之间插入 用于控制的中间点序列从而实现沿给定的平稳运动。 运动控制则是主要解决如何控制目标系统准确跟踪 指令轨迹的问题。即对于给定的指令轨迹，选择适 合的控制算法和参数，产生输出，控制目标实时， 准确地跟踪给定的指令轨迹。
θ
j l k 0
多段带有抛物线过渡的线性插值轨迹
t
如果要求机器人通过某个结点，同时速度不为零，怎么办？ 如果要求机器人通过某个结点，同时速度不为零，怎么办？ 可以在此结点两端规定两个“伪结点” 可以在此结点两端规定两个“伪结点”，令该结点在两伪结点的 连线上，并位于两过渡域之间的线性域上。 连线上，并位于两过渡域之间的线性域上。
因此得： 因此得：
θ (t ) = 15.0 + 20.0t 2 − 4.44t 3 θ (t ) = 40.0t − 13.32t 2 θ (t ) = 40.0 − 26.64t
•• •
b. 过路径点的三次多项式插值 方法是：把所有路径点都看成是“起点” 终点” 方法是：把所有路径点都看成是“起点”或“终点”，求解 逆运动学，得到相应的关节矢量值。 逆运动学，得到相应的关节矢量值。然后确定所要求的三次多项 式插值函数，把路径点平滑的连接起来。不同的是，这些“起点” 式插值函数，把路径点平滑的连接起来。不同的是，这些“起点” 和“终点”的关节速度不再是零。 终点”的关节速度不再是零。
θ θg
θv
θ0
0
t0
tv
tg t
对于方法（ ） 这里所说的启发式方法很简单， 对于方法（3）， 这里所说的启发式方法很简单，即假设用 直线段把这些路径点依次连接起来， 直线段把这些路径点依次连接起来，如果相邻线段的斜率在路径 点处改变符号，则把速度选定为零；如果相邻线段不改变符号， 点处改变符号，则把速度选定为零；如果相邻线段不改变符号， 则选择路径点两侧的线段斜率的平均值作为该点的速度。 则选择路径点两侧的线段斜率的平均值作为该点的速度。
（1） 根据工具坐标系在直角坐标空间中的瞬时线速度和角速 ） 该方法工作量大。 度来确定每个路径点的关节速度 ；该方法工作量大。
（2）为了保证每个路径点上的加速度连续，由控制系统按照 ）为了保证每个路径点上的加速度连续， 此要求自动地选择路径点的速度。 此要求自动地选择路径点的速度。
（3）在直角坐标空间或关节空间中采用某种适当的启发式方 ） 法，由控制系统自动地选择路径点的速度； 由控制系统自动地选择路径点的速度；
轨迹规划既可以在关节空间也可以在直角空间中进行。 轨迹规划既可以在关节空间也可以在直角空间中进行。
2.关节轨迹的插值 关节轨迹的插值 关节空间法计算简单、容易。再者，不会发生机构的奇异性 关节空间法计算简单、容易。再者， 问题。 问题。 轨迹规划方法一般是在机器人的初始位置和目标位置之间用 多项式函数来“内插” 逼近”给定的路径， 多项式函数来“内插”或“逼近”给定的路径，并产生一系列的 控制点。 控制点。 a. 三次多项式插值 只给定机器人起始点和终 止点的关节角度。 止点的关节角度。
机器人的轨迹规划
1 工业机器人的轨迹规划
{xk}
任务规划 器 压缩的数 据 图像分析 器 I(k,e) 摄象机
q(t) qd(t)
轨迹规划 器 机器人控 τ (t ) 操作臂动 制器 力学 操作臂运 动学
x(t) 环境
F(t)
力传感器
任务规划器
１.轨迹规划的一般性问题 轨迹规划的一般性问题 这里所谓的轨迹是指操作臂在运动过程中的位移、速度和 这里所谓的轨迹是指操作臂在运动过程中的位移、 加速度。 加速度。 常见的机器人作业有两种： 常见的机器人作业有两种：
► 运动控制的控制对象在启动和停止阶段分别
会出现加速和减速的过度过程。这既是伺服 电机对速度指令的时域响应而产生的自然现 象，又是为了保证进给运动的平稳而经人为 顺势设计的。为了保证伺服系统运动的平稳 性，要避免冲击和振荡，做到启动时不失步、 停止时不超程，就必须对过渡过程进行专门 而有效的加速控制，使加减速过程按照所采 用的控制算法进行。
θ3
θ0
θ
同理可以求得此时的三次多项式系数： 同理可以求得此时的三次多项式系数：
此时的 速度约 束条件 变为： 变为：
θ (0) = θ 0 θ (t f ) = θ f
• •
•
•
由上式确定的三次多项式描述了起始点和终止点具有任意给定位 置和速度的运动轨迹。 置和速度的运动轨迹。剩下的问题就是如何确定路径点上的关节 速度，有以下三种方法： 速度，有以下三种方法：
数控系统中用到的运动规划算法
►
►
►
逐点比较插补法：从给定的轨迹点出发，每进给一步都要与 给定轨迹上的坐标值进行比较，决定下一步的进给朝着减少 偏差的方向运动，直至到达轨迹的终点。 数字积分法（DDA法）：利用对速度分量的积分的方法来分 数字积分法（DDA法）：利用对速度分量的积分的方法来分 配脉冲，控制目标沿给定的路径平稳移动。其插补速度较快， 输出脉冲均匀，易于实现多坐标联动。 时间分割法：又叫数据采样插补法。是把加工一段直线或圆 弧的整段时间细分为许多相等的时间间隔，称为单位时间间 隔（或插补周期），每经过一个单位时间间隔就进行一次插 补计算，算出在这段时间间隔内各坐标轴的进给量，边计算， 边加工，直至加工结束。其插补速度更快，对于复杂多维曲 线的运动规划尤其有利。
算法的实现
► 根据上述运动控制算
法的分析，利用C 法的分析，利用C语 言设计、编制了全闭 环运动控制的测试程 序。具体的控制流程 图如下：为了增强系 统的稳定性，位置增 量采取限幅处理，因 为位置增量过大不利 于安全操作和系统的 稳定，而输出限幅处 理是为了在位置设定 值突变时，防止计算 结果可能大于执行机 构的极限。
θ
θA
θD
θC
θ0
θB
t0
tA tB
tC
tD t
路径点上速度的自动生成
如果对于运动轨迹的要求更为严格，约束条件增多， 如果对于运动轨迹的要求更为严格，约束条件增多， 那么 三次多项式就不能满足需要， 三次多项式就不能满足需要，必须用更高阶的多项式对运动轨 迹的路径段进行插值。例如， 迹的路径段进行插值。例如，对某段路径的起点和终点都规定 了关节的位置、速度和加速度（有六个未知的系数），则要用 了关节的位置、 速度和加速度（ 有六个未知的系数） 一个五次多项式进行插值。 一个五次多项式进行插值。
θ
θf
θ0
0 tf
单个关节的不同轨迹曲线
t
为了实现平稳运动，轨迹函数至少需要四个约束条件。 为了实现平稳运动，轨迹函数至少需要四个约束条件。即 ————满足起点和终点的关节角度约束 满足起点和终点的关节角度约束
————满足起点和终点的关节速度约束（满 满足起点和终点的关节速度约束（ 满足起点和终点的关节速度约束 足关节速度的连续性要求） 足关节速度的连续性要求）
运动控制算法
►
►
传统的交流伺服系统只能实现对速度的闭环控制，还不能 直接实现对位置的闭环控制。要实现对位置的闭环控制，必 须在伺服电机和控制系统之间构成一个位置环。这个位置环 是由数字伺服控制单元来完成的。位置环的功能是实现位置 闭环控制，输入脉冲实际上是一个差动脉冲，即由预先设定 好的脉冲给定与反馈回的实际测量脉冲的差值，即根据给定 位置和测量位置以及动态的误差，计算所要求要达到的速度 参考，并将其提供给伺服驱动单元。如图1 参考，并将其提供给伺服驱动单元。如图1所示为数字伺服 控制单元的运动控制算法框图： 整个数字伺服控制系统是靠实时监控和调整速度参数，即伺 服驱动单元的输入脉冲来控制伺服马达。而运动控制算法用 来计算这个速度参数。由上图，可以看出速度参数是由位置 给定、测量脉冲和反馈误差这三个参数来决定的。整个算法 是以PID(比例、积分和微分) 是以PID(比例、积分和微分)控制为理论基础，其最大的优 点在于不需了解被控对象的数学模型，只要根据经验调整上 述增益参数，便可获得满意的结果。
对于方法（ ） 为了保证路径点处的加速度连续， 对于方法（2），为了保证路径点处的加速度连续，可以设法 用两条三次曲线在路径点处按照一定的规则联系起来， 用两条三次曲线在路径点处按照一定的规则联系起来，拼凑成所 要求的轨迹。其约束条件是：联接处不仅速度连续， 要求的轨迹。其约束条件是：联接处不仅速度连续，而且加速度 也要连续。 也要连续。
θ (t ) = a0 + a1t + a2t 2 + a3t 3 + a4t 4 + a5t 5
c、用抛物线过渡的线性插值 、 单纯线性插值将导致在结点处关节运动速度不连续， 单纯线性插值将导致在结点处关节运动速度不连续，加速度 无限大。 无限大。
θ
θ
解决办法：在使用线性插值时， 解决办法：在使用线性插值时， 把每个结点的邻域内增加一段抛 物线的“缓冲区段” 物线的“缓冲区段”，从而使整 个轨迹上的位移和速度都连续。 个轨迹上的位移和速度都连续。
θf
θ0
0 tb tf-tb tf t
带抛物线过渡的线性插值（1）
θ
对于多解情况，如右图所示。 对于多解情况，如右图所示。加 速度的值越大，过渡长度越短。 速度的值越大，过渡长度越短。
θf
θh
θ0
0 th tf t
带抛物线过渡的线性插值（2）
d、过路径点的用抛物线过渡的线性插值 、 如图所示，某个关节在运动中设有 个路径点 个路径点， 如图所示，某个关节在运动中设有n个路径点，其中三个相 邻的路径点表示为j， 和 ， 邻的路径点表示为 ，k和l，每两个相邻的路径点之间都以线性 函数相连，而所有的路径点附近则有抛物线过渡。 函数相连，而所有的路径点附近则有抛物线过渡。（同样存在多 解）