第7章 机器人路径规划

ching

2019/09/02 发布于 教育 分类

文字内容
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=t0 <t1 <t2 <<tn-1 <tn=tf (7-22) 给出这些点处关节角位置函数的n+1个值(路径点)qi, i=0,1,2,…,n。要求构造一个三次样条插值函数q(t),满足以下 条件: ⑴ θ (ti)=θi,i=0,1,2,…,n。 ⑵ 在每个子区间[ti, ti+1]上,θ (ti)是三次多项式。 ⑶ θ (t)在整个运行区间[0, tf]上具有二阶连续导数。
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要求关节的速度 为无穷大,即在奇异点附近需要高关节速率。 因此,在笛卡尔空间规划路径要比在关节空间规划路径 困难,所以除非必要,一般均采用关节空间路径规划方法。 同时,由于机械臂驱动能力总是有限的,所以对关节的速度 和加速度需要进行限制。若规划的轨迹不满足要求,可以通 过增加运行时间的方法来减小关节的速度和加速度。