机器人关节空间轨迹规划--S型速度规划

Posted wdzeng

tags:

篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了机器人关节空间轨迹规划--S型速度规划相关的知识,希望对你有一定的参考价值。

  • 关节空间 VS 操作空间

  关节空间与操作空间轨迹规划流程图如下(上标ii和ff分别代表起始位置initial和目标位置final):

 技术图片

  在关节空间内进行轨迹规划有如下优点:

  1. 在线运算量更小,即无需进行机器人的逆解或正解解算
  2. 不受机器人奇异构型影响
  3. 可以根据机器人或驱动器手册直接确定最大速度或力矩

其缺点是对应操作空间的轨迹无法预测,增加了机械臂与环境碰撞的可能。例如,考虑下面的二连杆机构,关节运动的限制为:0°θ1180°0°≤θ1≤180°,0°θ2150°0°≤θ2≤150°

技术图片

   下图中,左侧为关节空间内规划的线性运动轨迹,而其对应在操作空间的轨迹却是弧线。机构末端的可达空间在图中由灰色背景表示,其大小和形状受关节运动范围的影响。

技术图片

   下图在操作空间中规划了一条直线轨迹,其对应的关节空间轨迹为一弧线,且在运动过程中超出了关节值限制。操作空间内进行轨迹规划优点是直观,缺点是计算量大(需要计算逆解),会遇到奇异性问题以及关节运动超限等。

技术图片

  到底是选择在关节空间还是操作空间内进行轨迹规划,取决于任务需要。需要考虑避障或必须沿特定轨迹运动时选择操作空间轨迹规划,只需考虑速度、力矩、关节范围等运动约束时选择关节空间轨迹规划(The joint space scheme is appropriate to achieve fast motions in a free space)。

  • 梯形速度曲线

  运动控制系统中常用的梯形速度曲线如下图所示,会出现加速度不连续的情形(从kajkaj到0的跳变),这样可能会导致机械系统出现冲击或不可预料的振动,不过由于机械系统存在一定的弹性并不是绝对刚体,这种加速度不连续造成的冲击会被机械机构滤除或减轻。而对于高速重载的机器人来说,这种加速度不连续造成的影响就不能忽略了。可以参考知乎上这个问题:多轴插补为什么普遍使用梯形速度曲线?

技术图片

  •  S型速度曲线

   为了使加速度连续,可对梯形速度规划中的加速度曲线进行修改,使加速度曲线变为连续的二次曲线(a)或者梯形曲线(b),如下图所示。其中,ττ′为加速段时间,λjkvjλjkvj为第jj个关节的最大运动速度

 

技术图片

  下面考虑a方法(Linear Trajectory with Polynomial Blends),关节jj的运动边界条件如下,即关节jj初始时刻位置为qijqji,初始速度加速度为0,ττ′时刻加速到最大速度λikvjsign(Di)λikvjsign(Di),kvjkvj为理论上关节jj允许的最大速度,λjλj为一比例系数(0λj10≤λj≤1),DjDj为从起始位置到目标位置的位移,它是一个有正负的数值。

 

技术图片

  根据边界条件加速度二次曲线表达式为:k(tτ)tk(t−τ′)t,对其进行积分,可得qj˙(t)=16k(2t3τ)t2+Cqj˙(t)=16k(2t−3τ′)t2+C,根据速度边界条件可知C=0C=0,k=6τ3λjkvjk=−6τ′3λjkvj。于是推算出加速度、速度、位置的表达式分别为:

