精华内容
下载资源
问答
  • R系列S型隔离安全栅

    2021-03-23 19:05:25
    R系列S型隔离安全栅.pdf 介绍了关于R系列S型隔离安全栅的详细说明,提供宇通的技术资料的下载。
  • R系列S型隔离安全栅RPA-210XS-Ex.pdf 介绍了关于R系列S型隔离安全栅RPA-210XS-Ex的详细说明,提供宇通的技术资料的下载。
  • 文章目录一、7种速度规划对比1、梯形速度规划2、余弦速度规划3、多项式速度规划4、7段S型速度规划5、7段修正S型速度规划6、15段S型速度规划7、31段S型速度规划二、7种速度规划示例演示 一、7种速度规划对比   一般...

    文章目录

    一、7种速度规划对比

      一般地,速度规划输入参数为:长度 L L L,起始速度 v s v_s vs,结束速度 v e v_e ve,最大速度 v c v_c vc,加速度 a c c acc acc,减速度 d e c dec dec,二次加速度 j e r k jerk jerk,三次加速度 s n a p snap snap,四次加速度 c r a c k l e crackle crackle。下面通过对比7种速度规划生成的位置、速度、加速度、二次加速度、三次加速度及四次加速度曲线的连续性与平滑性,可控性(是否可以限制在给定值以内),规划总时间长短,算法计算量四个方面来比较7种速度规划算法。

    1、梯形速度规划

      (1) 位置平滑,速度连续但不平滑,加速度跳变,二次加速度出现无穷大跳变;
      (2) 速度、加速度可控,更高阶不可控;
      (3) 所有速度规划算法中规划时间最短;
      (4) 算法最简单、计算量最小。

    1.1 只有一段

    1.1.1 加速段

      当 L = 1 , v s = 0 , v c = 10 , v e = 10 , a c c = 50 , d e c = 123 ( 任 意 ) L=1, v_s=0, v_c=10, v_e=10, acc =50, dec=123(任意) L=1,vs=0,vc=10,ve=10,acc=50,dec=123()时,只有加速段。
    在这里插入图片描述

    1.1.2匀速段

      当 L = 1 , v s = 1 , v c = 1 , v e = 1 , a c c = 123 ( 任 意 ) , d e c = 123 ( 任 意 ) L=1, v_s=1, v_c=1, v_e=1, acc =123(任意), dec=123(任意) L=1,vs=1,vc=1,ve=1,acc=123(),dec=123()时,只有匀速段。
    在这里插入图片描述

    1.1.3减速段

      当 L = 1 , v s = 10 , v c = 10 , v e = 0 , a c c = 123 ( 任 意 ) , d e c = 50 L=1, v_s=10, v_c=10, v_e=0, acc =123(任意), dec=50 L=1,vs=10,vc=10,ve=0,acc=123(),dec=50时,只有减速段。
    在这里插入图片描述

    1.2 只有两段

    1.2.1 加速段+匀速段

      当 L = 1 , v s = 0 , v c = 10 , v e = 10 , a c c = 80 , d e c = 123 ( 任 意 ) L=1, v_s=0, v_c=10, v_e=10, acc =80, dec=123(任意) L=1,vs=0,vc=10,ve=10,acc=80,dec=123()时,只有加速段+匀速段 。
    在这里插入图片描述

    1.2.2匀速段 +减速段

      当 L = 1 , v s = 10 , v c = 10 , v e = 0 , a c c = 123 ( 任 意 ) , d e c = 80 L=1, v_s=10, v_c=10, v_e=0, acc =123(任意), dec=80 L=1,vs=10,vc=10,ve=0,acc=123(),dec=80时,只有匀速段 +减速段。
    在这里插入图片描述

    1.2.3加速段+减速段

      当 L = 1 , v s = 1 , v c = 6 , v e = 2 , a c c = 20 , d e c = 30 L=1, v_s=1, v_c=6, v_e=2, acc =20,dec=30 L=1,vs=1,vc=6,ve=2,acc=20,dec=30时,只有加速段+减速段 。
    在这里插入图片描述

    1.3 有三段(加速段+匀速段 +减速段)

      当 L = 1 , v s = 1 , v c = 3 , v e = 2 , a c c = 40 , d e c = 30 L=1, v_s=1, v_c=3, v_e=2, acc =40 ,dec=30 L=1,vs=1,vc=3,ve=2,acc=40,dec=30时,有加速段+匀速段 +减速段。
    在这里插入图片描述

    2、余弦速度规划

      (1) 位置平滑,速度平滑,加速度连续但不够平滑,二次加速度跳变;
      (2) 速度、加速度可控,更高阶不可控;
      (3) 规划时间较长;
      (4) 算法较简单、计算量较小。

    2.1 只有一段

    2.1.1 加速段

      当 L = 1 , v s = 0 , v c = 10 , v e = 10 , a c c = 78.5398 , d e c = 123 ( 任 意 ) L=1, v_s=0, v_c=10, v_e=10, acc =78.5398, dec=123(任意) L=1,vs=0,vc=10,ve=10,acc=78.5398,dec=123()时,只有加速段。
    在这里插入图片描述

    2.1.2 匀速段

      当 L = 1 , v s = 1 , v c = 1 , v e = 1 , a c c = 123 ( 任 意 ) , d e c = 123 ( 任 意 ) L=1, v_s=1, v_c=1, v_e=1, acc =123(任意), dec=123(任意) L=1,vs=1,vc=1,ve=1,acc=123(),dec=123()时,只有匀速段。
    在这里插入图片描述

    2.1.3 减速段

      当$L=1, v_s=10, v_c=10, v_e=0, acc =123(任意), dec=78.5398时,只有减速段。
    在这里插入图片描述

    2.2 只有两段

    2.2.1 加速段+匀速段

      当 L = 1 , v s = 0 , v c = 10 , v e = 10 , a c c = 100 , d e c = 123 ( 任 意 ) L=1, v_s=0, v_c=10, v_e=10, acc =100, dec=123(任意) L=1,vs=0,vc=10,ve=10,acc=100,dec=123()时,只有加速段+匀速段 。
    在这里插入图片描述

    2.2.2 匀速段+减速段

      当 L = 1 , v s = 10 , v c = 10 , v e = 0 , a c c = 123 ( 任 意 ) , d e c = 100 L=1, v_s=10, v_c=10, v_e=0, acc =123(任意), dec=100 L=1,vs=10,vc=10,ve=0,acc=123(),dec=100时,只有匀速段 +减速段。
    在这里插入图片描述

    2.2.2 加速段+减速段

      当 L = 1 , v s = 1 , v c = 6 , v e = 2 , a c c = 20 , d e c = 30 L=1, v_s=1, v_c=6, v_e=2, acc =20, dec=30 L=1,vs=1,vc=6,ve=2,acc=20,dec=30时,只有加速段+减速段 。
    在这里插入图片描述

    2.3 有三段(加速段+匀速段 +减速段)

      当 L = 1 , v s = 1 , v c = 3 , v e = 2 , a c c = 40 , d e c = 30 L=1, v_s=1, v_c=3, v_e=2, acc =40 ,dec=30 L=1,vs=1,vc=3,ve=2,acc=40,dec=30时,有加速段+匀速段 +减速段。
    在这里插入图片描述

    3、多项式速度规划

      (1) 位置平滑,速度平滑,加速度平滑,二次加速度连续但不够平滑,三次加速度跳变;
      (2) 速度、加速度、二次加速度可控,更高阶不可控;
      (3) 规划时间长;
      (4) 算法较简单、计算量较小。

    3.1 只有一段

    3.1.1 加速段

      当 L = 1 , v s = 0 , v c = 10 , v e = 10 , a c c = 93.75 , d e c = 123 ( 任 意 ) , j e r k = 2000 L=1, v_s=0, v_c=10, v_e=10, acc =93.75, dec=123(任意),jerk=2000 L=1,vs=0,vc=10,ve=10,acc=93.75,dec=123(),jerk=2000时,只有加速段。
    在这里插入图片描述

    3.1.2 匀速段

      当 L = 1 , v s = 1 , v c = 1 , v e = 1 , a c c = 123 ( 任 意 ) , d e c = 123 ( 任 意 ) , j e r k = 123 ( 任 意 ) L=1, v_s=1, v_c=1, v_e=1, acc =123(任意), dec=123(任意), jerk=123(任意) L=1,vs=1,vc=1,ve=1,acc=123(),dec=123(),jerk=123()时,只有匀速段。
    在这里插入图片描述

    3.1.3 减速段

      当 L = 1 , v s = 10 , v c = 10 , v e = 0 , a c c = 123 ( 任 意 ) , d e c = 93.75 , j e r k = 2000 L=1, v_s=10, v_c=10, v_e=0, acc =123(任意), dec=93.75,jerk=2000 L=1,vs=10,vc=10,ve=0,acc=123(),dec=93.75,jerk=2000时,只有减速段。
    在这里插入图片描述

    2.2 只有两段

    3.2.1 加速段+匀速段

      当 L = 1 , v s = 0 , v c = 10 , v e = 10 , a c c = 150 , d e c = 123 ( 任 意 ) , j e r k = 2000 L=1, v_s=0, v_c=10, v_e=10, acc =150, dec=123(任意),jerk=2000 L=1,vs=0,vc=10,ve=10,acc=150,dec=123(),jerk=2000时,只有加速段+匀速段 。
    在这里插入图片描述

    3.2.2 匀速段+减速段

      当 L = 1 , v s = 10 , v c = 10 , v e = 0 , a c c = 123 ( 任 意 ) , d e c = 150 , j e r k = 2000 L=1, v_s=10, v_c=10, v_e=0, acc =123(任意), dec=150,jerk=2000 L=1,vs=10,vc=10,ve=0,acc=123(),dec=150,jerk=2000时,只有匀速段 +减速段。
    在这里插入图片描述

    3.2.2 加速段+减速段

      当 L = 1 , v s = 1 , v c = 6 , v e = 2 , a c c = 20 , d e c = 30 , j e r k = 2000 L=1, v_s=1, v_c=6, v_e=2, acc =20,dec=30,jerk=2000 L=1,vs=1,vc=6,ve=2,acc=20,dec=30,jerk=2000时,只有加速段+减速段 。
    在这里插入图片描述

    3.3 有三段(加速段+匀速段 +减速段)

      当 L = 1 , v s = 1 , v c = 3 , v e = 2 , a c c = 40 , d e c = 30 , j e r k = 2000 L=1, v_s=1, v_c=3, v_e=2, acc =40 ,dec=30,jerk=2000 L=1,vs=1,vc=3,ve=2,acc=40,dec=30,jerk=2000时,有加速段+匀速段 +减速段。
    在这里插入图片描述

    4、7段S型速度规划

      (1) 位置平滑,速度平滑,加速度连续但不平滑,二次加速度跳变;
      (2) 速度、加速度、二次加速度可控,更高阶不可控;
      (3) 规划时间较短;
      (4) 算法稍复杂、计算量稍大。

    4.1 只有加速段

    4.1.1 无匀加速段

    L = 0.36742346 , v s = 0 , v c = 3 , v e = 3 , a c c = 40 , d e c = 123 ( 任 意 ) , j e r k = 200 L=0.36742346, v_s=0, v_c=3, v_e=3, acc =40 ,dec=123(任意),jerk=200 L=0.36742346,vs=0,vc=3,ve=3,acc=40,dec=123(),jerk=200
    在这里插入图片描述

    4.1.2 有匀加速段

    L = 0.24 , v s = 0 , v c = 3 , v e = 3 , a c c = 30 , d e c = 123 ( 任 意 ) , j e r k = 500 L=0.24, v_s=0, v_c=3, v_e=3, acc =30 ,dec=123(任意),jerk=500 L=0.24,vs=0,vc=3,ve=3,acc=30,dec=123(),jerk=500
    在这里插入图片描述

    4.2 只有匀速段

    L = 1 , v s = 1 , v c = 1 , v e = 1 , a c c = 123 ( 任 意 ) , d e c = 123 ( 任 意 ) , j e r k = 123 ( 任 意 ) L=1, v_s=1, v_c=1, v_e=1, acc =123(任意), dec=123(任意), jerk=123(任意) L=1,vs=1,vc=1,ve=1,acc=123(),dec=123(),jerk=123()
    在这里插入图片描述

    4.3 只有减速段

    4.3.1 无匀减速段

    L = 0.36742346 , v s = 3 , v c = 3 , v e = 0 , a c c = 123 ( 任 意 ) , d e c = 40 , j e r k = 200 L=0.36742346, v_s=3, v_c=3, v_e=0, acc =123(任意) ,dec=40,jerk=200 L=0.36742346,vs=3,vc=3,ve=0,acc=123(),dec=40,jerk=200
    在这里插入图片描述

    4.3.2 有匀减速段

    L = 0.24 , v s = 3 , v c = 3 , v e = 0 , a c c = 123 ( 任 意 ) , d e c = 30 , j e r k = 500 L=0.24, v_s=3, v_c=3, v_e=0, acc =123(任意) ,dec=30,jerk=500 L=0.24,vs=3,vc=3,ve=0,acc=123(),dec=30,jerk=500
    在这里插入图片描述

    4.4 只有加速段+匀速段

    4.4.1 无匀加速段

    L = 0.36742346 , v s = 0 , v c = 2 , v e = 2 , a c c = 40 , d e c = 123 ( 任 意 ) , j e r k = 200 L=0.36742346, v_s=0, v_c=2, v_e=2, acc =40 ,dec=123(任意),jerk=200 L=0.36742346,vs=0,vc=2,ve=2,acc=40,dec=123(),jerk=200
    在这里插入图片描述

    4.4.2 有匀加速段

    L = 0.24 , v s = 0 , v c = 3 , v e = 3 , a c c = 30 , d e c = 123 ( 任 意 ) , j e r k = 1000 L=0.24, v_s=0, v_c=3, v_e=3, acc =30 ,dec=123(任意),jerk=1000 L=0.24,vs=0,vc=3,ve=3,acc=30,dec=123(),jerk=1000
    在这里插入图片描述

    4.5 只有匀速段+减速段

    4.5.1 无匀减速段

    L = 0.36742346 , v s = 2 , v c = 2 , v e = 0 , a c c = 123 ( 任 意 ) , d e c = 40 , j e r k = 200 L=0.36742346, v_s=2, v_c=2, v_e=0, acc =123(任意) ,dec=40,jerk=200 L=0.36742346,vs=2,vc=2,ve=0,acc=123(),dec=40,jerk=200
    在这里插入图片描述

    4.5.2 有匀减速段

    L = 0.24 , v s = 3 , v c = 3 , v e = 0 , a c c = 123 ( 任 意 ) , d e c = 30 , j e r k = 1000 L=0.24, v_s=3, v_c=3, v_e=0, acc =123(任意) ,dec=30,jerk=1000 L=0.24,vs=3,vc=3,ve=0,acc=123(),dec=30,jerk=1000
    在这里插入图片描述

    4.6 只有加速段+减速段

    4.6.1 无匀加速段、无匀减速段

    L = 1 , v s = 1 , v c = 6 , v e = 2 , a c c = 60 , d e c = 80 , j e r k = 500 L=1, v_s=1, v_c=6, v_e=2, acc =60 ,dec=80,jerk=500 L=1,vs=1,vc=6,ve=2,acc=60,dec=80,jerk=500
    在这里插入图片描述

    4.6.2 有匀加速段、无匀减速段

    L = 1 , v s = 1 , v c = 6 , v e = 2 , a c c = 50 , d e c = 80 , j e r k = 800 L=1, v_s=1, v_c=6, v_e=2, acc =50 ,dec=80,jerk=800 L=1,vs=1,vc=6,ve=2,acc=50,dec=80,jerk=800
    在这里插入图片描述

    4.6.3 无匀加速段、有匀减速段

    L = 1 , v s = 2 , v c = 6 , v e = 1 , a c c = 80 , d e c = 60 , j e r k = 800 L=1, v_s=2, v_c=6, v_e=1, acc =80 ,dec=60,jerk=800 L=1,vs=2,vc=6,ve=1,acc=80,dec=60,jerk=800
    在这里插入图片描述

    4.6.4有匀加速段、有匀减速段

    L = 1 , v s = 2 , v c = 6 , v e = 1 , a c c = 40 , d e c = 50 , j e r k = 800 L=1, v_s=2, v_c=6, v_e=1, acc =40 ,dec=50,jerk=800 L=1,vs=2,vc=6,ve=1,acc=40,dec=50,jerk=800
    在这里插入图片描述

    4.7 加速段+匀速段+减速段

    4.7.1 无匀加速段、无匀减速段

    L = 1 , v s = 2 , v c = 4 , v e = 1 , a c c = 40 , d e c = 50 , j e r k = 400 L=1, v_s=2, v_c=4, v_e=1, acc =40 ,dec=50,jerk=400 L=1,vs=2,vc=4,ve=1,acc=40,dec=50,jerk=400
    在这里插入图片描述

    4.7.2 有匀加速段、无匀减速段

    L = 1 , v s = 2 , v c = 4 , v e = 1 , a c c = 20 , d e c = 40 , j e r k = 400 L=1, v_s=2, v_c=4, v_e=1, acc =20 ,dec=40,jerk=400 L=1,vs=2,vc=4,ve=1,acc=20,dec=40,jerk=400
    在这里插入图片描述

    4.7.3 无匀加速段、有匀减速段

    L = 1 , v s = 2 , v c = 4 , v e = 1 , a c c = 40 , d e c = 30 , j e r k = 400 L=1, v_s=2, v_c=4, v_e=1, acc =40 ,dec=30,jerk=400 L=1,vs=2,vc=4,ve=1,acc=40,dec=30,jerk=400
    在这里插入图片描述

    4.7.4有匀加速段、有匀减速段

    L = 1 , v s = 2 , v c = 4 , v e = 1 , a c c = 30 , d e c = 40 , j e r k = 800 L=1, v_s=2, v_c=4, v_e=1, acc =30 ,dec=40,jerk=800 L=1,vs=2,vc=4,ve=1,acc=30,dec=40,jerk=800
    在这里插入图片描述

    5、7段修正S型速度规划

      (1) 位置平滑,速度平滑,加速度连续但不够平滑,二次加速度跳变;
      (2) 速度、加速度、二次加速度可控,更高阶不可控;
      (3) 规划时间较短;
      (4) 算法稍复杂、计算量稍大。

    5.1 只有加速段

    5.1.1 无匀加速段

    L = 0.496729413289805 , v s = 0 , v c = 10 , v e = 10 , a c c = 200 , d e c = 123 ( 任 意 ) , j e r k = 5000 L=0.496729413289805, v_s=0, v_c=10, v_e=10, acc =200 ,dec=123(任意),jerk=5000 L=0.496729413289805,vs=0,vc=10,ve=10,acc=200,dec=123(),jerk=5000
    在这里插入图片描述

    5.1.2 有匀加速段

    L = 0.614159265358979 , v s = 0 , v c = 10 , v e = 10 , a c c = 100 , d e c = 123 ( 任 意 ) , j e r k = 5000 L=0.614159265358979, v_s=0, v_c=10, v_e=10, acc =100 ,dec=123(任意),jerk=5000 L=0.614159265358979,vs=0,vc=10,ve=10,acc=100,dec=123(),jerk=5000
    在这里插入图片描述

    5.2 只有匀速段

    L = 1 , v s = 1 , v c = 1 , v e = 1 , a c c = 123 ( 任 意 ) , d e c = 123 ( 任 意 ) , j e r k = 123 ( 任 意 ) L=1, v_s=1, v_c=1, v_e=1, acc =123(任意), dec=123(任意), jerk=123(任意) L=1,vs=1,vc=1,ve=1,acc=123(),dec=123(),jerk=123()
    在这里插入图片描述

    5.3 只有减速段

    5.3.1 无匀减速段

    L = 0.496729413289805 , v s = 10 , v c = 10 , v e = 0 , a c c = 123 ( 任 意 ) , d e c = 200 , j e r k = 5000 L=0.496729413289805, v_s=10, v_c=10, v_e=0, acc =123(任意),dec=200,jerk=5000 L=0.496729413289805,vs=10,vc=10,ve=0,acc=123(),dec=200,jerk=5000
    在这里插入图片描述

    5.3.2 有匀减速段

    L = 0.614159265358979 , v s = 10 , v c = 10 , v e = 0 , a c c = 123 ( 任 意 ) , d e c = 100 , j e r k = 5000 L=0.614159265358979, v_s=10, v_c=10, v_e=0, acc =123(任意) ,dec=100,jerk=5000 L=0.614159265358979,vs=10,vc=10,ve=0,acc=123(),dec=100,jerk=5000
    在这里插入图片描述

    5.4 只有加速段+匀速段

    5.4.1 无匀加速段

    L = 0.61415926 , v s = 0 , v c = 10 , v e = 10 , a c c = 300 , d e c = 123 ( 任 意 ) , j e r k = 5000 L=0.61415926, v_s=0, v_c=10, v_e=10, acc =300 ,dec=123(任意),jerk=5000 L=0.61415926,vs=0,vc=10,ve=10,acc=300,dec=123(),jerk=5000
    在这里插入图片描述

    5.4.2 有匀加速段

    L = 0.61415926 , v s = 0 , v c = 10 , v e = 10 , a c c = 150 , d e c = 123 ( 任 意 ) , j e r k = 8000 L=0.61415926, v_s=0, v_c=10, v_e=10, acc =150 ,dec=123(任意),jerk=8000 L=0.61415926,vs=0,vc=10,ve=10,acc=150,dec=123(),jerk=8000
    在这里插入图片描述

    5.5 只有匀速段+减速段

    5.5.1 无匀减速段

    L = 0.61415926 , v s = 10 , v c = 10 , v e = 0 , a c c = 123 ( 任 意 ) , d e c = 300 , j e r k = 5000 L=0.61415926, v_s=10, v_c=10, v_e=0, acc =123(任意) ,dec=300,jerk=5000 L=0.61415926,vs=10,vc=10,ve=0,acc=123(),dec=300,jerk=5000
    在这里插入图片描述

    5.5.2 有匀减速段

    L = 0.61415926 , v s = 10 , v c = 10 , v e = 0 , a c c = 123 ( 任 意 ) , d e c = 150 , j e r k = 8000 L=0.61415926, v_s=10, v_c=10, v_e=0, acc =123(任意) ,dec=150,jerk=8000 L=0.61415926,vs=10,vc=10,ve=0,acc=123(),dec=150,jerk=8000
    在这里插入图片描述

    5.6 只有加速段+减速段

    5.6.1 无匀加速段、无匀减速段

    L = 1 , v s = 1 , v c = 6 , v e = 2 , a c c = 60 , d e c = 80 , j e r k = 500 L=1, v_s=1, v_c=6, v_e=2, acc =60 ,dec=80,jerk=500 L=1,vs=1,vc=6,ve=2,acc=60,dec=80,jerk=500
    在这里插入图片描述

    5.6.2 有匀加速段、无匀减速段

    L = 1 , v s = 1 , v c = 6 , v e = 2 , a c c = 40 , d e c = 80 , j e r k = 1200 L=1, v_s=1, v_c=6, v_e=2, acc =40 ,dec=80,jerk=1200 L=1,vs=1,vc=6,ve=2,acc=40,dec=80,jerk=1200
    在这里插入图片描述

    5.6.3 无匀加速段、有匀减速段

    L = 1 , v s = 2 , v c = 6 , v e = 1 , a c c = 80 , d e c = 40 , j e r k = 1200 L=1, v_s=2, v_c=6, v_e=1, acc =80 ,dec=40,jerk=1200 L=1,vs=2,vc=6,ve=1,acc=80,dec=40,jerk=1200
    在这里插入图片描述

    5.6.4有匀加速段、有匀减速段

    L = 1 , v s = 2 , v c = 6 , v e = 1 , a c c = 40 , d e c = 30 , j e r k = 1200 L=1, v_s=2, v_c=6, v_e=1, acc =40 ,dec=30,jerk=1200 L=1,vs=2,vc=6,ve=1,acc=40,dec=30,jerk=1200
    在这里插入图片描述

    5.7 加速段+匀速段+减速段

    5.7.1 无匀加速段、无匀减速段

    L = 1 , v s = 2 , v c = 3 , v e = 1 , a c c = 40 , d e c = 50 , j e r k = 400 L=1, v_s=2, v_c=3, v_e=1, acc =40 ,dec=50,jerk=400 L=1,vs=2,vc=3,ve=1,acc=40,dec=50,jerk=400
    在这里插入图片描述

    5.7.2 有匀加速段、无匀减速段

    L = 1 , v s = 2 , v c = 3 , v e = 1 , a c c = 20 , d e c = 40 , j e r k = 1000 L=1, v_s=2, v_c=3, v_e=1, acc =20 ,dec=40,jerk=1000 L=1,vs=2,vc=3,ve=1,acc=20,dec=40,jerk=1000
    在这里插入图片描述

    5.7.3 无匀加速段、有匀减速段

    L = 1 , v s = 2 , v c = 3 , v e = 1 , a c c = 40 , d e c = 20 , j e r k = 1000 L=1, v_s=2, v_c=3, v_e=1, acc =40 ,dec=20,jerk=1000 L=1,vs=2,vc=3,ve=1,acc=40,dec=20,jerk=1000
    在这里插入图片描述

    5.7.4有匀加速段、有匀减速段

    L = 1 , v s = 2 , v c = 4 , v e = 1 , a c c = 30 , d e c = 40 , j e r k = 3000 L=1, v_s=2, v_c=4, v_e=1, acc =30 ,dec=40,jerk=3000 L=1,vs=2,vc=4,ve=1,acc=30,dec=40,jerk=3000
    在这里插入图片描述

    6、15段S型速度规划

    在这里插入图片描述

      (1) 位置平滑,速度平滑,加速度平滑,二次加速度连续但不平滑,三次加速度跳变;
      (2) 速度、加速度、二次加速度、三次加速度可控,四次加速度不可控;
      (3) 规划时间稍长;
      (4) 算法较复杂、计算量较大。

    7、31段S型速度规划

    在这里插入图片描述

      (1) 位置平滑,速度平滑,加速度平滑,二次加速度平滑,三次加速度连续但不平滑,四次加速度跳变;
      (2) 位置的各阶导皆可控;
      (3) 规划时间稍长;
      (4) 算法较复杂、计算量较大。

       总体来说:
      连续性与平滑性:31段S型 > > > 多项式 ≈ \approx 15段S型 > > > 7段修正S型 > > > 余弦 > > > 7段S型 > > > 梯形;
      可控性:31段S型 > > > 15段S型 > > > 7段修正S型 = = = 7段S型 = = = 多项式 > > > 余弦 = = = 梯形;
      规划总时间:多项式 > > > 余弦 > > > 31段S型 > > > 15段S型 > > > 7段修正S型 > > > 7段S型 > > > 梯形;
      算法计算量:31段S型 > > > 15段S型 > > > 7段修正S型 > > > 7段S型 > > > 多项式 ≈ \approx 余弦 > > > 梯形。
      根据以上的对比,7种速度规划算法大致可分为低中高端三档。
      低档:梯形、余弦(普通学者,自身“阶数”较低,专业技能浮于表面,自律能力较差)
      中档:多项式、7段S型、7段修正S型(专家,自身“阶数”较高,专业技能深入,需要深入了解才能知道其学术造诣之深,自律能力强)
      高档:15段S型、31段S型(大师,自身“阶数”高,专业技能出神入化,需要一定造诣的人深入洞悉才能领悟其所思所为之精妙,自律能力非常人所能及)

    二、7种速度规划示例演示

      笔者采用统一的框架实现了以上7种速度规划算法,所有核心算法均用c语言实现,不调用第三方算法库,采用QT实现图形用户界面。下载链接:robot_velocity_planning _V2.0.rar。下面是示例演示,取长度 L = 300 L=300 L=300,起始速度 v s = 100 v_s = 100 vs=100,结束速度 v e = 200 v_e = 200 ve=200,最大速度 v c = 500 v_c = 500 vc=500,加速度 a c c = 2000 acc = 2000 acc=2000,减速度 d e c = 3000 dec = 3000 dec=3000,二次加速度 j e r k = 50000 jerk = 50000 jerk=50000,三次加速度 s n a p = 3000000 snap = 3000000 snap=3000000,四次加速度 c r a c k l e = 500000000 crackle = 500000000 crackle=500000000
    在这里插入图片描述

    展开全文
  • 1 引言  现在,人们对自己的身体健康状况十分关注....体温是人体的重要生理参数...日本欧姆龙研制的几款非接触式红外体温计和BJ40非接触式医用红外线体温计(精度为±tO.2℃,其主要器件是红外温度传感器。本文给
  • S型曲线主函数

    千次阅读 2019-03-12 14:01:03
    S型曲线主函数 目录 S型曲线主函数 S1段曲线 S2段曲线 S3段曲线末端速度倒推型 S3段曲线末端速度正推型 S4段曲线 S5段曲线 S6段曲线 S7段曲线倒推型 S7段曲线正推型 clc;clear;close; GoalV=100;%%单位...

     

    S型曲线主函数

    目录

    S型曲线主函数

    S1段曲线

    S2段曲线

    S3段曲线末端速度倒推型

    S3段曲线末端速度正推型

    S4段曲线

    S5段曲线

    S6段曲线

    S7段曲线倒推型

    S7段曲线正推型


    clc;clear;close;

    GoalV=100;%%单位mm/s
    GoalS=100;%%单位mm
    Acc=400;
    Jerk=30;
    %%%%%%%%%%%%%%%%可以修改
    InitV=0;
    InitS=0;
    %%%%%%%%%%%%%%%%不可以修改
    Vs2=InitV;
    n=1000;
    T1=Acc/Jerk;
    Vtemp=Acc^2/Jerk;%%这个速度变化量包括了加加速度段和加减速度段的速度变化

    Vmax=abs(GoalV-InitV);

    Vs=InitV;%%所以不用担心速度跟不上的情况,只要考虑好如何优化分配位移的情况就行了。

    J=Jerk;
    %T1=sqrt((((GoalV+Vs)/2-Vs)*2)/J);

    [s(1),v(1),a(1)]=SProfileT1(0,Jerk,InitV,T1);
    T3=T1;
    t=0;
    J=-Jerk;
    Vc=2*(v(1)-InitV)+InitV;

    s(3)=0+(double(1)/6)*J*T3.^3-(double(1)/2)*J*T3.^2*t+(double(1)/2)*J*T3*t.^2-(double(1)/6)*J*t.^3+Vc*T3-Vc*t;

    J=Jerk;

    s(7)=0+(double(1)/6)*J*T3.^3-(double(1)/2)*J*T3.^2*t+(double(1)/2)*J*T3*t.^2-(double(1)/6)*J*t.^3+Vs*T3-Vs*t;%%这部分只是计算相对T7段的位移变化量
    J=-Jerk;
    s(5)=0+Vc*T1+(double(1)/6)*J*T1.^3+(double(1)/2)*0*T1.^2;

    S(4)=s(3)+s(5)+s(1)+s(7);%%这是四段曲线位移的分界点
    J=Jerk;
    S(4)=4*InitV*T1+2*J*T1.^3;%%这个没加初始的位移变化量,所以前面的计算的s(1)、s(3)、s(5)、s(7)都没有加初始的位移
        
    if Vmax<=Vtemp%%速度最大变化量小于加加速度段和减减速度段的速度变化量,此时速度曲线分为四段或五段,此时看位移的情况当前的情况下速度都是可以跟的上的
        
        if S(4)<abs(GoalS-InitS) %&& InitV==Vs2  %%就用五段曲线 else是用四段曲线
              
                t1=linspace(0,T1,n);
                t=T1;
                [St{1},Vt{1},At{1}]=SProfileT1(InitS,Jerk,InitV,t1);

              
                t3=linspace(t,2*t,n);
                [St{3},Vt{3},At{3}]=SProfileT3(T1,-Jerk,GoalV,t3-t,S(4)/2+InitS,0);
                
                %[St{3},Vt{3},At{3}]=SProfileT32(At{1}(n),-Jerk,Vt{1}(n),t3-t,St{1}(n));
                t=2*t;
              
                T4=abs(S(4)-abs(GoalS-InitS))/GoalV;
                t4=linspace(t,t+T4,n);
                [St{4},Vt{4},At{4}]=SProfileT4(St{3}(n),GoalV,t4-t);
                t=t+T4;
               
                t5=linspace(t,t+T1,n);
                [St{5},Vt{5},At{5}]=SProfileT5(-Jerk,GoalV,t5-t,St{4}(n),At{4}(n));
                t=t+T1;
               
                t7=linspace(t,t+T1,n);
                [St{7},Vt{7},At{7}]=SProfileT7(T1,Jerk,InitV,t7-t,GoalS );
                
                %[St{7},Vt{7},At{7}]= SProfileT72(At{5}(n),J,Vt{5}(n),t7-t,St{5}(n));
                t=t+T1;
               
                
                figure('name','五段位置曲线');
                hold on
                plot(t1,St{1});
                plot(t3,St{3});
                plot(t4,St{4});
                plot(t5,St{5},'y');
                plot(t7,St{7},'b');
                figure('name','五段速度曲线');
                hold on
                plot(t1,Vt{1});
                plot(t3,Vt{3});
                plot(t4,Vt{4},'r');
                plot(t5,Vt{5},'y');
                plot(t7,Vt{7},'b');
                
                figure('name','五段加速度曲线');
                hold on
                plot(t1,At{1});
                plot(t3,At{3});
                plot(t4,At{4});
                plot(t5,At{5},'y');
                plot(t7,At{7},'b');
        elseif S(4)>=abs(GoalS-InitS)
               
                sc=abs(GoalS-InitS);
                J=Jerk;
                fun=@(T)(sc-4*InitV*T-2*J*T.^3);
                T=fsolve(fun,0.1);
                
                Vs=InitV;
                J=Jerk;

                t1=linspace(0,T,n);
                t=T;
                [St{1},Vt{1},At{1}]=SProfileT1(InitS,Jerk,Vs,t1);
                
             
                J=-Jerk;
                
                %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
               
                sm=0
                vm=Vt{1}(n);
                am=At{1}(n);

                t3=linspace(t,t+T,n);
                [St{3},Vt{3},At{3}]=SProfileT32(am,J,vm,t3-t,St{1}(n));
                t=T+T;
               
                
                J=-Jerk;
                Vc=Vt{3}(n);
                a4=At{3}(n);
                s4=St{3}(n);

                t5=linspace(t,t+T,n);
                [St{5},Vt{5},At{5}]=SProfileT5(J,Vc,t5-t,s4,a4);
                t=t+T;

                J=Jerk;

                t7=linspace(t,t+T,n);
                [St{7},Vt{7},At{7}]= SProfileT72(At{5}(n),J,Vt{5}(n),t7-t,St{5}(n));
                t=t+T;
                figure('name','四段位置曲线');
                hold on
                plot(t1,St{1});
                plot(t3,St{3});
                plot(t5,St{5},'y');
                plot(t7,St{7},'b');
                figure('name','四段速度曲线');
                hold on
                plot(t1,Vt{1});
                plot(t3,Vt{3});
                plot(t5,Vt{5},'y');
                plot(t7,Vt{7},'b');
                
                figure('name','四段加速度曲线');
                hold on
                plot(t1,At{1});
                plot(t3,At{3});
                plot(t5,At{5},'y');
                plot(t7,At{7},'b');
        end
    else
                     if S(4)<abs(GoalS-InitS) %%可分为六段和七段大于四段的位移量了
                        %%先计算六段的位移总量,采用的时间间隔都是一样
                            t=T1;
                            J=Jerk;
                            s(1)=0+(double(1)/6)*J*t.^3+Vs*t;
                            [s(1),v(1),a(1)]=SProfileT1(0,J,InitV,T1);
                            [s(2),v(2),a(2)]=SProfileT2(T1,J,v(1),T1,0);
                            J=-Jerk;
                            [s(3),v(3),a(3)]=SProfileT32(a(2),J,v(2),T1,0);
                            [s(5),v(5),a(5)]=SProfileT5(J,v(3),T1,0,0);
                            [s(6),v(6),a(6)]=SProfileT6( T1,J,v(5),T1,0);
                            J=Jerk;
                            [s(7),v(7),a(7)]=SProfileT72(a(6),J,v(6),T1,0);
                            S(6)=s(1)+s(2)+s(3)+s(5)+s(6)+s(7);
                            if S(6)<=abs(GoalS-InitS);%%大于六段的时候的位移

                                        J=Jerk;
                                        t1=linspace(0,T1,n);
                                        [St{1},Vt{1},At{1}]=SProfileT1(InitS,J,InitV,t1);
                                        t=T1;

                                        t2=linspace(t,t+T1,n);
                                        [St{2},Vt{2},At{2}]=SProfileT2(T1,J,Vt{1}(n),t2-t,St{1}(n));
                                        t=t+T1;

                                        J=-Jerk;
                                        t3=linspace(t,t+T1,n);
                                        [St{3},Vt{3},At{3}]=SProfileT32(At{2}(n),J,Vt{2}(n),t3-t,St{2}(n));
                                        t=t+T1;

                                        T4=abs(S(6)-abs(GoalS-InitS))/Vt{3}(n);
                                        t4=linspace(t,t+T4,n);
                                        [St{4},Vt{4},At{4}]=SProfileT4(St{3}(n),Vt{3}(n),t4-t);
                                        t=t+T4;


                                        t5=linspace(t,t+T1,n);
                                        [St{5},Vt{5},At{5}]=SProfileT5(J,Vt{4}(n),t5-t,St{4}(n),0);
                                        t=t+T1;

                                        t6=linspace(t,t+T1,n);
                                        [St{6},Vt{6},At{6}]=SProfileT6( T1,J,Vt{5}(n),t6-t,St{5}(n));
                                        t=t+T1;

                                        t7=linspace(t,t+T1,n);
                                        J=Jerk;
                                        [St{7},Vt{7},At{7}]=SProfileT72(At{6}(n),J,Vt{6}(n),t7-t,St{6}(n));

                                        t=t+T1;
                                    figure('name','七段位置曲线');
                                    hold on
                                    plot(t1,St{1});
                                    plot(t2,St{2});
                                    plot(t3,St{3});
                                    plot(t4,St{4});
                                    plot(t5,St{5},'y');
                                    plot(t6,St{6},'r');
                                    plot(t7,St{7},'b');
                                    figure('name','七段速度曲线');
                                    hold on
                                    plot(t1,Vt{1});
                                    plot(t2,Vt{2});
                                    plot(t3,Vt{3});
                                    plot(t4,Vt{4});
                                    plot(t5,Vt{5},'y');
                                    plot(t6,Vt{6},'r');
                                    plot(t7,Vt{7},'b');

                                    figure('name','七段加速度曲线');
                                    hold on
                                    plot(t1,At{1});
                                    plot(t2,At{2});
                                    plot(t3,At{3});
                                    plot(t4,At{4});
                                    plot(t5,At{5},'y');
                                    plot(t6,At{6});
                                    plot(t7,At{7},'b');
                         else 
                                    
                                    st=abs(GoalS-InitS)/2;
                                    
                                    T1=Acc/Jerk;
                                    J=Jerk;
                                    t1=linspace(0,T1,n);
                                    [St{1},Vt{1},At{1}]=SProfileT1(InitS,J,InitV,t1);
                                    t=T1;
                                    
                                    
                                    
                                    fun=@(tp)(-st+(double(1)/6)*J*T1.^3+InitV*T1+0+Vt{1}(n)*tp+(double(1)/2)*J*T1*tp.^2+(double(1)/6)*(-1*J)*T1.^3+(double(1)/2)*(J*T1+0*tp)*T1.^2+(Vt{1}(n)+J*T1*tp)*T1);
                                    T2=fsolve(fun,0.1);
                                    
                                    
                                    t2=linspace(t,t+T2,n);
                                    [St{2},Vt{2},At{2}]=SProfileT2(T1,J,Vt{1}(n),t2-t,St{1}(n));
                                    t=t+T2;
                                    
                                     J=-Jerk;
                                    t3=linspace(t,t+T1,n);
                                    [St{3},Vt{3},At{3}]=SProfileT32(At{2}(n),J,Vt{2}(n),t3-t,St{2}(n));
                                    t=t+T1;
                                    
                                    t5=linspace(t,t+T1,n);
                                    [St{5},Vt{5},At{5}]=SProfileT5(J,Vt{3}(n),t5-t,St{3}(n),0);
                                    t=t+T1;
                                    
                                    t6=linspace(t,t+T2,n);
                                    [St{6},Vt{6},At{6}]=SProfileT6( T1,J,Vt{5}(n),t6-t,St{5}(n));
                                    t=t+T2;
                                    
                                    t7=linspace(t,t+T1,n);
                                    J=Jerk;
                                    [St{7},Vt{7},At{7}]=SProfileT72(At{6}(n),J,Vt{6}(n),t7-t,St{6}(n));

                                    t=t+T1;
                                    
                                    figure('name','六段位置曲线');
                                    hold on
                                    plot(t1,St{1});
                                    plot(t2,St{2});
                                    plot(t3,St{3});
                                   
                                    plot(t5,St{5},'y');
                                    plot(t6,St{6},'r');
                                    plot(t7,St{7},'b');
                                    figure('name','六段速度曲线');
                                    hold on
                                    plot(t1,Vt{1});
                                    plot(t2,Vt{2});
                                    plot(t3,Vt{3});
                                   
                                    plot(t5,Vt{5},'y');
                                    plot(t6,Vt{6},'r');
                                    plot(t7,Vt{7},'b');

                                    figure('name','六段加速度曲线');
                                    hold on
                                    plot(t1,At{1});
                                    plot(t2,At{2});
                                    plot(t3,At{3});
                                   
                                    plot(t5,At{5},'y');
                                    plot(t6,At{6});
                                    plot(t7,At{7},'b');
                            end
                     else
                        %%%%%%%%%%%%%%%%%%%%四段曲线    
                        sc=abs(GoalS-InitS);
                        J=Jerk;
                        fun=@(T)(sc-4*InitV*T-2*J*T.^3);
                        T=fsolve(fun,0.1);

                        Vs=InitV;
                        J=Jerk;

                        t1=linspace(0,T,n);
                        t=T;
                        [St{1},Vt{1},At{1}]=SProfileT1(InitS,Jerk,Vs,t1);


                        J=-Jerk;

                        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

                        sm=0
                        vm=Vt{1}(n);
                        am=At{1}(n);

                        t3=linspace(t,t+T,n);
                        [St{3},Vt{3},At{3}]=SProfileT32(am,J,vm,t3-t,St{1}(n));
                        t=T+T;


                        J=-Jerk;
                        Vc=Vt{3}(n);
                        a4=At{3}(n);
                        s4=St{3}(n);

                        t5=linspace(t,t+T,n);
                        [St{5},Vt{5},At{5}]=SProfileT5(J,Vc,t5-t,s4,a4);
                        t=t+T;

                        J=Jerk;

                        t7=linspace(t,t+T,n);
                        [St{7},Vt{7},At{7}]= SProfileT72(At{5}(n),J,Vt{5}(n),t7-t,St{5}(n));
                        t=t+T;
                        figure('name','四段位置曲线');
                        hold on
                        plot(t1,St{1});
                        plot(t3,St{3});
                        plot(t5,St{5},'y');
                        plot(t7,St{7},'b');
                        figure('name','四段速度曲线');
                        hold on
                        plot(t1,Vt{1});
                        plot(t3,Vt{3});
                        plot(t5,Vt{5},'y');
                        plot(t7,Vt{7},'b');

                        figure('name','四段加速度曲线');
                        hold on
                        plot(t1,At{1});
                        plot(t3,At{3});
                        plot(t5,At{5},'y');
                        plot(t7,At{7},'b');
                end
    end

    S1段曲线

    function [ s,v,a ] = SProfileT1(S0,J,Vs,t)
    %UNTITLED2 Summary of this function goes here
    %   Detailed explanation goes here
    s=S0+(double(1)/6)*J*t.^3+Vs*t;
    v=(double(1)/2)*J*t.^2+Vs;
    a=J*t;
    end

    S2段曲线

    function [ s,v,a] = SProfileT2(T1,J,Vl,t,S0)%%备注一下这里的T2时间必须等于T1时间
    %UNTITLED3 Summary of this function goes here
    %   Detailed explanation goes here
    s=S0+Vl*t+(double(1)/2)*J*T1*t.^2;
    v=Vl+J*T1*t;
    a=J*T1+0*t;
    end

    S3段曲线末端速度倒推型

    function [ s,v,a] = SProfileT3( T3,J,Vc,t,St3,a3)
    %UNTITLED Summary of this function goes here
    %   Detailed explanation goes here
    v=Vc+(double(1)/2)*J*(T3-t).^2-a3*(T3-t);%%因为现在根据时间去求速度Vm,而速度V2的速度求解是按绝对时间来的,即可以按It的间隔来此时时间的总长就是T3即是It
    s=St3-(double(1)/6)*J*(T3-t).^3+(double(1)/2)*a3*(T3-t).^2-Vc*(T3-t);
    a=a3-J*(T3-t);
    end

    S3段曲线末端速度正推型

    function [ s,v,a ] = SProfileT32( am,J,vm,t,sm)
    %UNTITLED Summary of this function goes here
    %   Detailed explanation goes here
    a=am+J*t;
    v=vm+am*t+(double(1)/2)*J*t.^2;
    s=sm+(double(1)/6)*J*t.^3+(double(1)/2)*am*t.^2+vm*t;
    end

    S4段曲线

    function [ s,v,a ] = SProfileT4(S0,Vc,t)
    %UNTITLED Summary of this function goes here
    %   Detailed explanation goes here
    a=t*0;
    v=Vc+0*t;
    s=Vc*t+S0;
    end

    S5段曲线

    function [ s,v,a ] = SProfileT5(J,Vc,t,s4,a4)
    %UNTITLED2 Summary of this function goes here
    %   Detailed explanation goes here
    v=Vc+(double(1)/2)*J*t.^2+a4*t;
    s=s4+Vc*t+(double(1)/6)*J*t.^3+(double(1)/2)*a4*t.^2;
    a=a4+J*t;
    end

    S6段曲线

    function [ s,v,a] = SProfileT6( T5,J,Vm2,t,S5)
    %UNTITLED Summary of this function goes here
    %   Detailed explanation goes here
    v=Vm2+J*T5*t;
    s=S5+Vm2*t+(double(1)/2)*J*T5*t.^2;
    a=J*T5+0*t;
    end

    S7段曲线倒推型

    function [ s,v,a] = SProfileT7( T7,J,Vs2,t,St7 )
    %UNTITLED2 Summary of this function goes here
    %   Detailed explanation goes here
    v=Vs2+(double(1)/2)*J*(T7-t).^2;
    s=St7-(double(1)/6)*J*T7.^3+(double(1)/2)*J*T7.^2*t-(double(1)/2)*J*T7*t.^2+(double(1)/6)*J*t.^3-Vs2*T7+Vs2*t;
    a=-J*(T7-t);
    end

    S7段曲线正推型

    function [ s,v,a ] = SProfileT72( al2,J,vl2,t,sl2)
    %UNTITLED Summary of this function goes here
    %   Detailed explanation goes here
    a=al2+J*t;
    v=vl2+al2*t+(double(1)/2)*J*t.^2;
    s=sl2+(double(1)/6)*J*t.^3+(double(1)/2)*al2*t.^2+vl2*t;
    end

    展开全文
  • 基于S型曲线的连续多段曲线插补平滑过渡的规划算法(Matlab) 写在前面 前面的博客已经写了关于空间直线与空间圆弧的常用插补算法,而这些都是单一路径,实际中并不实用。对于连续多段路径,传统方法是将多段路径...

    写在前面

    学习代码都记录在个人github上,欢迎关注~

    前面的博客已经写了关于空间直线与空间圆弧的常用插补算法,而这些都是单一路径,实际中并不实用。对于连续多段路径,传统方法是将多段路径细分,然后对每段路径采用首尾速度为0的加减速算法(S型曲线或梯形曲线),这就带来了频繁启停的问题,容易对机械臂造成冲击,同时运行时间较长。

    下面我把前面博客中提到的非对称S型加减速算法与空间中多段路径相结合,实现多段路径平滑过渡,减少运行时间。简单画一个轮廓状的“S”字符:

    本文方法:

    在这里插入图片描述在这里插入图片描述

    速度与加速度曲线:

    在这里插入图片描述

    采用分段路径首尾速度为0的加减速控制方法:

    在这里插入图片描述

    速度与加速度曲线:

    在这里插入图片描述

    对比结果还是很明显滴~本文的方法运行时间更短了,没有频繁启停,轮廓曲线更圆滑。如果在加上动力学方面的约束,就更好了。这个以后再讨论。

    预处理

    “预处理”,顾名思义就是拿到了一个轮廓轨迹,先想想这个轨迹如何分段,分段后如何衔接。

    一般工业机器人任务空间的运动轨迹都是由圆弧段和直线段组成的,对于多段连续直线路径而言,任意两段相连直线段的连接处需要平滑处理,这里选择建立圆弧模型进行路径拐角过渡。下面给出圆弧模型的建立过程:

    在这里插入图片描述

    P i P_{i} Pi, v i , v_{i}, vi, r i r_{i} ri分别为用户设定的拾放路径目标点、速度和过渡半径; P T i P_{Ti} PTi, v T i v_{Ti} vTi, C i C_{i} Ci分别为圆弧转接点、转接点的速度和圆心; M i M_{i} Mi为两圆弧转接点的中点; θ i \theta_{i} θi, l i l_{i} li 分别为拐角大小和角平分线; d i d_{i} di为分割后的小路径段长度。

    针对由 P 0 P_{0} P0, P 1 P_{1} P1, P 2 P_{2} P2构成的路径段转接模型,设计求解步骤为:

    在这里插入图片描述

    在这里插入图片描述

    Matlab函数如下:

    % 拾取放置操作分段路径转接模型
    % 参数: 用户设定的三个拾放路径点P0,P1,P2的位置
    %       过渡半径r
    %       最大加速度Amax
    %       插补周期t
    % 返回值: 转接点Pt1,Pt2,分割后的小路径长度d1,d2,圆心C,圆心角theta
    %         
    function [Pt1, Pt2, d1, d2, C, theta] = PathSegmentTrans(P0, P1, P2, r, t)
    % 求拐角theta
    P1P0 = sqrt(power(P0(1) - P1(1), 2) + power(P0(2) - P1(2), 2) + power(P0(3) - P1(3), 2));
    P1P2 = sqrt(power(P2(1) - P1(1), 2) + power(P2(2) - P1(2), 2) + power(P2(3) - P1(3), 2));
    vec_P1P0 = [P0(1) - P1(1), P0(2) - P1(2), P0(3) - P1(3)];
    vec_P1P2 = [P2(1) - P1(1), P2(2) - P1(2), P2(3) - P1(3)];
    theta = acos((dot(vec_P1P0, vec_P1P2)) / (P1P0*P1P2));
    % 求转接点Pt1、Pt2
    vec_P1Pt1 = (r/tan(theta/2)/P1P0) * vec_P1P0;
    vec_P1Pt2 = (r/tan(theta/2)/P1P2) * vec_P1P2;
    Pt1 = P1 + vec_P1Pt1;
    Pt2 = P1 + vec_P1Pt2;
    % 求路径长度d1、弧长d2
    d1 = sqrt(power(Pt1(1) - P0(1), 2) + power(Pt1(2) - P0(2), 2) + power(Pt1(3) - P0(3), 2));
    d2 = (pi - theta) * r;
    % % 求转接速度vt
    % 这是考虑机械系统动力学匀速因素得到的转接速度
    % a = sqrt(Amax * r);
    % b = d2 / t;
    % if (a > b)
    %     vt  = b;
    % else
    %     vt = a;
    % end
    % 求圆心C
    vec_Pt1M = (1/2) * (Pt2 - Pt1);
    M = Pt1 + vec_Pt1M;
    vec_P1M = M - P1;
    P1M = sqrt(power(M(1) - P1(1), 2) + power(M(2) - P1(2), 2) + power(M(3) - P1(3), 2));
    P1C = r / sin(theta/2);
    vec_P1C = (P1C / P1M) * vec_P1M;
    C = P1 + vec_P1C;
    end
    

    基于非对称S曲线的轨迹规划

    核心方法:机器人学回炉重造(6):关节空间规划方法——梯形加减速(与抛物线拟合的线性函数)、S型曲线规划

    一些文献中给S型曲线加了很多修饰词,比如非对称、带约束啥的,其实就是我上面那篇博客的S曲线,首尾速度可以不一样,带有位移、速度以及加速度约束。

    下面这幅图中, T 1 T_{1} T1 ~ T 3 T_{3} T3对应路径段 P 0 P T 1 P_{0}P_{T1} P0PT1 ,分为加加速段、匀加速段、减加速段和匀速段; T 4 T 5 T_{4}T_{5} T4T5 对应过渡圆弧 P T 1 P T 2 P_{T1}P_{T2} PT1PT2,为匀速运动; T 6 T_{6} T6 ~ T 9 T_{9} T9对应路径段 P T 2 P T 3 P_{T2}P_{T3} PT2PT3,分为减加速段、匀减速段、减减速段和匀速段。把下面这幅图和上面那篇博客中的S曲线对照着看,容易理解直线段 d i d_{i} di(i为奇数,1,3,5,……)均为加速段或者减速段,圆弧段 d i d_{i} di(i为偶数,2,4,6,……)均为匀速段,并且对于直线段而言,其最大速度为圆弧连接点的速度 v T i ( v T i = v T i + 1 ) v_{Ti}(v_{Ti}=v_{Ti+1}) vTi(vTi=vTi+1)

    在这里插入图片描述

    对每段 P i P i + 1 P i + 2 P_{i}P_{i+1}P_{i+2} PiPi+1Pi+2进行S曲线加减速规划,得到转接点速度,也就是规划得到的曲线实际能够达到的最大速度vlim,Matlab函数如下:

    % 获取转接运动参数(速度)
    % 参数: 分段路径长度d1,d2,d3
    %       P0、P1、P2的速度v0, v1, v2(P1为中间点)
    %       最大加速度amax,最大加加速度jmax
    % 返回值: 转接点速度vt(即为路径上能够达到的最大速度)
    function [vt] = TransMotionPara(d1, d2, d3, v0, v1, v2, amax, jmax)
    % 得到规划参数Ta, Tv, Td, Tj1, Tj2, q0, q1, v0, v1, vlim, amax, amin, alima, alimd, jmax, jmin
    q0 = 0; q1 = d1 + d2 + d3
    para = STrajectoryPara(q0, q1, v0, v2, v1, amax, jmax);
    vt = para(10);
    end
    

    插补

    这里先只讲位置插补

    预处理过后,轨迹就分段成为了一大堆直线段和一大堆圆弧段,这里要做的就是对这些空间直线和空间圆弧进行插补,于是又回到老话题了……

    这里直线段插补不多说,和前面博客中提到的一样,空间单一直线位置插补+ S型加减速曲线归一化处理,具体方法见基于带约束S型加减速曲线的空间直线插补与空间圆弧插补算法(Matlab)

    圆弧段插补也大同小异,方法有所不同,但基本是采用局部坐标系的空间圆弧插补方法和齐次坐标变换原理。新建立的局部坐标系UVW如下图所示,下面图中点的下标和我上面写的有些不一样,忽略所有 P b i P_{bi} Pbi并将 P e i P_{ei} Pei看成 P T i P_{Ti} PTi就行了,我就没有重新画图了。以第一段轨迹为例,新坐标系建立过程如下:

    P T 1 P_{T1} PT1为原点, P 0 P 1 → \overrightarrow{P_{0}P_{1}} P0P1 为V轴, P 2 P 1 → × P 1 P 0 → \overrightarrow{{P_{2} P_{1}}} \times \overrightarrow{P_{1} P_{0}} P2P1 ×P1P0 为W,

    V轴单位向量为: V = P 0 P 1 → ∣ P 0 P 1 → ∣ V=\frac{\overrightarrow{P_{0} P_{1}}}{\left|\overrightarrow{P_{0} P_{1}}\right|} V=P0P1 P0P1

    W轴单位向量为: W = P 2 P 1 → × P 1 P 0 → ∣ P 2 P 1 → × P 1 P 0 → ∣ W=\frac{\overrightarrow{{P_{2} P_{1}}} \times \overrightarrow{P_{1} P_{0}}}{\left|\overrightarrow{{P_{2} P_{1}}} \times \overrightarrow{P_{1} P_{0}}\right|} W=P2P1 ×P1P0 P2P1 ×P1P0

    U轴单位向量为: U = V × W U=V \times W U=V×W

    在新坐标系UVW上圆弧过渡段的插补点坐标 ( x i , y i , z i ) (x_{i},y_{i},z_{i}) (xi,yi,zi)坐标值为:
    x i = r i − r i ⋅ cos ⁡ ( v t i ⋅ t r i ) y i = r i ⋅ sin ⁡ ( v t i ⋅ t r i ) z i = 0 \begin{array}{c}{x_{i}=r_{i}-r_{i} \cdot \cos \left(\frac{vt_{i} \cdot t}{r_{i}}\right)} \\ {y_{i}=r_{i} \cdot \sin \left(\frac{vt_{i} \cdot t}{r_{i}}\right)} \\ {z_{i}=0}\end{array} xi=riricos(rivtit)yi=risin(rivtit)zi=0
    其中, r i r_{i} ri为设定的该段圆弧的半径参数, v t i vt_{i} vti为规划得到的过渡圆弧速度.

    然后就是老生常谈的话题,把局部新坐标系中的圆弧通过左乘该坐标系相对基座标系的齐次变换矩阵从而变换回原来的XYZ值。

    在这里插入图片描述

    Matlab函数如下,我在函数中留了姿态插补四元数参数的位置,留着以后加姿态插补用:

    % 过渡圆弧的插补算法,搭配ContinueSpaceLine使用
    % 参数: Pt1,Pt2,P1(Pt1和Pt2的中间点),Qt1,Qt2
    %       过渡圆弧弧长d、过渡半径r、插补周期t
    %       圆弧插补速度vt
    % 返回值: 插补点数N,插补时间Tt
    function [x y z qk N Tt] = Transition_arc(Pt1, Pt2, P1, Qt1, Qt2, d, r, t, vt)
    % 建立新坐标系UVW
    % 新坐标轴V
    vec_Pt1P1 = P1 - Pt1;
    Pt1P1 = sqrt(power(P1(1) - Pt1(1), 2) + power(P1(2) - Pt1(2), 2) + power(P1(3) - Pt1(3), 2));
    V = (1/Pt1P1) * vec_Pt1P1;
    ox = V(1); oy = V(2); oz = V(3);
    % 新坐标系W
    vec_Pt2P1 = P1 - Pt2;
    vec_W_ = cross(vec_Pt1P1, vec_Pt2P1);
    W_ = sqrt(power(vec_W_(1), 2) + power(vec_W_(2), 2) + power(vec_W_(3), 2));
    W = (1/W_) * vec_W_;
    ax = W(1); ay = W(2); az = W(3);
    % 新坐标系U
    U = cross(V, W);
    nx = U(1); ny = U(2); nz = U(3);
    % 相对于基座标系{O-XYZ}, 新坐标系{C-UVW}的坐标变换矩阵
    T = [nx ox ax Pt1(1);
         ny oy ay Pt1(2);
         nz oz az Pt1(3);
          0  0  0  1];
    
    % 计算两个四元数之间的夹角
    dot_q = Qt1.s*Qt2.s + Qt1.v(1)*Qt2.v(1) + Qt1.v(2)*Qt2.v(2) + Qt1.v(3)*Qt2.v(3);
    if (dot_q < 0)
        dot_q = -dot_q;
    end
    % 插补时长
    Tt = d / vt;
    i = 1;
    for j = 0: t: Tt
        % 位置插补
        x_(i) = r -  r * cos(vt*j/r);
        y_(i) = r * sin(vt*j/r);
        P = T*[x_(i); y_(i); 0; 1];
        x(i) = P(1);
        y(i) = P(2);
        z(i) = P(3);
        % 单位四元数球面线性姿态插补
        % 插值点四元数
        if (dot_q > 0.9995)
            k0 = 1-t;
            k1 = t;
        else
            sin_t = sqrt(1 - power(dot_q, 2));
            omega = atan2(sin_t, dot_q);
            k0 = sin((1-t)*omega) / sin(omega);
            k1 = sin(t*omega) / sin(omega);
        end
        qk(i) = Qt1 * k0 + Qt2 * k1;
        N = i;
        i = i + 1;
    end
    
    end
    

    字符轮廓提取

    CAD中画50mm*50mm的栅格图,在栅格图内绘制字体轮廓,然后提取关键点坐标。

    以栅格左下角的点作为坐标原点,以水平方向为X轴,以垂直方向为Y轴建立平面直角坐标系,字符“S”的关键点坐标如图所示

    在这里插入图片描述

    提取坐标原点O的坐标值,然后用其他关键点的坐标值减去原点的,就能得到这些关键点在当前栅格坐标系下的坐标值,如下表

    关键点ABCDEFGHIJKL
    X54545121246465538385
    Y00222839394546221677

    ok完事儿~

    拼起来

    是的,最后总的程序就是把上面分段规划得到的一堆直线插补点和一堆圆弧插补点,按照时间顺序拼起来。总程序我就不放了,仁者见仁智者见智。

    参考文献

    [1]王斌锐,王涛,李正刚,陈立,陈迪剑.多路径段平滑过渡的自适应前瞻位姿插补算法[J].控制与决策,2019,34(06):1211-1218.
    [2]陈伟华. 工业机器人笛卡尔空间轨迹规划的研究[D].华南理工大学,2010.
    [3]类延超. 五自由度写字机器人系统研究[D].山东大学,2012.
    [4]王涛,陈立,郭振武,王斌锐,李振娜.基于圆弧转接和跨段前瞻的拾放操作轨迹规划[J/OL].计算机集成制造系统:1-13[2019-08-09]


    2019.12.8
    发现一处问题:
    子程序Transition_arc中原 k0 = sin((1-t*omega)) / sin(omega);修改为 k0 = sin((1-t)*omega) / sin(omega);


    写算法不易,要点积分ok把~
    很多朋友需要代码,每次发邮件也挺费劲,所以我就上传了文档供大家下载,仅供思路参考哈~
    这只是matlab理论代码,还是有很多bug的,实际运用写成c还是要根据实际情况修改很多很多。我的邮箱是neuxuyan@163.com,欢迎大家和我交流!
    下载链接如下:
    https://download.csdn.net/download/qq_26565435/12630819

    展开全文
  • T型、S型曲线

    千次阅读 2019-12-31 10:18:38
    总的运行段数为七段,因此S型曲线也成为七段式曲线。 下面是S曲线的算法步骤: 第一步 :由于初始时刻和终止时刻的位置存在谁大谁小的问题,即q0或者q0>q1,这会对最后的轨迹曲线造成影响。需经如下处理: ...

    梯形加减速(与抛物线拟合的线性函数)

    在这里插入图片描述
    梯形曲线在电机控制中经常被用到,因为其速度曲线形状为梯形,因此被称为梯形速度曲线。
    一般情况下,用户给定起始速度、终止速度、加速度、减速度、最大速度以及起始时刻和终止时刻的位移参数,需要计算出加速段、匀速段以及减速段对应的时间Ta、Tv、Td,然后再根据位移、速度以及加速度公式计算轨迹。
    (1)首先需要计算用户给定参数下所能达到的最大速度。要使能够达到的速度最大,则只有加速段和减速段,没有匀速段。此时h = p1 - p0,有如下公式
    在这里插入图片描述
    根据上述公式,可得到给定参数下所能达到的最大速度:
    在这里插入图片描述
    (2)比较理论最大速度vf与用户限定最大速度vmax之间的大小。当vf<vmax时,说明系统能够达到的最大速度无法满足用户要求,此时匀速段的最大速度为vv = vf;当vf>vmax时,说明给定的参数可以达到用户限定的最大速度,但是又不能超过限定的最大速度,因此匀速段的最大速度为vv = vmax
    (3)计算加速段、匀速段以及减速段的时间和位移,计算公式如下:
    在这里插入图片描述
    (4)计算各时刻下的位移、速度以及加速度,公式如下:
    在这里插入图片描述

    %% 与抛物线拟合的线性函数(线性轨迹)
    % 梯形加减速,可限制最大速度和最大加速度
    % 用户给定起始速度、终止速度、加速度、减速度、最大速度及位移参数
    % 该算法需要计算出加速段、匀速段以及减速段对应的时间Ta Tv Td
    % t0 = 0, p0 = 5, p1 = 30, v0 = 50, vmax = 150, v1 = 20, aa = 1000
    % ad = -1500
    t0 = 2;
    p0 = 5; p1 = 30;
    v0 = 50; vmax = 150; v1 = 20;
    aa = 1000; ad = -1500;
    h = p1 - p0;
    % 可达到的最大速度
    vf = sqrt((2.0*aa*ad*h - aa*v1^2 + ad*v0^2) / (ad - aa));
    % 确定匀速阶段速度
    if (vf < vmax)
        vv = vf;
    else
        vv = vmax;
    end
    % 计算加速阶段的时间和位移
    Ta = (vv - v0) / aa;
    La = v0*Ta + (1.0/2.0)*aa*Ta^2;
    % 计算匀速阶段的时间和位移
    Tv = (h - (vv^2 - v0^2)/(2.0*aa) - (v1^2 - vv^2)/(2.0*ad)) / vv;
    Lv = vv*Tv;
    % 计算减速阶段的时间和位移
    Td = (v1 - vv) / ad;
    Ld = vv*Td + (1.0/2.0)*ad*Td^2;
    k = 1;
    ts = 0.001;
    % 计算轨迹的离散点
    for t = t0: ts: (t0+Ta+Tv+Td)
        time(k) = t;
        t = t - t0;
        if (t >= 0 && t < Ta)
            p(k) = p0 + v0*t + (1.0/2.0)*aa*t^2;
            pd(k) = v0 + aa*t;
            pdd(k) = aa;
        elseif (t >= Ta && t < Ta+Tv)
            p(k) = p0 + La + vv*(t - Ta);
            pd(k) = vv;
            pdd(k) = 0;
        elseif (t >= Ta+Tv && t <= Ta+Tv+Td)
            p(k) = p0 + La + Lv + vv*(t - Ta - Tv) + (1.0/2.0)*ad*power(t - Ta - Tv, 2);
            pd(k) = vv + ad*(t - Ta - Tv);
            pdd(k) = ad;
        end
        k = k + 1;
    end
    figure(1)
    subplot(3, 1, 1)
    plot(time, p, 'r', 'LineWidth', 1.5)
    ylabel('position')
    grid on
    subplot(3, 1, 2)
    plot(time, pd, 'b', 'LineWidth', 1.5)
    ylabel('velocity')
    grid on
    subplot(3, 1, 3)
    plot(time, pdd, 'g', 'LineWidth', 1.5)
    ylabel('acceleration')
    grid on
    

    在这里插入图片描述
    由上图可以看出,梯形曲线在t0、Ta、Ta+Tv以及Ta+Tv+Td时刻时加速度不连续,速度过渡不平滑,存在冲击,这是梯形曲线的缺点,因此该加减速曲线常用于低速、低成本的运动控制过程。下面的S型曲线能解决这个问题。

    S型曲线

    在这里插入图片描述
    定义:

    • 加速段 0 <= t < Ta:
      • 以恒定的jerk J(加速度的导数)使加速度从0增加到预先设定的a;
      • 以恒定的加速度加速;
      • 已恒定的负的jerk J(加速度的导数)使加速度从预先设定的a减到0;
    • 最大速度段 Ta <= t < Ta + Tv:
      • 以恒定的速度v匀速运动;
    • 减速段 Ta + Tv <= t <= Ta + Tv + Td:
      • 已恒定的负的jerk J(加速度的导数)使加速度从0减到预先设定的-a;
      • 以恒定的加速度-a减速;
      • 以恒定的jerk J(加速度的导数)使加速度从预先设定的-a增加到0;

    总的运行段数为七段,因此S型曲线也成为七段式曲线。

    下面是S曲线的算法步骤:

    第一步:由于初始时刻和终止时刻的位置存在谁大谁小的问题,即q0<q1或者q0>q1,这会对最后的轨迹曲线造成影响。需经如下处理:
    在这里插入图片描述
    然后进行第二步;
    第二步:判断是否能够达到最大速度,步骤如下:
    在这里插入图片描述
    在这里插入图片描述
    此时得到匀速时间Tv,进行第三步;

    第三步:如果Tv > 0,则跳转到第四步;反之,则跳转到第五步;

    第四步:此时满足条件Tv > 0,说明系统能够达到的最大速度与设定的最大速度相等,即vlim = vmax,存在匀速时间,整理计算得到Ta、Tv、Td、Tj1、Tj2直接跳转到第十一步计算轨迹;

    第五步:此时满足Tv <= 0,说明系统不能达到指定的最大速度vlim < vmax,系统不存在匀速段。此时Tv = 0。此时先假设系统能够达到最大加速度和最小加速度,进行如下计算:
    在这里插入图片描述
    得到Ta、Td,对两者进行讨论,进行第六步;

    第六步:判断Ta <0 or Td < 0?,如果Ta < 0 || Td < 0,则跳转第七步;反之,则跳转至第八步;

    第七步:满足Ta < 0 || Td < 0的条件。如果此时Ta < 0,令Ta = 0,说明系统没有加速度段,只有减速段,减速段参数利用如下公式计算:
    在这里插入图片描述
    此时得到Ta、Tj,跳转至第十一步直接计算轨迹;
    如果Td < 0,令Td = 0,说明系统没有减速段,只有加速段,加速段参数利用如下公式计算:
    在这里插入图片描述
    此时得到Td、Tj,跳转至第十一步直接计算轨迹;

    第八步:判断Ta >= 2Tj && Td >= 2Tj是否成立,如果成立则跳转至第九步;反之,则跳转至第十步;

    第九步:此时Ta >= 2Tj && Td >= 2Tj,说明系统的加速段和减速段均能达到最大加速度,整理(3.26a)、(3.26b)、(3.26c)得到的Ta、Td和Tj,直接跳转至第十一步计算轨迹;

    第十步:系统加速和减速阶段至少有一段不能达到最大加速度,这种情况很少见,处理方法为逐渐减小amax,令amax = lambda*amax(0<lambda<1)(这种方法不是最优,但是从计算的角度可以接受),然后跳转至第第五步进行循环,循环结束条件为Ta >= 2Tj && Td >= 2Tj成立。这一步也体现了本文S曲线规划的缺点,一旦出现第十步的情况,则无法获得最优规划曲线,也就是在指定的参数范围内,无法获得时间最短的轨迹。

    第十一步:计算位置、速度、加速度以及加加速度轨迹曲线,公式如下,此时假设t0 = 0
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述

    S曲线流程图

    配合流程图,更容易理解上述计算步骤:
    在这里插入图片描述

    Matlab实现程序

    % 单关节空间规划
    %% S曲线参数计算(S型速度规划,又称七段式轨迹)
    function para = STrajectoryPara(q0, q1, v0, v1, vmax, amax, jmax)
    % 得到规划参数Ta, Tv, Td, Tj1, Tj2, q0, q1, v0, v1, vlim, amax, amin, alima, alimd, jmax, jmin
    % 用户给定参数
    % 边界条件q0 = 10, q1 = 0, v0 = -7, v1 = 0
    % 约束条件vmax = 10, amax = 10, jamx = 30
    % q0 = 0; q1 = 10; 
    % v0 = 7.5; v1 = 0;
    % vmax = 10; amax = 10; jmax = 30;
    vmin = -vmax; amin = -amax; jmin = -jmax;
    

    %% 利用公式(3.31)(3.32)转化得到实际的q_0、q_1、v_max、a_max
    sigma = sign(q1 - q0);
    q_0 = sigmaq0;
    q_1 = sigma
    q1;
    v_0 = sigmav0;
    v_1 = sigma
    v1;
    v_max = ((sigma+1)/2)*vmax + ((sigma-1)/2)*vmin;
    v_min = ((sigma+1)/2)*vmin + ((sigma-1)/2)*vmax;
    a_max = ((sigma+1)/2)*amax + ((sigma-1)/2)*amin;
    a_min = ((sigma+1)/2)*amin + ((sigma-1)/2)*amax;
    j_max = ((sigma+1)/2)*jmax + ((sigma-1)/2)*jmin;
    j_min = ((sigma+1)/2)*jmin + ((sigma-1)/2)*jmax;

    %% 判断是否达到最大速度
    if ((v_max - v_0)j_max < a_max^2)
    Tj1 = sqrt((v_max - v_0) / j_max); % 达不到a_max
    Ta = 2
    Tj1;
    a_lima = j_max * Tj1;
    else
    Tj1 = a_max / j_max; % 能够达到a_max
    Ta = Tj1 + (v_max - v_0) / a_max;
    a_lima = a_max;
    end
    if ((v_max - v_1)j_max < a_max^2)
    Tj2 = sqrt((v_max - v_1) / j_max); % 达不到a_min
    Td = 2
    Tj2;
    a_limd = -j_max * Tj2;
    else
    Tj2 = a_max / j_max; % 能够达到a_min
    Td = Tj2 + (v_max - v_1) / a_max;
    a_limd = -a_max;
    end
    % 根据(3.25)计算匀速段时间
    Tv = (q_1 - q_0)/v_max - (Ta/2)(1 + v_0/v_max) - (Td/2)(1 + v_1/v_max);

    %% 对Tv进行讨论
    if (Tv > 0)
    % 达到最大速度v_max,即存在匀速阶段
    vlim = v_max;
    T = Ta + Tv + Td;
    para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
    return;
    else
    % 达不到最大速度,即匀速阶段Tv=0
    % 假设最大加速度和最小加速度均能达到
    Tv = 0;
    Tj = a_max / j_max;
    Tj1 = Tj;
    Tj2 = Tj;
    delta = (a_max4/j_max2) + 2*(v_0^2 + v_1^2) + a_max*(4*(q_1 - q_0) - 2*(a_max/j_max)(v_0 + v_1));
    Ta = ((power(a_max, 2)/j_max) - 2.0
    v_0 + sqrt(delta)) / (2.0a_max);
    Td = ((power(a_max, 2)/j_max) - 2.0
    v_1 + sqrt(delta)) / (2.0a_max);
    % 对Ta和Td进行讨论
    if (Ta < 0 || Td < 0)
    if (Ta < 0)
    % 没有加速段,只有减速段
    Ta = 0; Tj1 = 0;
    Td = 2
    (q_1 - q_0) / (v_0 + v_1);
    Tj2 = (j_max*(q_1 - q_0) - sqrt(j_max*(j_maxpower(q_1 - q_0, 2) + power(v_1 + v_0, 2)(v_1 - v_0)))) / (j_max*(v_1 + v_0));
    a_lima = 0;
    a_limd = -j_maxTj2;
    vlim = v0;
    para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
    return;
    elseif (Td < 0)
    % 没有减速段,只有加速段
    Td = 0; Tj2 = 0;
    Ta = 2
    (q_1 - q_0) / (v_0 + v_1);
    Tj1 = (j_max*(q_1 - q_0) - sqrt(j_max*(j_maxpower(q_1 - q_0, 2)) - power(v_1 + v_0, 2)(v_1 - v_0))) / (j_max*(v_1 + v_0));
    a_lima = j_maxTj1;
    a_limd = 0;
    vlim = v_0 + a_lima
    (Ta - Tj1);
    para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
    return;
    end
    elseif (Ta >= 2Tj && Td >= 2Tj)
    % 加速段和减速段都能达到最大加速度
    a_lima = a_max;
    a_limd = -a_max;
    vlim = v0 + a_lima*(Ta - Tj);
    para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
    return;
    else
    % 加速和减速阶段至少有一段不能达到最大加速度
    lambda = 0.99; % 系统取0<lambda<1
    while (Ta < 2Tj || Td < 2Tj)
    % 循环
    a_max = lambdaa_max;
    Tv = 0;
    Tj = a_max / j_max;
    Tj1 = Tj;
    Tj2 = Tj;
    delta = (a_max4/j_max2) + 2
    (v_0^2 + v_1^2) + a_max*(4*(q_1 - q_0) - 2*(a_max/j_max)(v_0 + v_1));
    Ta = ((power(a_max, 2)/j_max) - 2.0
    v_0 + sqrt(delta)) / (2.0a_max);
    Td = ((power(a_max, 2)/j_max) - 2.0
    v_1 + sqrt(delta)) / (2.0a_max);
    if (Ta < 0 || Td < 0)
    if (Ta < 0)
    % 没有加速段,只有减速段
    Ta = 0; Tj1 = 0;
    Td = 2
    (q_1 - q_0) / (v_0 + v_1);
    Tj2 = (j_max*(q_1 - q_0) - sqrt(j_max*(j_maxpower(q_1 - q_0, 2) + power(v_1 + v_0, 2)(v_1 - v_0)))) / (j_max*(v_1 + v_0));
    a_lima = 0;
    a_limd = -j_maxTj2;
    vlim = v0;
    para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
    return;
    elseif (Td < 0)
    % 没有减速段,只有加速段
    Td = 0; Tj2 = 0;
    Ta = 2
    (q_1 - q_0) / (v_0 + v_1);
    Tj1 = (j_max*(q_1 - q_0) - sqrt(j_max*(j_maxpower(q_1 - q_0, 2)) - power(v_1 + v_0, 2)(v_1 - v_0))) / (j_max*(v_1 + v_0));
    a_lima = j_maxTj1;
    a_limd = 0;
    vlim = v_0 + a_lima
    (Ta - Tj1);
    para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
    return;
    end
    elseif (Ta >= 2Tj && Td >= 2Tj)
    % 加速段和减速段都能达到最大加速度
    a_lima = a_max;
    a_limd = -a_max;
    vlim = v0 + a_lima*(Ta - Tj);
    para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
    return;
    end
    end
    end
    end
    end

    %% 计算位移
    function q = S_position(t, Ta, Tv, Td, Tj1, Tj2, q0, q1, v0, v1, vlim, amax, amin, alima, alimd, jmax, jmin)
    T = Ta + Tv + Td;
    % 加速段
    if (t >= 0 && t < Tj1)
    q = q0 + v0t + jmaxt^3/6;
    elseif (t >= Tj1 && t < Ta - Tj1)
    q = q0 + v0t +(alima/6)(3t^2 - 3Tj1t + Tj1^2);
    elseif (t >= Ta - Tj1 && t < Ta)
    q = q0 + (vlim + v0)
    (Ta/2) - vlim*(Ta - t) - jmin*((Ta - t)^3/6);
    % 匀速段
    elseif (t >= Ta && t < Ta + Tv)
    q = q0 + (vlim + v0)(Ta/2) + vlim(t - Ta);
    % 减速段
    elseif (t >= Ta + Tv && t < T - Td + Tj2)
    q = q1 - (vlim + v1)(Td/2) + vlim(t - T + Td) - jmax*(power(t - T + Td, 3)/6);
    elseif (t >= T - Td + Tj2 && t < T - Tj2)
    q = q1 - (vlim + v1)(Td/2) + vlim(t - T + Td) + (alimd/6)(3power(t - T + Td, 2) - 3Tj2(t - T + Td) + Tj2^2);
    elseif (t >= T - Tj2 && t <= T)
    q = q1 - v1*(T - t) - jmax*(power(T - t, 3)/6);
    end
    end

    %% 计算速度
    function qd = S_velocity(t, Ta, Tv, Td, Tj1, Tj2, q0, q1, v0, v1, vlim, amax, amin, alima, alimd, jmax, jmin)
    T = Ta + Tv + Td;
    if (t >= 0 && t < Tj1)
    qd = v0 + jmax*(t^2/2);
    elseif (t >= Tj1 && t < Ta - Tj1)
    qd = v0 + alima*(t - Tj1/2);
    elseif (t >= Ta - Tj1 && t < Ta)
    qd = vlim + jmin*(power(Ta - t, 2)/2);
    % 匀速段
    elseif (t >= Ta && t < Ta + Tv)
    qd = vlim;
    % 减速段
    elseif (t >= Ta + Tv && t < T - Td + Tj2)
    qd = vlim - jmax*(power(t - T + Td, 2)/2);
    elseif (t >= T - Td + Tj2 && t < T - Tj2)
    qd = vlim + alimd*(t - T + Td - Tj2/2);
    elseif (t >= T - Tj2 && t <= T)
    qd = v1 + jmax*(power(t - T, 2)/2);
    end
    end

    %% 计算加速度
    function qdd = S_acceleration(t, Ta, Tv, Td, Tj1, Tj2, q0, q1, v0, v1, vlim, amax, amin, alima, alimd, jmax, jmin)
    T = Ta + Tv + Td;
    if (t >= 0 && t < Tj1)
    qdd = jmaxt;
    elseif (t >= Tj1 && t < Ta - Tj1)
    qdd = alima;
    elseif (t >= Ta - Tj1 && t < Ta)
    qdd = -jmin
    (Ta - t);
    % 匀速段
    elseif (t >= Ta && t < Ta + Tv)
    qdd = 0;
    % 减速段
    elseif (t >= Ta + Tv && t < T - Td + Tj2)
    qdd = -jmax*(t - T + Td);
    elseif (t >= T - Td + Tj2 && t < T - Tj2)
    qdd = alimd;
    elseif (t >= T - Tj2 && t <= T)
    qdd = -jmax*(T - t);
    end
    end

    %% 计算加加速度
    function qddd = S_jerk(t, Ta, Tv, Td, Tj1, Tj2, q0, q1, v0, v1, vlim, amax, amin, alima, alimd, jmax, jmin)
    T = Ta + Tv + Td;
    if (t >= 0 && t < Tj1)
    qddd = jmax;
    elseif (t >= Tj1 && t < Ta - Tj1)
    qddd = 0;
    elseif (t >= Ta - Tj1 && t < Ta)
    qddd = jmin;
    % 匀速段
    elseif (t >= Ta && t < Ta + Tv)
    qddd = 0;
    % 减速段
    elseif (t >= Ta + Tv && t < T - Td + Tj2)
    qddd = -jmax;
    elseif (t >= T - Td + Tj2 && t < T - Tj2)
    qddd = 0;
    elseif (t >= T - Tj2 && t <= T)
    qddd = jmax;
    end
    end

    测试程序

    %% S曲线规划
    % 边界条件
    q0 = 0; q1 = 10; 
    v0 = 1; v1 = 0;
    vmax = 10; amax = 10; jmax = 30;
    sigma = sign(q1 - q0);
    % 得到规划参数Ta, Tv, Td, Tj1, Tj2, q0, q1, v0, v1, vlim, amax, amin, alima, alimd, jmax, jmin
    para = STrajectoryPara(q0, q1, v0, v1, vmax, amax, jmax)
    i = 1; 
    T = para(1) + para(2) + para(3)
    for t = 0: 0.001: T
       time(i) = 0.001*i;
       q(i) = S_position(t, para(1), para(2), para(3), para(4), para(5), para(6), para(7), para(8), para(9), para(10), para(11), para(12), para(13), para(14), para(15), para(16));
       qd(i) = S_velocity(t, para(1), para(2), para(3), para(4), para(5), para(6), para(7), para(8), para(9), para(10), para(11), para(12), para(13), para(14), para(15), para(16));
       qdd(i) = S_acceleration(t, para(1), para(2), para(3), para(4), para(5), para(6), para(7), para(8), para(9), para(10), para(11), para(12), para(13), para(14), para(15), para(16));
       qddd(i) = S_jerk(t, para(1), para(2), para(3), para(4), para(5), para(6), para(7), para(8), para(9), para(10), para(11), para(12), para(13), para(14), para(15), para(16));
       i = i + 1;
    end
    q = sigma*q;
    qd = sigma*qd;
    qdd = sigma*qdd;
    qddd = sigma*qddd;
    figure(1)
    subplot(4, 1, 1)
    plot(time, q, 'r', 'LineWidth', 1.5)
    grid on
    subplot(4, 1, 2)
    plot(time, qd, 'b', 'LineWidth', 1.5)
    grid on
    subplot(4, 1, 3)
    plot(time, qdd, 'g', 'LineWidth', 1.5)
    grid on
    subplot(4, 1, 4)
    plot(time, qddd, 'LineWidth', 1.5)
    grid on
    

    在这里插入图片描述


    展开全文
  • STM32-步进电机S型加减速控制

    万次阅读 多人点赞 2017-05-16 19:57:16
    基于STM32的步进电机S型加减速控制算法STM32简介 STM32代表ARM Cortex-M内核的32位微控制器。专为要求高性能、低成本、低功耗的嵌入式应用专门设计的: STM32系列的内核主要有:Cortex-M0、Cortex-M0+、Cortex-M3、...
  • 写在前面 在前面的博文中: 基于抛物线过渡(梯形加减速)的空间直线插补算法与空间圆弧插补算法(Matlab)...下面我尝试了S型加减速曲线的规划方法并结合到空间插补算法中,仿真效果还可以,加速度曲线连续,更柔...
  • 给出了Hilber's型线性算子范数T:lr→lr(r>1,r=p,q).利用加强的Holder不等式对带范数的线性算子Hilbert型不等式作了改进,建立了一些新的不等式。
  • 带双参数的双极S型函数

    千次阅读 2016-11-16 00:15:08
    带双参数的双极S型函数sigmod(x,α\alpha,β\beta)
  • 现在,人们对自己的身体健康状况十分关注.都希望自己有一个好的身体。体温是人体的重要生理参数之一,对它的监测十分重要。本文给出了应用GE公司的ZTP135SR型红外温度传感器进行的体温计设计。
  • 下面的梯形加减速和S型曲线就不会存在这种问题。 梯形加减速(与抛物线拟合的线性函数) 梯形曲线在电机控制中经常被用到,因为其速度曲线形状为梯形,因此被称为梯形速度曲线。 一般情况...
  • S型函数:Sigmoid 函数

    2016-11-03 23:50:00
    Logistic函数或Logistic曲线是一种常见的S形函数,它是皮埃尔·弗朗索瓦·韦吕勒在1844或1845年在研究它与人口增长的关系时命名的。广义Logistic曲线可以模仿一些情况人口增长(P)的S形曲线。起初阶段大致是指数...
  • 步进电机S型曲线加减速算法与实现

    万次阅读 多人点赞 2016-08-17 15:31:38
    一年前做过的S型曲线加减速算法,再次做的时候竟然犯错,在此总结记录一下,方便以后查阅,同时希望帮组初学者提供简单的参考资料(注:本项目采用的带细分的驱动器。MCU的OC比较输出模块产生50%的PWM方波)。 S型...
  • 电机的S型曲线加速

    万次阅读 2015-12-23 16:09:05
    S 曲线算法由于其加速度和速度曲线的连续性,能够保证步进电机在运动过程中速度和加速度没有突变,减小冲击,提高步进电机运动的平稳性。 电机转速与脉冲和细分的关系: 两相步进电机的基本步距角是1.8°,即一个...
  • 编译原理之Java实现判断输入的是几文法 实验一:文法的判断与处理 实验要求: 1)编写一段程序,接受文法的输入; 2)从文法的产生式中分离出非终结符和终结符,并输出; 3)判断该文法的文法类型,是否为0,1...
  • 热电偶分度表(ITS90)(B型,E型,J型,K型,N型,R型,S型,T型)
  • 欧姆龙微型功率继电器插入端子G2R-□-Spdf,欧姆龙微型功率继电器插入端子G2R-□-S:G2R继电器新增了电路检查用带闭锁摆杆。实现无镉、无铅。1极10A、2极5A通用功率继电器。线圈接点间耐压5,000V、耐浪涌电压10...
  • %d:整型、%f:浮点 %s:string类型 %r:万能 #%d:整型 %f:浮点 %s:string类型 %r:万能 %d表示是整型,用%d表示。 如果有多个整型,表示方法是(%d%d(%变量1,%变量2)),浮点、整理、str类型一致 a...
  • 构造出了Schr odinger方程的两类半隐式差分格式,这些格式是可以显示求解的,最后讨论了它们的稳定性条件
  • 摘 要:红外体温计采用红外温度传感器测量体温,利用GE公司的zTP135SR型红外热电堆温度传感器实现对温度信号的非接触测量。详细介绍了ZTP135SR型传感器的工作原理和性能。它与自带ADC的megal68位单片机相连,...
  • 在本文中,我们考虑了以下Schrödinger-Kirchhoff问题:-(a + b RN |∇u| 2dx)∇u+ V(x)u = RN中的f(x,u)。 我们对亚临界增长下的非线性f提出了一般假设,并通过使用链接定理找到了基态解,它是与Nehari-...
  • 关于一般(alpha,m,r)-凸函数的Sugeno积分的Sandor不等式
  • R语言制作箱

    千次阅读 2016-01-25 15:29:57
    及其在R语言中的制作 箱线图(Boxplot)也称箱须图(Box-whiskerPlot),是利用数据中的五个统计量:最小值、第一四分位数、中位数、第三四分位数与最大值来描述数据的一种方法,它也可以粗略地看出数据是否具有有...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 300,663
精华内容 120,265
热门标签
关键字:

如何判断是r型还是s型