zoukankan      html  css  js  c++  java
  • 机器人关节空间轨迹规划--S型速度规划

    • 关节空间 VS 操作空间

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

     

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

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

    其缺点是对应操作空间的轨迹无法预测,增加了机械臂与环境碰撞的可能。例如,考虑下面的二连杆机构,关节运动的限制为:0θ11800∘≤θ1≤180∘,0θ21500∘≤θ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

     
  • 相关阅读:
    mfc cef<转>
    js人形时钟
    opencv给图片添加文字水印<转>
    strcore.cpp(156) 内存泄漏
    WebAssembly相关
    mingw 搭建Emscripten 环境
    mingw 设置python 设置git环境变量
    android平台yuv缩放相关<转>
    多媒体基础知识之PCM数据《 转》
    iOS 5 故事板进阶(2)
  • 原文地址:https://www.cnblogs.com/wdzeng/p/11633512.html
Copyright © 2011-2022 走看看