???????qj(t)=qij1τ3λjkvjsign(Dj)(12tτ)t3qj˙(t)=1τ3λjkvjsign(Dj)(2t3τ)t2qj¨(t)=6τ3λjkvjsign(Dj)(tτ)t{qj(t)=qji−1τ′3λjkvjsign(Dj)(12t−τ′)t3qj˙(t)=−1τ′3λjkvjsign(Dj)(2t−3τ′)t2qj¨(t)=−6τ′3λjkvjsign(Dj)(t−τ′)t

 

  加速度在t=τ/2t=τ′/2时最大,其幅值为∣∣q¨jmax∣∣=32λjkvjτ=υjkaj|q¨jmax|=32λjkvjτ′=υjkaj,则有:

τ=32λjkvjυjkajτ′=32λjkvjυjkaj

 

  根据上式和qj(t)qj(t)的表达式,可以计算出加速阶段的位移为:

|qijqj(τ)|=34(λjkvj)2υjkaj|qji−qj(τ′)|=34(λjkvj)2υjkaj

 

  速度曲线与时间轴围成的面积为|Dj||Dj|,根据计算可以得到关系式:

tf=τ+|Dj|λjkvjtf′=τ′+|Dj|λjkvj

 

  在加速度为0的阶段(最大速度阶段,τtτ+hτ′≤t≤τ′+h′),关节速度表达式为:

qi(t)=qj(τ)+(tτ)λjkvjsign(Dj)qi(t)=qj(τ′)+(t−τ′)λjkvjsign(Dj)

 

  减速阶段与加速阶段对称(tf=2τ+htf′=2τ′+h′),减速阶段在时间段τ+httfτ′+h′≤t≤tf′上的轨迹为:

???????qj(t)=qfj+12[1τ3(t3τh)(tτh)3+(2t3τ2h)]λjkvjsign(Dj)qj˙(t)=[1τ3(2t5τ2h)(tτh)2+1]λjkvjsign(Dj)qj¨(t)=6τ3(t2τh)(tτh)λjkvjsign(Dj){qj(t)=qjf+12[1τ′3(t−3τ′−h′)(t−τ′−h′)3+(2t−3τ′−2h′)]λjkvjsign(Dj)qj˙(t)=[1τ′3(2t−5τ′−2h′)(t−τ′−h′)2+1]λjkvjsign(Dj)qj¨(t)=6τ′3(t−2τ′−h′)(t−τ′−h′)λjkvjsign(Dj)

 

  如果目标点距离初始位置过近,可能达不到最大速度和加速度就要开始减速,考虑以最大速度做匀速直线运动阶段的时间为0这种临界状态(The minimum time tftf is obtained when the parameters λjλj and υjυj are the largest),为了能以最大速度运动,位移|Dj||Dj|必须满足如下条件:

|Dj|>32k2vjkaj|Dj|>32kvj2kaj
  如果该条件不能满足,则最大速度值应为:
kvj=23|Dj|kaj−−−−−−−√kvj′=23|Dj|kaj
  前面的计算都只考虑单轴运动的情况,当需要多轴同步时,要考虑运动时间最长的轴(与每个轴的最大速度、运动位移等因素有关),将该时间作为同步运动的时间。在确定了同步时间之后,需要重新计算速度曲线的最大速度(运动快的轴要降低最大速度等待慢的轴),使得各轴在同一时刻到达设定的目标位置。

 

  参考《Modeling Identification and Control of Robots》的第 13.3.4节 Continuous acceleration profile with constant velocity phase 以及 libfranka MotionGenerator,修改关节空间轨迹规划代码,并在while循环中进行轨迹生成的模拟。

  traj.h

技术图片 View Code

   traj.cpp

技术图片 View Code

  main.cpp

技术图片 View Code

  注意以下几点:

  1. 原examples_common.h代码中的ddq_max_start_为加速度,ddq_max_goal_为减速度(接近目标点,开始减速),大多数情况下两者相等

  2. 在根据速度曲线与时间轴围成的面积计算最大同步速度的时候,会遇到一元二次方程av2sync+bvsync+c=0a⋅vsync2+b⋅vsync+c=0求解的问题,对于大于零的两个解要选其中数值小的那个,否则会超过最大速度限制,即取值为bb24ac√2a−b−b2−4ac2a。可以简要证明如下:

  这两个解分布在v=b2av=−b2a的两侧,而b2a=tfka3=13(32kvka+|Dj|kv)ka=12kv+12(2|Dj|ka3kv)−b2a=tfka3=13(32kvka+|Dj|kv)ka=12kv+12(2|Dj|ka3kv),根据|Dj||Dj|的条件 2|Dj|ka3kvkv=13kv(2|Dj|ka3k2v)>02|Dj|ka3kv−kv=13kv(2|Dj|ka−3kv2)>0,因此b2a>kv−b2a>kv,即值较大的解会超出速度限制。

   将时间、轴1轴2的关节角度和速度保存在CSV文件中,用Excel画出散点图。关节角度随时间变化曲线如下(轴1从0→45°,轴2从0→90°):

技术图片

  关节速度随时间变化曲线如下:

技术图片

 

 

 

参考:

S型速度规划

线性轨迹的抛物线过渡(梯形加减速)

工业机器人运动轨迹规划方法简述

机器人中的轨迹规划(Trajectory Planning )

周期同步位置模式(CSP),轮廓位置模式(PPM),位置模式(PM)

libfranka MotionGenerator

Robot and interface specifications

Trajectory Planning for Automatic Machines and Robots

Modeling Identification and Control of Robots. 13.3.4 Continuous acceleration profile with constant velocity phase

 

以上是关于机器人关节空间轨迹规划--S型速度规划的主要内容,如果未能解决你的问题,请参考以下文章

matlab 七段式轨迹 S型速度规划

路径规划基于matlab DWA算法机器人局部避障路径规划含Matlab源码 890期

路径规划基于matlab GUI改进的DWA算法机器人动态避障路径规划含Matlab源码 1271期

Minimum Snap轨迹规划详解

机器人学 —— 轨迹规划(Sampling Method)

机器人学 —— 轨迹规划(Configuration Space)