1、第第 3章章 机器人轨迹规划机器人轨迹规划3.1 机器人轨迹规划概述机器人轨迹规划概述3.1.1 机器人轨迹的概念机器人轨迹的概念轨迹：点的轨迹、位移、速度和加速度。轨迹：点的轨迹、位移、速度和加速度。轨迹规划：将手部运动轨迹换算成关节空间；轨迹规划：将手部运动轨迹换算成关节空间；对关节空间进行插值。对关节空间进行插值。3.1.2 轨迹规划的一般性问题轨迹规划的一般性问题通用作业描述方法通用作业描述方法 :（ 1）描述成工具坐标系）描述成工具坐标系 T相对于工件坐标系相对于工件坐标系 S3.1.2 轨迹规划的一般性问题轨迹规划的一般性问题通用作业描述方法通用作业描述方法 :（ 1）描述成工具坐
2、标系）描述成工具坐标系 T相对于工件坐标系相对于工件坐标系S的一系列运动。的一系列运动。（ 2）轨迹的点：包含位置和姿态；）轨迹的点：包含位置和姿态；（ 3）插值的原则：保证运动平稳。即位移、）插值的原则：保证运动平稳。即位移、速度函数必须连续，有时甚至加速度也要求连续速度函数必须连续，有时甚至加速度也要求连续。（ 4）关节空间插值：将所有关节变量表示为）关节空间插值：将所有关节变量表示为时间的函数，用这些关节函数及其一阶、二阶导时间的函数，用这些关节函数及其一阶、二阶导数描述机器人预期的运动；数描述机器人预期的运动；（ 5）直角坐标空间插值：将手爪位姿、速度）直角坐标空间插值：将手爪位姿、速
3、度和加速度表示为时间的函数，再算出所有关节位和加速度表示为时间的函数，再算出所有关节位置、速度和加速度函数值。置、速度和加速度函数值。3.1.4 轨迹规划涉及的主要问题轨迹规划涉及的主要问题轨迹规划一般过程：轨迹规划一般过程：(1) 作业描述：（用示教方法）给出轨迹上的若干作业描述：（用示教方法）给出轨迹上的若干个结点。个结点。(2) 插值：在结点之间进行插补，得到直角空间的插值：在结点之间进行插补，得到直角空间的X(t)或关节空间的位移或关节空间的位移 q(t)；(3) 以以 X(t)或或 q(t) 为依据设计控制规律。为依据设计控制规律。(4) 考虑路径上是否存在障碍。考虑路径上是否存在障
4、碍。3.2 插补方式分类与轨迹控制插补方式分类与轨迹控制3.2.1 插补方式分类插补方式分类点位控制点位控制 (PTP控制控制 )：只要求起终点位姿，没有路：只要求起终点位姿，没有路径约束。径约束。插补的依据是（插补的依据是（ 1）关节最大速度和加速度；（）关节最大速度和加速度；（ 2）速度连续，各轴协调。速度连续，各轴协调。连续轨迹控制连续轨迹控制 (CP控制控制 )：有路径约束，要对路径进：有路径约束，要对路径进行设计。行设计。路径控制与插补方式分类路径控制与插补方式分类不插不插 补补 关关 节节 插插 补补 (平滑平滑 ) 空空 间间 插插 补补PTP(1) 各各 轴轴 独立独立快速到达
5、。快速到达。(2) 关关 节节 最大最大加速度限制加速度限制(1) 各各 轴协调轴协调 运运 动动 定定 时时插插 补补 。(2) 各关各关 节节 最大加速度最大加速度限制限制CP(1) 在空在空 间间 插插 补补 点点 间进间进行关行关 节节 定定 时时 插插 补补 。(2) 用关用关 节节 的低的低 阶阶 多多 项项式式 拟拟 合空合空 间间 直直 线线 使各使各轴协调轴协调 运运 动动 。(3) 关关 节节 最大加速度限最大加速度限制制(1) 直直 线线 、 圆圆 弧弧、曲、曲 线线 等距插等距插 补补。(2)给给 定起停定起停 线线速度、速度、 线线 加速度加速度；关；关 节节 速度、
6、加速度、加速度限制速度限制3.2.2 机器人轨迹控制过程（机器人轨迹控制过程（ 示教示教 -再现过程）：再现过程）：（ 1）对于有规律的轨迹，仅示教几个特征点，如）对于有规律的轨迹，仅示教几个特征点，如直线需要示教两点，圆弧需要示教三点；直线需要示教两点，圆弧需要示教三点；（ 2）计算机利用插补算法获得中间点的坐标；）计算机利用插补算法获得中间点的坐标；（ 3）计算机求出）计算机求出 (1, , n)，（ 4）闭环控制系统实现预期轨迹。）闭环控制系统实现预期轨迹。3.3 机器人轨迹插值计算机器人轨迹插值计算给出结点（位置姿态）；进行运动学反解；关节给出结点（位置姿态）；进行运动学反解；关节变量
7、的插值计算。变量的插值计算。3.3.1 直线插补直线插补直线插补和圆弧插补是机器人系统中的基本插补直线插补和圆弧插补是机器人系统中的基本插补算法。对于其他轨迹，可以采用直线或圆弧逼近，以算法。对于其他轨迹，可以采用直线或圆弧逼近，以实现这些轨迹。实现这些轨迹。 已知（示教给出）直线始末两点的坐标值已知（示教给出）直线始末两点的坐标值 P0(X0， Y0，Z0)、 Pe(Xe， Ye， Ze)及姿态，要求走空间直线：及姿态，要求走空间直线：求各轨迹中间点求各轨迹中间点 (插补点插补点 )的位置和姿态。的位置和姿态。设设 v为要求的沿直线运动的速度；为要求的沿直线运动的速度； ts为插补时间间隔。
8、为插补时间间隔。 ts间隔内行程：间隔内行程： d = vts；插补总步数插补总步数 N： L/d+1，取整；，取整；各插补点坐标值各插补点坐标值各坐标轴增量各坐标轴增量直线长度：直线长度：3.3.2 圆弧插补圆弧插补一、平面圆弧插补（圆弧在坐标平面内）一、平面圆弧插补（圆弧在坐标平面内）已知（示教给出）不在一条直线上的三点已知（示教给出）不在一条直线上的三点 P1、P2、 P3及对应姿态。及对应姿态。求各轨迹中间点求各轨迹中间点 (插补点插补点 )的位置和姿态。的位置和姿态。设设 v为沿圆弧运动速度；为沿圆弧运动速度； ts为插补时时间隔。为插补时时间隔。 (1) 计算计算 P1、 P2、
9、P3决定的圆弧半径决定的圆弧半径 R。(2)计算总的圆心角计算总的圆心角 =1+2:(3) ts时间内角位移量：时间内角位移量： =tsv/R，(4) 总插补步数总插补步数 (取整数取整数 )： N = / + 1式中：式中： Xi=R cosi； Yi=Rsin i。 二、空间圆弧插补二、空间圆弧插补空间圆弧插补可分三步来处理：空间圆弧插补可分三步来处理：(1) 把三维问题转化成二维，找出圆弧所在平面。把三维问题转化成二维，找出圆弧所在平面。(2) 利用二维平面插补算法求出插补点坐标利用二维平面插补算法求出插补点坐标 (Xi+1, Yi+1)。(3) 把该点的坐标值转变为基础坐标系下的值。把
10、该点的坐标值转变为基础坐标系下的值。3.3.3 定时插补与定距插补定时插补与定距插补一、定时插补一、定时插补ts与精度有关：与精度有关：ts的上限值：受刚度限制，的上限值：受刚度限制， ts一般不超过一般不超过 25 msts的下限值：受计算量限制（计算机要在的下限值：受计算量限制（计算机要在 ts时间时间里完成一次插补运算和一次逆向运动学计算）。约里完成一次插补运算和一次逆向运动学计算）。约需几毫秒。需几毫秒。二、定距插补二、定距插补两插补点的距离两插补点的距离 Pi Pi+1恒为一个足够小的值，恒为一个足够小的值，以保证轨迹精度，以保证轨迹精度， ts就要变化。就要变化。3.3.4 关节空
11、间插补关节空间插补给定：机器人在起始点和终止点手臂的位形。给定：机器人在起始点和终止点手臂的位形。可以给出：首末两点的力、速度和加速度的要可以给出：首末两点的力、速度和加速度的要求；求；插值依据：关节位移、速度、加速度连续性；插值依据：关节位移、速度、加速度连续性；关节变量的容许范围等。关节变量的容许范围等。一、三次多项式插值一、三次多项式插值二、过路径点的三次多项式插值二、过路径点的三次多项式插值三、高阶多项式插值若对于运动轨迹的要求更为严格，约束条件增多，三次多项式就不能满足需要，须用更高阶的多项式对运动轨迹的路径段进行插值。例如 :起始点和终止点都规定了关节的位置、速度和加速度，则要用一
12、个五次多项式进行插值 。四、用抛物线过渡的线性插值四、用抛物线过渡的线性插值 3.4 机器人手部路径的轨迹规划机器人手部路径的轨迹规划3.4.2 作业的描述作业的描述3.4 机器人手部路径的轨迹规划机器人手部路径的轨迹规划3.4.2 作业的描述作业的描述结结 点点 P0 P1 P2 P2运运 动动 INIT MOVE MOVE GRASP目目 标标 原始原始 接近螺栓接近螺栓 到达到达 抓住抓住3.4 机器人手部路径的轨迹规划机器人手部路径的轨迹规划3.4.2 作业的描述作业的描述P3 P4 P5 P5 P6MOVE MOVE MOVE RELEASE MOVE提升提升 接近托架接近托架 插入孔中插入孔中 松松 夹夹 移开移开
