第7章　机器人路径规划

2020-03-01 160浏览

  • 1.第7章 机器人路径规划 第7章 机器人学简明教程 机器人路径规划 7.1 移动机器人路径规划 7.2 机械臂路径规划
  • 2.第7章 机器人路径规划 7.1 移动机器人路径规划 移动机器人路径规划的任务是指，在已知机器人初始位 姿、给定机器人目标位姿的条件下，在存在障碍的环境中规 划一条无碰撞、时间（能量）最优的路径。若已知环境地图， 即已知机器人模型和障碍模型，可以采用基于模型的路径规 划。若机器人在未知或动态环境中移动，机器人需要向目标 移动，同时需要使用传感器探测障碍，称为基于传感器的路 径规划。本节主要介绍基于模型的路径规划方法。
  • 3.第7章 机器人路径规划 为了简化问题描述，假定机器人为两个自由度，即只考 虑机器人的位置，不考虑其姿态。任务是规划一条路径，使 得机器人从起点达到目标点（终点），同时不与环境中的障 碍发生碰撞。 以平面全向移动机器人为例，假设机器人为半径为r的 圆形机构。首先，由于机器人可以全方向移动，所以可以忽 略移动机器人的方向（姿态的自由度）。其次，因为能用圆 表示机器人，所以可把障碍物沿径向扩张r的宽度，同时将 机器人收缩成一个点（如图7－1所示）。因此，移动机器人 路径规划可以简化为在扩张了障碍物的地图上点机器人的路 径规划问题。
  • 4.第7章 机器人路径规划 图7－1 障碍物扩张法路径规则
  • 5.第7章 机器人路径规划 1.人工势场方法 人工势场的基本思想是构造目标位置引力场和障碍物周 围斥力场共同作用下的人工势场。可以通过搜索势函数的下 降方向来寻找无碰撞路径。下面给出各种势场的定义。 (1)目标引力场： 1 Eatt  p   K pgoal  p 2 2 (7－1) 式中，p是机器人位置;pgoal是目标位置;K是引力常数。
  • 6.第7章 机器人路径规划 (2)障碍物斥力场: 2   1 1       Erep  p    2  p  pobs d 0   0  p  pobs  d 0 else (7－2) 根据式（7－1），机器人受到的引力表示为 Fatt  p   Eatt  K  pgoal  p  (7－3) 根据式（7－2），机器人在障碍物的影响范围内受到的斥力表示为  1 1  p  pobs Frep  p       3 p  p d obs 0  p  pobs  (7－4)
  • 7.第7章 机器人路径规划 参见图7－2可得机器人所受合力为 Ftotal =Fatt + Fobs (7－5) 这样，我们就在环境地图中定义了机器人的引力场，因此， 机器人的路径规划问题被转化为点在引力场中的运动问题。 而点在引力场中的运动问题在物理学和数学中已经研究得非 常清楚，可以比较方便地进行求解。
  • 8.第7章 机器人路径规划 图7-2 机器人受力示意图
  • 9.第7章 机器人路径规划 人工势场方法具有如下优点: （1）简单方便，可以实时规划控制，并能考虑多个障碍， 连续移动。 （2）规划的路径比较平滑安全。 人工势场方法具有如下缺点: (1)规划算法是局部最优算法。 (2)复杂多障碍环境中可能出现局部极值点，即在非目标点 达到平衡状态而停滞，因而不能规划出达到目标点的路径。 前面只介绍了基本的人工势场方法。近年来，针对基本人 工势场方法的不足，人们提出了许多改进的人工势场方法。
  • 10.第7章 机器人路径规划 2.栅格法 将机器人工作空间划分为多个简单区域，称为栅格。若 栅格内没有障碍物称其为自由栅格，否则称为障碍栅格。将 栅格编号，机器人路径规划就是搜索由起点到目标点的自由 栅格组成的连通域。可以用栅格序号表示，再将栅格序号转 换成机器人空间的实际坐标，令机器人按此路径运动。这就 是栅格法的基本思想。图7－3给出了栅格法路径规划的示 意图。
  • 11.第7章 机器人路径规划 图7－3 栅格法的基本思想
  • 12.第7章 机器人路径规划 栅格法路径规划步骤: (1)建立栅格。将机器人和目标点间区域划分栅格，大小与机 器人相关。 (2)障碍地图生成。标注障碍栅格和自由栅格。 (3)搜索无障碍最优路径，采用A（A*）搜索算法、遗传算法、 人工势场、蚁群算法等。 栅格法具有如下优点: （1）若存在最优路径，算法得当一定可以得到问题最优解。 (2)有成熟的路径搜索算法使用。 栅格法具有如下缺点: （1）栅格粒度影响较大。栅格划分细时，存储量大和搜索 时间长。 (2)得到的是折线，需要进行光滑处理。
  • 13.第7章 机器人路径规划 7.2 机械臂路径规划 1.关节空间规划方法 机械臂的期望运动一般由指定的路径点来描述，其中的 每个点都代表工具坐标系{T}相对工作台坐标系{S}的位姿。 我们可以采用第4章介绍的逆运动学方法获得这些路径点对 应的关节角度，该过程实际上就是把路径点由笛卡尔坐标空 间描述转换到关节空间描述。规定机械臂的关节同步运动， 即每个关节角都同时达到路径点期望的角度。上述规定是指 在相邻路径点之间每个关节的运行时间都是相等的。这样， 我们可以独立规划每个关节的轨迹，关节之间没有影响。
  • 14.第7章 机器人路径规划 下面以两关节机械臂对规划的独立性进行说明。假设期 望关节角包括一个中间点，即要求规划的关节角为{θ0，θm， θf}，其中矢量 θ=［θ1,θ2］T 这里使用上标表示关节号，而用下标表示路径点标号。 路径规划问题可以描述为 确定函数θ1(t)和θ2(t)，使之满足:   1 (0)   01  1 1  ( t )    m m  1 (t )   1 f f    2 (0)   02  2 2  ( t )    m m  2 (t )   2 f f  (7－6)
  • 15.第7章 机器人路径规划 其中路径点矢量和分量表示的关系为  θ0  [ 01 ,  02 ]T  1 2 T θ  [  ,   m m m] θ  [ 1 ,  2 ]T f f  f (7－7) 观察式(7－6)和式(7－7)可以发现，函数θ1(t)和θ2(t)之间 是没有直接关联的，只要在路径点处取指定的数值即可满足 规划要求。因此，机械臂轨迹规划问题可以分解为n个独立 的单关节轨迹规划问题。以下部分将只讨论单关节的轨迹规 划问题。
  • 16.第7章 机器人路径规划 2.单区间三次多项式插值 在考虑工具在一定时间内从初始位置移动到目标位置的 问题中，任务就是确定函数θ(t)，使其在t=0时刻的值为关节 角的初始位置，在t=tf时刻的值为关节角的目标位置。另外， 一般要求在初始时刻和终止时刻关节的速度均为零。因此， 关节轨迹规划在数学上就是满足4个约束条件的函数插值问 题。显然，满足该条件的光滑函数不是唯一的。多项式插值 比较简单，便于计算，因此常用来解决函数插值问题。满足 4个约束条件的多项式插值函数是三次多项式。
  • 17.第7章 机器人路径规划 位置约束: 速度约束：  (0)   0   (t f )   f (7－8) (0)  0   (t f )  0 (7－9) 关节角轨迹可以用三次多项式表示为  (t )  a0  a1t  a2t 2  a3t 3 (7－10) 因此关节角速度和加速度轨迹可以表示为 2    ( t )  a  2 a t  3 a t  1 2 3     (t )  2a2  6a3t (7－11)
  • 18.第7章 机器人路径规划 把式(7－8)和式(7－9)代入到式(7－10)和式(7－11)中得:  0  a0  2 3   a  a t  a t  a t 0 1 f 2 f 3 f  f  0  a1 0  a  2a t  3a t 2 1 2 f 3 f  (7－12) 式(7－12)是关于4个未知量a0~a3的线性方程组，其解为 a0   0 a  0  1  3 (7－13) a2  t 2 ( f   0 ) f   2 a3  3 ( 0   f ) tf 
  • 19.第7章 机器人路径规划 将式(7－13)代入到式(7－10)可以得到满足约束条件的 三次多项式: 3 2 2  (t )   0  2 ( f   0 )t  3 ( f   0 )t 3 tf tf (7－14)
  • 20.第7章 机器人路径规划 例7－1 假设一个具有单旋转关节、单自由度的机器人， 处于静止状态时，关节角θ=15°。期望在2s内平滑移动到关 节角θ=75°的目标位置，并在目标位置处于静止状态。求满 足约束条件的三次多项式，并画出关节角位置、速度和加速 度随时间变化的曲线。 解: tf=2，位置和速度约束分别为 (0)  0  (0)    15 0  (t f )   f  75 (t f )  0 代入到 (7-14)式得关节角轨迹：  (t )  15  45t 2  15t 3
  • 21.第7章 机器人路径规划 关节角速度和加速度轨迹为： (t )  90t  45t 2 (t )  90  90t 图7－4给出了机器人关节角位置、速度和加速度随时 间变化的曲线。从图中可以看出，起点和终点的角度值等 于指定角度，且速度为零，加速度呈线性变化。
  • 22.第7章 机器人路径规划 图7－4 关节角位置、速度和加速度随时间变化的曲线
  • 23.第7章 机器人路径规划 3.具有中间点的三次多项式插值 一般情况下，机械臂需要连续经过若干中间点，因此需 要建立满足这些约束的插值函数。若仍然采用三次多项式插 值函数，则必须采用分段插值方法，即在相邻路径点组成的 每个区间内进行三次多项式插值，同时要求在两段曲线的连 接处满足一定的光滑条件。比较简单的做法是，指定中间点 关节角的位置和速度，这样每个区间可以独立进行插值计算。 该方法与单区间三次多项式插值基本类似，只是式(7－9)的 速度约束一般不为零，而是指定的速度: (0)  0    (t f )   f (7－15)
  • 24.第7章 机器人路径规划 满足约束条件的三次多项式系数的四个方程如下:  0   f   0   f  a0  a0  a1t f  a2t f2  a3t 3f  a1 (7－16)  a1  2a2t f  3a3t f2 该线性方程组的解为 a0  0   a   1 0   3 2  1  a  (    )  0   f  2 f 0 2 tf tf tf   2 1  a   (    )  ( f  0 )  3 f 0 3 2 tf tf  (7－17)
  • 25.第7章 机器人路径规划 例7－2 假设一个具有单旋转关节、单自由度的机器人， 起始点和终止点速度为零，且位置满足θ0=15°，θf=45°。设 置一个中间点，位置和速度分别为θm=75°和 θm=－10deg/s。假设两段区间的长度均为2s。求满足约束条 件的分段三次多项式，并画出关节角位置、速度和加速度随 时间变化的曲线。 解:先计算起始点到中间点的三次多项式。其中tf=2，位 置和速度约束分别为 a0   0   a1   0  3 2  1  a  (    )  0   f  2 f 0 2 tf tf tf   2 1  a3   3 ( f   0 )  2 ( f  0 ) tf tf  (7－17)
  • 26.第7章 机器人路径规划 例7－2 假设一个具有单旋转关节、单自由度的机器人， 起始点和终止点速度为零，且位置满足θ0=15°，θf=45°。设 置一个中间点，位置和速度分别为θm=75° . 和θm=－10deg/s。假设两段区间的长度均为2s。求满足约束 条件的分段三次多项式，并画出关节角位置、速度和加速度 随时间变化的曲线。 解：先计算起始点到中间点的三次多项式。其中tf =2， 位置和速度约束分别为：  (0)   0  15  (t f )   f  75 (0)  0 (t )  10 f
  • 27.第7章 机器人路径规划 代入到式(7－17)得关节角轨迹  (t )  15  50t 2  17.5t 3 关节角速度和加速度轨迹为： (t )  100t  52.5t 2 (t )  100  105t 中间点到终止点的三次多项式计算如下： tf =2，位置和速度约束分别为：  (0)   0  75  (t f )   f  45 (0)  10 (t )  0 f
  • 28.第7章 机器人路径规划 代入到 (7-17)式得关节角轨迹：  (t )  75  10t  12.5t  5t 2 3 关节角速度和加速度轨迹为： (t )  10  25t  15t 2 (t )  25  30t 中间点到终止点的三次多项式的计算如下: tf=2，位置和速度约束分别为  (0)   0  75  (t f )   f  45 (0)  10 (t )  0 f
  • 29.第7章 机器人路径规划 图7－5给出了机器人关节角位置、速度和加速度随时间 变化的曲线。从图7－5（a）可以看出，起始点、中间点、终 止点的角度值都等于指定值，且轨迹是光滑的。从图7－5（b） 可以发现，速度在整个区间内都是连续的，但在t=2s时刻曲 线出现折角，即速度的导数是不连续的。在图7－5（c）所示 的加速度轨迹上，我们发现加速度值是线性变化的，但在中 间点t=2s处不连续。
  • 30.第7章 机器人路径规划 图7－5 带中间点的关节角位置、速度和加速度随时间变化的曲线
  • 31.第7章 机器人路径规划 4.具有抛物线拟合的线性插值 连接相邻两个路径点的最简单曲线是直线，因此人们通 常希望采用线性插值，但线性插值在连接点处速度不连续。 获得速度连续的光滑曲线的方法是在直线段两端采用抛物线 拟合段。因为抛物线是二次函数，所以在拟合段内加速度为 常数。采用该方法构造的简单路径如图7－6所示，直线段和 两段抛物线组合成一条位置和速度均连续的路径。图中tb是 连接点所处的时刻，tf-tb是另一个连接时刻，假设采用时间 对称插值。
  • 32.第7章 机器人路径规划 图7－6 带抛物线拟合的线性插值
  • 33.第7章 机器人路径规划 假设两端抛物线拟合段的加速度数值相等（符号相反）， 如图7－6所示，满足条件的解不唯一，但每个解都是关于时 间中点tm和位置中点θm对称的。同时抛物线和直线的连接点 处的速度必须相同，而整个直线段内速度是常值，所以   tb    m b t m  tb (7－18) · ¨ 式中，θb是tb时刻的角度值；θ是拟合段加速度值；θ 是直线 段速度值。θb可以用下式计算（假设初始点速度为零） 1  2  b   0   tb 2 (7-19)
  • 34.第7章 机器人路径规划 将(7-19)式代入到(7-18)式，并且注意到tm=t/2，  m=(q0+qf)/2可以得到以下关系式 tb2  ttb   f  0  0 (7-20) 在式(7－20)中，t是期望运行时间。对于给定的初始、终点位 ¨ 置和期望时间θ 、θ 、t，可以通过式(7－20)选择t 和θ来获得 0 f b ¨ 路径。通常做法是，先选择加速度θ，再根据式(7－20)计算时 间tb。式(7－20)是关于tb的二次方程，有实数解的条件为方程 的判别式大于零，由此得到   4( f   0 ) / t (7-21) 当式(7－21)中等号成立时，tb=t/2，因此直线部分长度变为0， 整个路径由两条抛物线连接而成。当不等号成立时，直线部 分的长度随着加速度的增加而增加。
  • 35.第7章 机器人路径规划 5.具有中间点的三次样条插值 前面介绍的具有中间点的三次多项式插值方法需要指定 中间点的速度，同时在中间点处的加速度不连续。那么能否 得到不需要指定中间点的速度，同时在整个运行时间内位置、 速度和加速度都是连续的插值函数？答案是肯定的，方法就 是采用样条插值技术。样条插值技术被称为20世纪计算数学 的三大发现之一，已经广泛应用于汽车等工业产品的外形设 计当中。下面将简要介绍三次样条插值问题的定义，并对问 题的可解性进行分析。关于三次样条插值详细的分析求解过 程可以在任何一本较详细的数值分析教科书中找到。
  • 36.第7章 机器人路径规划 样条一词来源于工程中的样条曲线。绘图员为了将一些 指定的点（称做样点）连接成一条光滑曲线，用细长的木条 （绘图员称其为样条）把相近的几点连接在一起，再逐步延 伸连接全部样点，使之形成光滑的样条曲线。该曲线在连接 点处具有连续的二阶导数。对绘图员的样条曲线进行模拟， 得到的插值函数叫做样条函数。下面针对关节角轨迹规划问 题，给出常用的三次样条插值函数的定义。
  • 37.第7章 机器人路径规划 在机械臂运行区间［0,tf］上取n+1个时间节点 0=t038.第7章 机器人路径规划 从上面的定义可以发现，样条插值函数θ(t)是分段三次多 项式,即在每个子区间上都是一个三次多项式。确定样条插 值函数θ(t)只需要n+1个路径点θi值，而不需要其导数（速度） 值，且在整个机械臂运行时间内样条插值函数θ(t)的二阶导 数都是连续的。根据微积分知识可知,函数θ(t)及其导数一定 是连续的。函数θ(t)的二阶导数都是连续的,即关节角加速度 连续,使得驱动机械臂运动所需的力矩连续变化,可以使得机 械臂运行平稳,避免了因力矩突变引起的冲击作用。下面分 析满足条件的三次样条插值函数θ(t)的存在性。39.第7章 机器人路径规划 首先分析需要确定的未知量个数。因为每个区间都是一 个三次多项式,因此有4个未知量,共有n个子区间,所以未知量 个数共有4n个。 再分析约束条件个数。每个区间两个端点的函数值是事 先指定的,所以有2n个约束。n-1个中间点处的一阶和二阶导 数(速度和加速度)连续,所以有2(n-1)个约束。因此,约束的个 数为 2n+2(n-1)=4n -2 因此，单从三次样条插值函数θ(t)的定义不能唯一确定函数 本身。对于机械臂轨迹规划问题,一般要求初始点和终止点的 速度(一阶导数)为零,恰好补充两个约束条件。因此,三次样条 插值函数θ(t)可以唯一确定。40.第7章 机器人路径规划 函数的具体确定过程还是比较复杂的,下面只给出求解的 一些思路。因为样条插值函数θ(t)是分段三次多项式,所以它的 二阶导数在每个子区间内是线性函数。可以先将路径点处的二 ¨ 阶导数θi作为未知参数,然后确定每个子区间内的二阶导函数。 因为它是线性的,所以可以由区间端点的两个值唯一确定。再 对该二阶导函数积分两次,得到一阶导数和函数本身在每个子 区间的表达式。最后再用前面提到的函数值约束和一阶导数值 ¨ 约束条件得到关于参数θi的线性方程组，它是三对角的，可以 比较方便地进行求解。 下面以具有一个中间点的关节角轨迹规划问题为例验证三 次样条插值方法。假定两段区间的长度均为tf，指定中间点关 节角的位置（速度值不需指定），轨迹满足的约束条件如下41.第7章 机器人路径规划 第一段曲线方程及其参数为  (t )  a10  a11t  a12t 2  a13t 3 (7-23)  (0)   0   (0)  0  (t f )   m (7-24) 第二段曲线方程及其参数为  (t )  a20  a21t  a22t 2  a23t 3 (7-25)  (0)   m   (t f )  0  (t f )   f (7-26)42.第7章 机器人路径规划 同时要求两段曲线在中间点速度和加速度连续，此时不 能每段独立求解。满足约束条件的两个三次多项式系数的8 个方程如下:  0  a10  m  a10  a11t f  a12t 2f  a13t 3f   m  a20 2 3   a  a t  a t  a t  f 20 21 f 22 f 23 f 0  a 11  2 0  a  2 a t  3 a t 21 22 f 23 f  a21  a11  2 a12t f  3a13t 2f  2a  2a  6a t 12 13 f  22 (7－27)43.第7章 机器人路径规划 该线性方程组的解为 a10   0 a11  0 a  (12  3  9 ) /(4t 2 ) m f 0 f  12 3 a  (  8   3   5  ) /(4 t  13 m f 0 f ) a   m  20 a21  (3 f  30 ) /(4t f ) a22  (12 m  6 f  60 ) /(4t 2f ) a  (8  5  3 ) /(4t 3 ) m f 0 f  23 (7－28) 在每个区间的三次多项式系数使用式(7－28)的结果即可得到 各路径点，且关节角轨迹在整个运行时间内位置、速度和加 速度都是连续的。44.第7章 机器人路径规划 例7－3 假设一个具有单旋转关节、单自由度的机器人， 起始点和终止点速度为零，且位置满足θ0=15°，θf=45°。设 置一个中间点，位置为θm=75°。假设两段区间的长度均为2s。 求满足约束条件的三次样条插值多项式，并画出关节角位置、 速度和加速度随时间变化的曲线。 解:先计算起始点到中间点的三次多项式。其中tf=2，位 置和速度约束分别为  0  15,  m  75,  f  45 代入到 (7-28)式得第一段关节角轨迹：  (t )  15  39.375t 2  12.1875t 345.第7章 机器人路径规划 关节角速度和加速度轨迹为： (t )  78.75t  36.5625t 2 (t )  78.75  73.125t 第二段关节角轨迹：  (t )  75  11.25t  33.75t 2  10.3125t 3 关节角速度和加速度轨迹为： (t )  11.25  67.5t  30.9375t 2 (t )  67.5  61.875t46.第7章 机器人路径规划 图7－7给出了采用三次样条插值生成的机器人关节角位 置、速度和加速度随时间变化的曲线。为了表示清楚，在每 张图的两段曲线连接处画了一条竖直虚线。从图7－7（a） 可以看出，起始点、中间点、终止点的角度值都等于指定值， 且轨迹是光滑的。从图7－7（b）可以发现，速度在整个区 间内都是连续的，而且在t=2s时刻曲线也是光滑的，即速度 的导数是连续的。在图7－7（c）的角加速度轨迹上，加速 度值是线性变化的，且在中间点t=2s处出现转折点，但函数 本身是连续的。与图7－5对比，样条插值技术使两段轨迹实 现了完美连接。47.第7章 机器人路径规划 图7－7 采用样条插值的关节角位置、速度和加速度随时间变化的曲线48.第7章 机器人路径规划 6.笛卡尔空间规划方法 前面介绍的关节空间规划方法可以保证机械臂能够达到 中间点和目标点。但是连接这些点的中间路径在笛卡尔空间 可能是非常复杂的，其复杂程度取决于机械臂的运动学结构。 如果我们关心机械臂在笛卡尔空间的整个路径，而不仅仅是 关键点，如期望工具直线运动、画圆等，就需要采用笛卡尔 空间规划方法。49.第7章 机器人路径规划 笛卡尔空间路径规划方法的思路与关节空间规划方法相 同，一般事先指定若干路径点，再采用插值技术确定整个路 径。但不同的是，这里确定的不再是关节角轨迹，而是工具 坐标系{T}相对工作台坐标系{S}位姿（坐标变换矩阵）的 轨迹。笛卡尔空间路径规划阶段不需要求解逆运动学问题， 但是在机械臂执行该路径时，一般需要实时求解逆运动学问 题，因为机械臂运动是通过关节运动来实现的。50.第7章 机器人路径规划 假设我们希望规划简单的路径，使工具末端在空间做直 线运动。在规划笛卡尔直线路径时，最好使用带抛物线拟合 的直线插值。在每段路径的直线部分，工具末端位置的三个 分量按线性变化，并在笛卡尔空间做直线运动。但是，如果 在每个路径点处用旋转矩阵表示工具的姿态，则不能用插值 方法确定整个路径。因为旋转矩阵是正交矩阵，在两个有效 的正交矩阵之间对矩阵元素进行线性插值并不能保证生成的 插值矩阵满足正交条件。 可以采用3个独立参数描述姿态，如用欧拉角或者等效 转轴描述，这样，每个路径点的位姿都用6个独立参数来定 义。可以采用与前面关节空间轨迹生成完全类似的方法来生 成轨迹，路径点之间选择固定的运动时间，每个参数独立生 成轨迹即可。51.第7章 机器人路径规划 笛卡尔空间路径规划方法需要考虑与工作空间和奇异点 有关的问题。一个问题是即使起始点和终止点都在工作空间 内部，插值得到的点也可能不在工作空间之内，即可能存在 不可达的中间点问题。另一个问题是在奇异位姿附近，实现 规划轨迹可能需要非常高的速度，如例5－3要求关节的速度 为无穷大，即在奇异点附近需要高关节速率。 因此，在笛卡尔空间规划路径要比在关节空间规划路径 困难，所以除非必要，一般均采用关节空间路径规划方法。 同时，由于机械臂驱动能力总是有限的，所以对关节的速度 和加速度需要进行限制。若规划的轨迹不满足要求，可以通 过增加运行时间的方法来减小关节的速度和加速度。
  • 38.第7章 机器人路径规划 从上面的定义可以发现，样条插值函数θ(t)是分段三次多 项式,即在每个子区间上都是一个三次多项式。确定样条插 值函数θ(t)只需要n+1个路径点θi值，而不需要其导数（速度） 值，且在整个机械臂运行时间内样条插值函数θ(t)的二阶导 数都是连续的。根据微积分知识可知,函数θ(t)及其导数一定 是连续的。函数θ(t)的二阶导数都是连续的,即关节角加速度 连续,使得驱动机械臂运动所需的力矩连续变化,可以使得机 械臂运行平稳,避免了因力矩突变引起的冲击作用。下面分 析满足条件的三次样条插值函数θ(t)的存在性。
  • 39.第7章 机器人路径规划 首先分析需要确定的未知量个数。因为每个区间都是一 个三次多项式,因此有4个未知量,共有n个子区间,所以未知量 个数共有4n个。 再分析约束条件个数。每个区间两个端点的函数值是事 先指定的,所以有2n个约束。n-1个中间点处的一阶和二阶导 数(速度和加速度)连续,所以有2(n-1)个约束。因此,约束的个 数为 2n+2(n-1)=4n -2 因此，单从三次样条插值函数θ(t)的定义不能唯一确定函数 本身。对于机械臂轨迹规划问题,一般要求初始点和终止点的 速度(一阶导数)为零,恰好补充两个约束条件。因此,三次样条 插值函数θ(t)可以唯一确定。
  • 40.第7章 机器人路径规划 函数的具体确定过程还是比较复杂的,下面只给出求解的 一些思路。因为样条插值函数θ(t)是分段三次多项式,所以它的 二阶导数在每个子区间内是线性函数。可以先将路径点处的二 ¨ 阶导数θi作为未知参数,然后确定每个子区间内的二阶导函数。 因为它是线性的,所以可以由区间端点的两个值唯一确定。再 对该二阶导函数积分两次,得到一阶导数和函数本身在每个子 区间的表达式。最后再用前面提到的函数值约束和一阶导数值 ¨ 约束条件得到关于参数θi的线性方程组，它是三对角的，可以 比较方便地进行求解。 下面以具有一个中间点的关节角轨迹规划问题为例验证三 次样条插值方法。假定两段区间的长度均为tf，指定中间点关 节角的位置（速度值不需指定），轨迹满足的约束条件如下
  • 41.第7章 机器人路径规划 第一段曲线方程及其参数为  (t )  a10  a11t  a12t 2  a13t 3 (7-23)  (0)   0   (0)  0  (t f )   m (7-24) 第二段曲线方程及其参数为  (t )  a20  a21t  a22t 2  a23t 3 (7-25)  (0)   m   (t f )  0  (t f )   f (7-26)
  • 42.第7章 机器人路径规划 同时要求两段曲线在中间点速度和加速度连续，此时不 能每段独立求解。满足约束条件的两个三次多项式系数的8 个方程如下:  0  a10  m  a10  a11t f  a12t 2f  a13t 3f   m  a20 2 3   a  a t  a t  a t  f 20 21 f 22 f 23 f 0  a 11  2 0  a  2 a t  3 a t 21 22 f 23 f  a21  a11  2 a12t f  3a13t 2f  2a  2a  6a t 12 13 f  22 (7－27)
  • 43.第7章 机器人路径规划 该线性方程组的解为 a10   0 a11  0 a  (12  3  9 ) /(4t 2 ) m f 0 f  12 3 a  (  8   3   5  ) /(4 t  13 m f 0 f ) a   m  20 a21  (3 f  30 ) /(4t f ) a22  (12 m  6 f  60 ) /(4t 2f ) a  (8  5  3 ) /(4t 3 ) m f 0 f  23 (7－28) 在每个区间的三次多项式系数使用式(7－28)的结果即可得到 各路径点，且关节角轨迹在整个运行时间内位置、速度和加 速度都是连续的。
  • 44.第7章 机器人路径规划 例7－3 假设一个具有单旋转关节、单自由度的机器人， 起始点和终止点速度为零，且位置满足θ0=15°，θf=45°。设 置一个中间点，位置为θm=75°。假设两段区间的长度均为2s。 求满足约束条件的三次样条插值多项式，并画出关节角位置、 速度和加速度随时间变化的曲线。 解:先计算起始点到中间点的三次多项式。其中tf=2，位 置和速度约束分别为  0  15,  m  75,  f  45 代入到 (7-28)式得第一段关节角轨迹：  (t )  15  39.375t 2  12.1875t 3
  • 45.第7章 机器人路径规划 关节角速度和加速度轨迹为： (t )  78.75t  36.5625t 2 (t )  78.75  73.125t 第二段关节角轨迹：  (t )  75  11.25t  33.75t 2  10.3125t 3 关节角速度和加速度轨迹为： (t )  11.25  67.5t  30.9375t 2 (t )  67.5  61.875t
  • 46.第7章 机器人路径规划 图7－7给出了采用三次样条插值生成的机器人关节角位 置、速度和加速度随时间变化的曲线。为了表示清楚，在每 张图的两段曲线连接处画了一条竖直虚线。从图7－7（a） 可以看出，起始点、中间点、终止点的角度值都等于指定值， 且轨迹是光滑的。从图7－7（b）可以发现，速度在整个区 间内都是连续的，而且在t=2s时刻曲线也是光滑的，即速度 的导数是连续的。在图7－7（c）的角加速度轨迹上，加速 度值是线性变化的，且在中间点t=2s处出现转折点，但函数 本身是连续的。与图7－5对比，样条插值技术使两段轨迹实 现了完美连接。
  • 47.第7章 机器人路径规划 图7－7 采用样条插值的关节角位置、速度和加速度随时间变化的曲线
  • 48.第7章 机器人路径规划 6.笛卡尔空间规划方法 前面介绍的关节空间规划方法可以保证机械臂能够达到 中间点和目标点。但是连接这些点的中间路径在笛卡尔空间 可能是非常复杂的，其复杂程度取决于机械臂的运动学结构。 如果我们关心机械臂在笛卡尔空间的整个路径，而不仅仅是 关键点，如期望工具直线运动、画圆等，就需要采用笛卡尔 空间规划方法。
  • 49.第7章 机器人路径规划 笛卡尔空间路径规划方法的思路与关节空间规划方法相 同，一般事先指定若干路径点，再采用插值技术确定整个路 径。但不同的是，这里确定的不再是关节角轨迹，而是工具 坐标系{T}相对工作台坐标系{S}位姿（坐标变换矩阵）的 轨迹。笛卡尔空间路径规划阶段不需要求解逆运动学问题， 但是在机械臂执行该路径时，一般需要实时求解逆运动学问 题，因为机械臂运动是通过关节运动来实现的。
  • 50.第7章 机器人路径规划 假设我们希望规划简单的路径，使工具末端在空间做直 线运动。在规划笛卡尔直线路径时，最好使用带抛物线拟合 的直线插值。在每段路径的直线部分，工具末端位置的三个 分量按线性变化，并在笛卡尔空间做直线运动。但是，如果 在每个路径点处用旋转矩阵表示工具的姿态，则不能用插值 方法确定整个路径。因为旋转矩阵是正交矩阵，在两个有效 的正交矩阵之间对矩阵元素进行线性插值并不能保证生成的 插值矩阵满足正交条件。 可以采用3个独立参数描述姿态，如用欧拉角或者等效 转轴描述，这样，每个路径点的位姿都用6个独立参数来定 义。可以采用与前面关节空间轨迹生成完全类似的方法来生 成轨迹，路径点之间选择固定的运动时间，每个参数独立生 成轨迹即可。
  • 51.第7章 机器人路径规划 笛卡尔空间路径规划方法需要考虑与工作空间和奇异点 有关的问题。一个问题是即使起始点和终止点都在工作空间 内部，插值得到的点也可能不在工作空间之内，即可能存在 不可达的中间点问题。另一个问题是在奇异位姿附近，实现 规划轨迹可能需要非常高的速度，如例5－3要求关节的速度 为无穷大，即在奇异点附近需要高关节速率。 因此，在笛卡尔空间规划路径要比在关节空间规划路径 困难，所以除非必要，一般均采用关节空间路径规划方法。 同时，由于机械臂驱动能力总是有限的，所以对关节的速度 和加速度需要进行限制。若规划的轨迹不满足要求，可以通 过增加运行时间的方法来减小关节的速度和加速度。