1第四章 运动轨迹生成第四章 运动轨迹生成?4.1 概述概述?4.2 关节空间轨迹生成关节空间轨迹生成?4.3 直角空间轨迹生成直角空间轨迹生成?4.4 轨迹的实时生成轨迹的实时生成LEC.8-0724.1 概述4.1 概述?研究内容研究内容（1）如何描述机器人的运动轨迹）如何描述机器人的运动轨迹q(t)或或X(t)；（；（2）运动轨迹的生成方法。）运动轨迹的生成方法。?三个问题（三个问题（1）描述机器人的空间运动）描述机器人的空间运动允许用户用比较简单的方式描述机器人的运动，而复杂的细节问题则由计算机系统解决。用一条轨迹来 通过或逼近结点，该轨迹按一定原则优化（如加速度平滑、精确 等）。轨迹应在满足作业要求的同时尽量简化计算。如：用户可只 给出末端的目标位姿，让系统由此确定到达目标的途径点、持续时 间、速度等轨迹参数。允许用户用比较简单的方式描述机器人的运动，而复杂的细节问题则由计算机系统解决。用一条轨迹来 通过或逼近结点，该轨迹按一定原则优化（如加速度平滑、精确 等）。轨迹应在满足作业要求的同时尽量简化计算。如：用户可只 给出末端的目标位姿，让系统由此确定到达目标的途径点、持续时 间、速度等轨迹参数。轨迹规划轨迹规划确定轨迹参数的过程。确定轨迹参数的过程。3轨迹生成的三个问题轨迹生成的三个问题（2）建立轨迹的计算机内部描述）建立轨迹的计算机内部描述根据所确定的轨迹参数，在计算机内部描述期望的轨迹。这主要是选根据所确定的轨迹参数，在计算机内部描述期望的轨迹。这主要是选择合理的软件数据结构。择合理的软件数据结构。（3）生成轨迹）生成轨迹对内部描述的轨迹进行实际计算，即根据轨迹参数（如途径点位姿、对内部描述的轨迹进行实际计算，即根据轨迹参数（如途径点位姿、速度、加速度、轨迹函数的系数等）生成出整个轨迹。速度、加速度、轨迹函数的系数等）生成出整个轨迹。计算是实时进行的，每一个轨迹点的计算速度称为轨迹更新速度。在计算是实时进行的，每一个轨迹点的计算速度称为轨迹更新速度。在典型的机器人系统中，轨迹更新速度为典型的机器人系统中，轨迹更新速度为501000HZ.4轨迹规划器轨迹规划器)(),(),(tqtqtq? ?)(),(),(tXtXtX? ?5轨迹描述和生成的一般考虑（轨迹描述和生成的一般考虑（1）1 运动任务描述运动任务描述把机器人的运动作为把机器人的运动作为工具工具标架相对于标架相对于台台/基座基座标架的运动。标架的运动。优点：优点：1）有利于用户的想象、说明、任务描述；）有利于用户的想象、说明、任务描述；2）把运动的描述从任何特定的机器人、末端执行器或工件的耦合中解脱出来。这导致某种标准化，将可以把这 个轨迹描述用于不同的机器人或相同的机器人而工具尺 寸不同。）把运动的描述从任何特定的机器人、末端执行器或工件的耦合中解脱出来。这导致某种标准化，将可以把这 个轨迹描述用于不同的机器人或相同的机器人而工具尺 寸不同。6轨迹描述和生成的一般考虑（轨迹描述和生成的一般考虑（2）2 轨迹描述方法轨迹描述方法1） PTP方法方法让机器人从一个初始位置运动到某个目标位置，对中间轨迹无要求。让机器人从一个初始位置运动到某个目标位置，对中间轨迹无要求。用户须明确规定初始点、末点或加上若干个中间点的位姿、速度、加速度的约束，由系统轨迹规划器选择合适的满足上述约束的轨 迹。通常在关节空间描述。用户须明确规定初始点、末点或加上若干个中间点的位姿、速度、加速度的约束，由系统轨迹规划器选择合适的满足上述约束的轨 迹。通常在关节空间描述。2） CP方法方法用户明确规定机器人末端必须经过的连续轨迹，如直线、圆及其它空间曲线。通常在直角空间描述。用户明确规定机器人末端必须经过的连续轨迹，如直线、圆及其它空间曲线。通常在直角空间描述。两种方法：两种方法：（1） 用直线或曲线方程描述；） 用直线或曲线方程描述；（2）给出一连串的期望经过点（如通过示教方法）来近似表示某一 轨迹。）给出一连串的期望经过点（如通过示教方法）来近似表示某一 轨迹。7轨迹描述和生成的一般考虑（轨迹描述和生成的一般考虑（3）3约束条件约束条件1） 运动约束） 运动约束主要要求是运动光滑。（主要要求是运动光滑。（1） 轨迹函数连续且其一阶导数连续；（） 轨迹函数连续且其一阶导数连续；（2） 除） 除1）以外，还要求加速度连续，以减少冲击；（）以外，还要求加速度连续，以减少冲击；（3） 多余的运动如摆动、超调和停顿应最小。） 多余的运动如摆动、超调和停顿应最小。2） 障碍约束） 障碍约束3） 力矩约束） 力矩约束84.2 关节空间轨迹生成4.2 关节空间轨迹生成?关节空间法关节空间法用机器人关节角度的函数来描述运动轨迹用机器人关节角度的函数来描述运动轨迹?轨迹结点轨迹结点（中间点（中间点/途径点）通常用工具标架相对于台标架的位姿来表示。在每一个结点上，通过逆运动学计算可以途径点）通常用工具标架相对于台标架的位姿来表示。在每一个结点上，通过逆运动学计算可以?如果能为每一个关节如果能为每一个关节找到一个平滑函数找到一个平滑函数，它通过各结点， 并在，它通过各结点， 并在CP场合能逼近理想轨迹，就可生成整个机器人从起始点，通过中间结点，最后到达终点的满足要求的运动轨迹场合能逼近理想轨迹，就可生成整个机器人从起始点，通过中间结点，最后到达终点的满足要求的运动轨迹?只要在任何一段轨迹内只要在任何一段轨迹内每一个关节的运动时间都相同每一个关节的运动时间都相同，则 所有的关节都将同时到达途径点和终止点。，则 所有的关节都将同时到达途径点和终止点。?工具标架工具标架在各个结点上，具有预期的直角坐标位姿在各个结点上，具有预期的直角坐标位姿。得到其相应的一组关节的角度。得到其相应的一组关节的角度。9关节空间法的特点关节空间法的特点?计算过程简单；计算过程简单；?没有奇异点问题；没有奇异点问题；?可以确切反映关节的可以确切反映关节的位置、速度、加速度位置、速度、加速度极限，可实现快速启极限，可实现快速启动；动；?适用于大范围内不要适用于大范围内不要求路径限制及精确定求路径限制及精确定位的运动；位的运动；?在结点之间的轨迹形状在关节空间虽相当简单，但在直角坐标空间 的描述是复杂的，难以想像。在结点之间的轨迹形状在关节空间虽相当简单，但在直角坐标空间 的描述是复杂的，难以想像。10一、三次多项式一、三次多项式?问题问题已知：已知：起始点的关节角度，终点关节角度起始点的关节角度，终点关节角度求：求：用来对关节量进行插值的光滑函数为实现单个关节的平稳运动，至少需要满足四个约束条件：考虑一种最简单的情况，有由此可唯一确定一个三次多项式：关节速度和加速度为：可解得用来对关节量进行插值的光滑函数为实现单个关节的平稳运动，至少需要满足四个约束条件：考虑一种最简单的情况，有由此可唯一确定一个三次多项式：关节速度和加速度为：可解得注：注：这组解只适用于关节初始速度和末速度为这组解只适用于关节初始速度和末速度为0的场合。的场合。( )00t( )fft ( )t ( )t( )( )( )( )0, 00,00=ffftt?( )3 32 210tatataat+= ( ) ( )+=+=taattataat322 321 6232 ? ?()()0330221002,3, 0,=f ff ftataaa11举例举例?设设只有一个旋转关节的单杆机器人处于静止状态时，只有一个旋转关节的单杆机器人处于静止状态时，15度，要求它 在度，要求它 在3s之内平稳到达之内平稳到达75度，末点速度为度，末点速度为0。?求求其轨迹。其轨迹。?解：解：把，值代入上式，可求出三次多项式的系数：把，值代入上式，可求出三次多项式的系数：?由此可得由此可得0f0f0? f?st44. 4, 0 .20, 0, 0 .153210=aaaa( )3244. 40 .200 .15ttt+= ( ) ( )= ttttt 66.260 .4033.130 .402 ? ?12二、带中间点的轨迹三次多项式二、带中间点的轨迹三次多项式?问题问题如果机器人在途径点停留，可直接应用前面的三次多项式的解。但通常我们希望能不停地通过中间点，所以有必要推广上述方法。如果机器人在途径点停留，可直接应用前面的三次多项式的解。但通常我们希望能不停地通过中间点，所以有必要推广上述方法。?方法方法只要把所有的中间结点也看作是一些终止点或起始点只要把所有的中间结点也看作是一些终止点或起始点，求解逆运动学，得到一组关节值，然后就可以确定相应的三次多项式，从而把结点 平滑地连接起来。 设结点上关节速度为已知，则约束式变为：可求得三次多项式的四个系数为：，求解逆运动学，得到一组关节值，然后就可以确定相应的三次多项式，从而把结点 平滑地连接起来。 设结点上关节速度为已知，则约束式变为：可求得三次多项式的四个系数为：( )( )( )( )fffftt?=,0,000()()()020330022010012,123,?+=f ff ff fff fttatttaaa13轨迹三次多项式（续）轨迹三次多项式（续）?由上式确定的三次多项式描述了始点和终点为任意位置和速度的运 动轨迹。由上式确定的三次多项式描述了始点和终点为任意位置和速度的运 动轨迹。?在各个结点规定关节速度，然后简单地对每个段应用上式即可求出 各段的轨迹方程。在各个结点规定关节速度，然后简单地对每个段应用上式即可求出 各段的轨迹方程。?在起点加速度有突变，柔性冲击。在起点加速度有突变，柔性冲击。?确定各结点关节速度的三种方法：确定各结点关节速度的三种方法：1. 根据该结点直角空间速度确定关节空间速度；需求出雅可比矩阵的逆，有奇异点问题。根据该结点直角空间速度确定关节空间速度；需求出雅可比矩阵的逆，有奇异点问题。2. 在直角空间或关节空间采用适当的启发式方法，由控制系统自动地选择途径点速度。在直角空间或关节空间采用适当的启发式方法，由控制系统自动地选择途径点速度。3. 根据保证每个结点的加速度连续的原则，由控制系统自动地选择结点的速度。根据保证每个结点的加速度连续的原则，由控制系统自动地选择结点的速度。14利用启发式信息确定关节速度利用启发式信息确定关节速度?假设用直线段把这些结点连接起来，若相邻线段的斜率在 结点处改变符号，就把速度选定为假设用直线段把这些结点连接起来，若相邻线段的斜率在 结点处改变符号，就把速度选定为0；若斜率不改变符号 ，则选取结点两侧线段斜率的平均值作为该结点的速度。；若斜率不改变符号 ，则选取结点两侧线段斜率的平均值作为该结点的速度。?只要给出了期望的途径点，系统就能用这种方法自动地生成相应的速度。只要给出了期望的途径点，系统就能用这种方法自动地生成相应的速度。?从概念到方法都很简单。从概念到方法都很简单。15根据加速度连续原则确定关节速度根据加速度连续原则确定关节速度?思路思路为保证途径点的加速度连续，可以设法用两条三次曲线在结点处连接起来，拚成期望的轨迹。为保证途径点的加速度连续，可以设法用两条三次曲线在结点处连接起来，拚成期望的轨迹。拚接的约束条件拚接的约束条件是：连接处不但速度连续，而且要求加速度连续。是：连接处不但速度连续，而且要求加速度连续。?方法方法设所考虑的结点处关节角度为，与该结点相邻的前后 两点的关节角度分别为和，相应于从运动到的三次多项式为相应于从运动到的三次多项式为上述两个三次多项式的时间区间分别为设所考虑的结点处关节角度为，与该结点相邻的前后 两点的关节角度分别为和，相应于从运动到的三次多项式为相应于从运动到的三次多项式为上述两个三次多项式的