精华内容
下载资源
问答
  • 速度规划
    千次阅读
    2020-04-06 14:43:07

    一、问题描述

      梯形速度规划是最快也是最简单的速度规划方法。由于其速度连续但不平滑,加速度可控但会跳变,加加速度不可控,通常会引起受控对象震动,控制效果较差。S形速度规划由于其速度连续且平滑,加速度可控且连续,加加速度可控,控制效果好,广泛用于工业现场。但是,S形速度规划算法较复杂,在某些条件下,需要迭代计算进行规划,计算时间稍长。那么,有没有这样的方法:采用简单的梯形速度规划方法得到位置曲线,再通过一些方法,生成平滑的S形位置曲线?有的!本文提供了两种方法,效果如下:
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述

    二、MATLAB代码

      计算两列数据的卷积:

    %{
    Function: my_conv
    Description: 计算两列数据的卷积
    Input: 数据列u,数据列v
    Output: 数据列w
    Author: Marc Pony(marc_pony@163.com)
    Date: 2020.04.06
    %}
    function w = my_conv(u, v)
    m = length(u);
    n = length(v);
    count = m + n - 1;
    w = zeros(count, 1);
    for k = 1 : count
        w(k) = 0.0;
        startIndex = max([1, k - n + 1]);
        endIndex = min([k, m]);
        for j = startIndex : endIndex
            w(k) = w(k) + u(j) * v(k - j + 1);
        end
    end
    end
    

      梯形速度规划:

    %{
    Function: trapezoidal_velocity_planning
    Description: 梯形速度规划
    Input: 运动参数motionParams
    Output: 规划好了的运动参数motionParams,状态变量sta(1表示成功,0表示失败)
    Author: Marc Pony(marc_pony@163.com)
    Date: 2020.04.06
    %}
    function [motionParams, sta] = trapezoidal_velocity_planning(motionParams)
    sta = 1;
    if (motionParams.vs < motionParams.ve)
        if (motionParams.L + eps < (motionParams.ve * motionParams.ve - motionParams.vs * motionParams.vs) / (2.0 * motionParams.acc))
            sta = 0;
            return;
        end
    elseif (motionParams.vs > motionParams.ve)
        if (motionParams.L + eps < (motionParams.vs * motionParams.vs - motionParams.ve * motionParams.ve) / (2.0 * motionParams.dec))
            sta = 0;
            return;
        end
    else
        %to do nothing
    end
    
    motionParams.vm = sqrt((2.0 * motionParams.acc * motionParams.dec * motionParams.L + motionParams.dec * motionParams.vs * motionParams.vs + motionParams.acc * motionParams.ve * motionParams.ve) / (motionParams.acc + motionParams.dec));
    if (motionParams.vm > motionParams.vc)
        motionParams.vm = motionParams.vc;
    end
    
    motionParams.T1 = (motionParams.vm - motionParams.vs) / motionParams.acc;
    motionParams.T3 = (motionParams.vm - motionParams.ve) / motionParams.dec;
    
    motionParams.L1 = 0.5 * (motionParams.vm + motionParams.vs) * motionParams.T1;
    motionParams.L3 = 0.5 * (motionParams.vm + motionParams.ve) * motionParams.T3;
    motionParams.L2 = motionParams.L - motionParams.L1 - motionParams.L3;
    motionParams.T2 = motionParams.L2 / motionParams.vm;
    
    motionParams.t1 = motionParams.T1;
    motionParams.t2 = motionParams.t1 + motionParams.T2;
    motionParams.t3 = motionParams.t2 + motionParams.T3;
    motionParams.totalT = motionParams.t3;
    
    motionParams.s1 = motionParams.L1;
    motionParams.s2 = motionParams.s1 + motionParams.L2;
    motionParams.s3 = motionParams.s2 + motionParams.L3;
    end
    

      梯形速度规划的主轴插补:

    %{
    Function: main_axis_interpolation_for_trapezoidal_velocity
    Description: 梯形速度规划的主轴插补
    Input: 已经规划好了的运动参数motionParams
    Output: 主轴位置pos(mm)、主轴速度vel(mm/s)、主轴加速度acc(mm/s^2)
    Author: Marc Pony(marc_pony@163.com)
    Date: 2020.04.06
    %}
    function [pos, vel, acc] = main_axis_interpolation_for_trapezoidal_velocity(motionParams)
    if (motionParams.t < motionParams.t1)
        pos = motionParams.vs * motionParams.t + 0.5 * motionParams.acc * motionParams.t * motionParams.t;
        vel = motionParams.vs + motionParams.acc * motionParams.t;
        acc = motionParams.acc;
    elseif (motionParams.t < motionParams.t2)
        pos = motionParams.L1 + (motionParams.t - motionParams.t1) * motionParams.vm;
        vel = motionParams.vm;
        acc = 0.0;
    elseif (motionParams.t < motionParams.t3)
        pos = motionParams.L1 + motionParams.L2 + motionParams.vm * (motionParams.t - motionParams.t2) - 0.5 * motionParams.dec * (motionParams.t - motionParams.t2) * (motionParams.t - motionParams.t2);
        vel = motionParams.vm - motionParams.dec *(motionParams.t - motionParams.t2);
        acc = -motionParams.dec;
    else
        motionParams.t = motionParams.totalT;
        pos = motionParams.L;
        vel = motionParams.ve;
        acc = -motionParams.dec;
    end
    
    end
    

      采用梯形速度规划生成S形速度曲线(卷积平滑方法和滑动平滑滤波方法):

    clc;
    clear;
    close all;
    
    %% 输入参数
    motionParams = struct();
    motionParams.dt = 0.001;  %s
    motionParams.L = 100;     %mm
    motionParams.vs = 0;      %mm/s
    motionParams.ve = 0;      %mm/s
    motionParams.vc = 100;    %mm/s
    motionParams.acc = 1000;  %mm/s^2
    motionParams.dec = 1500;  %mm/s^2
    smoothFactor = 50; %平滑因子,用于衡量加加速度,非负整数,时间增加smoothFactor*插补周期
    
    %% 梯形速度规划
    [motionParams, sta] = trapezoidal_velocity_planning(motionParams);
    if sta == 0
        error('当前运动参数下无法规划!');
    end
    
    %% 梯形速度插补
    count = ceil(motionParams.totalT / motionParams.dt) + 1;
    time = zeros(count, 1);
    pos = zeros(count, 1);
    vel = zeros(count, 1);
    acc = zeros(count, 1);
    vel(1) = motionParams.vs;
    acc(1) = motionParams.acc;
    motionParams.t = 0.0;
    for i = 2 : count
        motionParams.t = motionParams.t + motionParams.dt;
        [pos(i), vel(i), acc(i)] = main_axis_interpolation_for_trapezoidal_velocity(motionParams);
        time(i) = motionParams.t;
    end
    
    %% 方法1:卷积平滑
    newPos1 = [pos ; ones(smoothFactor, 1) * pos(end)];
    temp = ones(smoothFactor, 1) / smoothFactor;
    smoothNewPos1 = my_conv(newPos1, temp);
    smoothNewPos1 = smoothNewPos1(1 : length(newPos1));
    smoothNewVel1 = diff(smoothNewPos1) / motionParams.dt;
    smoothNewAcc1 = diff(smoothNewVel1) / motionParams.dt;
    
    %% 方法2:滑动平滑滤波
    newPos2 = [ones(smoothFactor, 1) * pos(1); pos; ones(smoothFactor, 1) * pos(end)];
    count = length(pos) + smoothFactor;
    smoothNewPos2 = zeros(count, 1);
    for i = 1 : count
        smoothNewPos2(i) = mean(newPos2(i : i + smoothFactor));
    end
    smoothNewVel2 = diff(smoothNewPos2) / motionParams.dt;
    smoothNewAcc2 = diff(smoothNewVel2) / motionParams.dt;
    
    %% 绘图验证结果
    figure(1);
    plot(pos, 'r');
    hold on
    plot(smoothNewPos1, 'g--');
    plot(smoothNewPos2, 'b:');
    legend('梯形速度规划', '卷积平滑', '滑动平滑滤波')
    grid;
    title('位置曲线 [mm]');
    
    figure(2);
    plot(vel, 'r');
    hold on
    plot(smoothNewVel1, 'g--');
    plot(smoothNewVel2, 'b:');
    legend('梯形速度规划', '卷积平滑', '滑动平滑滤波')
    grid;
    title('速度曲线 [mm/sec]');
    
    figure(3);
    plot(acc, 'r');
    hold on
    plot(smoothNewAcc1, 'g--');
    plot(smoothNewAcc2, 'b:');
    legend('梯形速度规划', '卷积平滑', '滑动平滑滤波')
    grid;
    title('加速度曲线 [mm/sec^2]');
    

    三、总结

      采用梯形速度规划,采用卷积平滑方法和滑动平滑滤波方法均可以很容易生成S形速度曲线。卷积平滑方法不适用于在线插补,滑动平滑滤波方法则适用。两种方法有个缺点是:不适用于起始速度vs和结束速度ve不为0的情况。

    更多相关内容
  • 2 分段式S形曲线速度规划 已知初速度V0,末速度Ve,最大速度Vmax,路程距离s,最大加速度amax,最大jerk(加加速度)jmax,要规划位置、速度、加速度、加加速度随时间变化的曲线。具体步骤如下: 1) 假设能够达到最大...
  • 基于运动控制器的PVT运动控制设置,用于单轴运动控制
  • 不使用bezier库;改进Bizuer,避免在重复输出控制点; 同时将等间隔的Bizuer曲线改进为等间距的Bizuer曲线,避免因为距离过短导致点过于密集;增加速度规划的功能
  • T型速度规划曲线

    2018-12-03 15:32:23
    自己写的一个简单的T型速度规划m文件。需要给定最大的加速度、运行时间、开始角度 、结束角度。
  • 嵌入式数控插补系统速度规划研究及软件架构设计_余炼.caj
  • 梯形加减速速度规划

    2019-04-13 10:21:57
    梯形加减速的速度规划,可以根据起始点位置、速度,终点位置、速度,按照设定的速度和加减速度进行速度规划
  • 7段S型曲线速度规划matlab实现,采用连续的,线性的加速度曲线,速度曲线抛物线过渡的线段组成。给定起始角度,最大速度、最大加速度、最大加加速度等参数,实现位置曲线,速度曲线,加速度曲线、加加速度曲线的规划...
  • 机器人速度规划

    2018-07-02 22:42:23
    机器人速度规划算法,包括7种速度规划算法:梯形、余弦、多项式、7段s型,7段修正s型,15段s型,31段s型速度规划。所有核心算法均用c语言实现,不调用第三方算法库。
  • S型速度规划

    千次阅读 多人点赞 2020-10-11 22:07:55
    S型速度规划 摘要 梯形速度曲线出现加速度不连续的情形,这样会导致机械系统出现冲击或不可预料的振动效应。因此有必要定义一种更加平滑的运动曲线,例如采用连续的,线性的加速度曲线,如下图所示,速度曲线抛物线...

    S型速度规划

    摘要

    梯形速度曲线出现加速度不连续的情形,这样会导致机械系统出现冲击或不可预料的振动效应。因此有必要定义一种更加平滑的运动曲线,例如采用连续的,线性的加速度曲线,如下图所示,速度曲线抛物线过渡的线段组成。由于速度曲线的形状,这个轨迹称为双S速度曲线。这个轨迹有七段加加速度恒定的轨迹组成,因此又叫七段式轨迹。
    在这里插入图片描述

    分析讨论

    假设
    j m i n = − j m a x , a m i n = − a m a x , v m i n = − v m a x j_{min}=-j_{max},\text a_{min}=-\text a_{max},v_{min}=-\text v_{max} jmin=jmax,amin=amax,vmin=vmax
    为了方便分析,首先假设 t 0 = 0 , q 1 > q 0 t_0=0,q_1>q_0 t0=0,q1>q0。那么边界条件为

    • 初始速度和终止速度为 v 0 , v 1 v_0,v_1 v0,v1.
    • 初始和终止加速度 a 0 , a 1 \text a_0,\text a_1 a0,a1均为0.

    曲线可以分为三段

    1. 加速段, t ∈ [ 0 , T a ] t \in [0,T_a] t[0,Ta],加速度从0线性增加到最大,再减小到0.
    2. 最大速度段, t ∈ [ T a , T a + T v ] t\in [T_a,T_a+T_v] t[Ta,Ta+Tv],具有恒定的速度。
    3. 减速段, t ∈ [ T a + T v , T ] t\in [T_a+T_v,T] t[Ta+Tv,T],这里 T = T a + T v + T d T=T_a+T_v+T_d T=Ta+Tv+Td,减速度曲线与加速段相反。

    给定最大的加加速度 j m a x j_{max} jmax,加速度和期望位移 h = q 1 − q 0 h=q_1-q_0 h=q1q0.轨迹的计算按照式(3.30a)-(3.30g)。

    第一步是检验轨迹是否真的存在。实际上轨迹根据给定参数需要分几种情况计算。比如,给定位移h太小时,速度无法从 v 0 v_0 v0变化到 v 1 v_1 v1。这样的限制下可能只有加速段( v 0 < v 1 v_0<v_1 v0<v1)或减速段( v 0 > v 1 v_0>v_1 v0>v1)。

    因此首先需要检验是否同时存在加速段和减速段。定义
    T j ∗ = min ( ∣ v 1 − v 0 ∣ j m a x , a m a x j m a x ) (3.17) T_j^*=\text {min} \left (\sqrt {\frac {|v_1-v_0|}{j_{max}}},\frac{ \text a_{max} }{j_{max}} \right ) \tag{3.17} Tj=min(jmaxv1v0 ,jmaxamax)(3.17)
    T j ∗ = a m a x / j m a x T_j^*=\text a_{max}/j_{max} Tj=amax/jmax,加速度可以达到最大值,有可能存在加加速度为0的线段。

    如果满足以下条件,则轨迹可行
    q 1 − q 0 > { T j ∗ ( v 0 + v 1 ) , if  T j ∗ < a m a x j m a x 1 2 ( v 0 + v 1 ) ( T j ∗ + v 1 − v 0 j m a x ) , if  T j ∗ = a m a x j m a x (3.18) q_1-q_0>\begin{cases} T_j^*(v_0+v_1), \text{if} \ T_j^*<\frac{\text a_{max}}{j_{max}}\\ \frac{1}{2}(v_0+v_1)\left ( T_j^*+\frac{v_1-v_0}{\text j_{max}} \right),\text{if}\ T_j^*=\frac{\text a_{max}}{j_{max}} \tag{3.18} \end{cases} q1q0>{Tj(v0+v1),if Tj<jmaxamax21(v0+v1)(Tj+jmaxv1v0),if Tj=jmaxamax(3.18)
    若上式满足,则可以计算轨迹的参数,否则给定的参数都无法在指定的位移从初始速度变化到末速度(这时候可以改变初始速度或末速度的值,可另外讨论。这不在本文的讨论范围内)。假设运动中的最大速度为 v l i m = max ( q ˙ ( t ) ) v_{lim}=\text {max}(\dot q(t)) vlim=max(q˙(t)),这时存在两种情况

    Case1 : v l i m = v m a x v_{lim}=v_{max} vlim=vmax.

    Case2: v l i m < v m a x v_{lim}<v_{max} vlim<vmax.

    第二种情形,最大速度不能达到,只有加速段和减速段,没有匀速段,只有计算轨迹参数后才能判断。

    当给定位移较小时,在Case1和Case2中有可能都达不到最大加速度。

    我们定义

    T j 1 T_{j1} Tj1 :在加速度段加加速度恒定( j m a x 或 j m i n j_{max}或j_{min} jmaxjmin)的时间;

    T j 2 T_{j2} Tj2:在减速段加加速度恒定( j m a x 或 j m i n j_{max}或j_{min} jmaxjmin)的时间;

    T a T_a Ta:加速度段时间;

    T v T_v Tv:匀速段时间;

    T d T_d Td:减速段时间;

    T T T:总时间( = T a + T v + T d =T_a+T_v+T_d =Ta+Tv+Td

    Case1: v l i m = v m a x v_{lim}=v_{max} vlim=vmax

    通过下面的条件判断是否达到最大加速度

    if
    ( v m a x − v 0 ) j m a x < a m a x 2 (3.19) (v_{max}-v_0)j_{max}<\text a^2_{max} \tag{3.19} (vmaxv0)jmax<amax2(3.19)
    a m a x \text a_{max} amax达不到

    if
    ( v m a x − v 1 ) j m a x < a m a x 2 (3.20) (v_{max}-v_1)j_{max}<\text a^2_{max} \tag{3.20} (vmaxv1)jmax<amax2(3.20)
    若(3.19)满足,则加速段的时间用下式计算
    T j 1 = v m a x − v 0 j m a x , T a = 2 T j 1 (3.21) T_{j1}=\sqrt{\frac{v_{max}-v_0}{j_{max}}},T_a=2T_{j1}\tag{3.21} Tj1=jmaxvmaxv0 ,Ta=2Tj1(3.21)
    否则
    T j 1 = a m a x j m a x , T a = T j 1 + v m a x − v 0 a m a x (3.22) T_{j1}=\frac{\text a_{max}}{j_{max}},T_a=T_{j1}+\frac{v_{max}-v_0}{\text a_{max}}\tag{3.22} Tj1=jmaxamax,Ta=Tj1+amaxvmaxv0(3.22)
    若(3.20)满足,则减速段的时间用下式计算
    T j 2 = v m a x − v 1 j m a x , T d = 2 T j 2 (3.23) T_{j2}=\sqrt {\frac{v_{max}-v_1}{j_{max}}},T_{d}=2T_{j2}\tag{3.23} Tj2=jmaxvmaxv1 ,Td=2Tj2(3.23)
    否则
    T j 2 = a m a x j m a x , T d = T j 2 + v m a x − v 1 a m a x (3.24) T_{j2}=\frac{\text a_{max}}{j_{max}},T_d=T_{j2}+\frac{v_{max}-v_1}{\text a_{max}}\tag{3.24} Tj2=jmaxamax,Td=Tj2+amaxvmaxv1(3.24)
    最后,计算匀速段时间
    T v = q 1 − q 0 v m a x − T a 2 ( 1 + v 0 v m a x ) − T d 2 ( 1 + v 1 v m a x ) (3.25) T_v=\frac{q_1-q_0}{v_{max}}-\frac{T_a}{2}(1+\frac{v_0}{v_{max}})-\frac{T_d}{2}(1+\frac{v_1}{v_{max}})\tag{3.25} Tv=vmaxq1q02Ta(1+vmaxv0)2Td(1+vmaxv1)(3.25)
    T v T_v Tv进行讨论:
    (1)若 T v > 0 T_v>0 Tv>0,则达到最大速度。这时直接可以用式(3.21)-式(3.25)计算轨迹的值。

    (2)若 T v < 0 T_v<0 Tv<0,只是意味着 v l i m v_{lim} vlim小于 v m a x v_{max} vmax。这时候考虑Case2.

    例子**Example3.9 **:如下面是case1达到最大速度的一个例子,给定的条件如下

    边界条件: q 0 = 0 , q 1 = 10 , v 0 = 1 , v 1 = 0 q_0=0,q_1=10,v_0=1,v_1=0 q0=0,q1=10,v0=1,v1=0,

    约束条件: v m a x = 5 , a m a x = 10 , j m a x = 30 v_{max}=5,a_{max}=10,j_{max}=30 vmax=5,amax=10,jmax=30.

    计算得到轨迹的各段时间为

          Tj1: 0.3333
           Ta: 0.7333
           Tv: 1.1433
          Tj2: 0.3333
           Td: 0.8333
            T: 2.7100
           q0: 0
           q1: 10
           v0: 1
           v1: 0
         vlim: 5
        alima: 10
        alimd: -10
         jmax: 30
    

    轨迹的图形如下图所示

    [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-jVMAGVRb-1602424996880)(image/Example3.9.png)]

    Case2: v l i m < v m a x v_{lim}<v_{max} vlim<vmax

    在这种情形下不能达到指定最大速度,所以匀速段的时间( T v = 0 T_v=0 Tv=0 )。

    若最大加速度和最小加速度都能达到,那么各段的时间可以按下式计算
    T j 1 = T j 2 = T j = a m a x j m a x (2.26a) T_{j1}=T_{j2}=T_j=\frac{\text a_{max}}{j_{max}}\tag{2.26a} Tj1=Tj2=Tj=jmaxamax(2.26a)

    T a = a m a x 2 j m a x − 2 v 0 + Δ 2 a m a x (2.26b) T_a=\frac{\frac{\text a_{max}^2}{j_{max}}-2v_0+\sqrt{\Delta}}{2\text a_{max}} \tag{2.26b} Ta=2amaxjmaxamax22v0+Δ (2.26b)

    T d = a m a x 2 j m a x − 2 v 1 + Δ 2 a m a x (2.26c) T_d=\frac{\frac{\text a_{max}^2}{j_{max}}-2v_1+\sqrt{\Delta}}{2\text a_{max}} \tag{2.26c} Td=2amaxjmaxamax22v1+Δ (2.26c)

    其中
    Δ = a m a x 4 j m a x 2 + 2 ( v 0 2 + v 1 2 ) + a m a x ( 4 ( q 1 − q 0 ) − 2 a m a x j m a x ( v 0 + v 1 ) ) (2.27) \Delta=\frac{\text a_{max}^4}{j_{max}^2}+2(v_0^2+v_1^2)+\text a_{max}(4(q_1-q_0)-2\frac{\text a_{max}}{j_{max}}(v_0+v_1))\tag{2.27} Δ=jmax2amax4+2(v02+v12)+amax(4(q1q0)2jmaxamax(v0+v1))(2.27)

    T a , T d T_a,T_d Ta,Td进行讨论
    { T a ≥ 2 T j T d ≥ 2 T j \begin{cases} T_a\ge2T_j\\ T_d\ge 2T_j \end{cases} {Ta2TjTd2Tj

    (1)若上式成立,则加速段和减速段都能达到最大加速度。

    例子Example3.10下面是case2加速段和减速段都能达到最大加速度的一个例子。

    边界条件: q 0 = 0 , q 1 = 10 , v 0 = 1 , v 1 = 0 , q_0=0,q_1=10,v_0=1,v_1=0, q0=0,q1=10,v0=1,v1=0,

    约束条件: v m a x = 10 , a m a x = 10 , j m a x = 30. v_{max}=10,\text a_{max}=10,j_{max}=30. vmax=10,amax=10,jmax=30.

    规划得到轨迹的结果为

          Tj1: 0.3333
           Ta: 1.0747
           Tv: 0
          Tj2: 0.3333
           Td: 1.1747
            T: 2.2494
           q0: 0
           q1: 10
           v0: 1
           v1: 0
         vlim: 8.4136
        alima: 10
        alimd: -10
         jmax: 30
    

    最大速度为 8.4136

    轨迹的图形如下图所示

    在这里插入图片描述

    (2)否则,加速度段和减速段至少有一段不能达到最大加速度。

    这时就不能使用式(3.26a),(3.26b),(3.26c)计算轨迹的各段时间。这种情形(实际上是很少见的)要规划轨迹的参数是相当困难的,而寻找一种近似的方案是很方便的,这种方案不是最优的,但是从计算的角度可以接受。这种方式是逐渐减小 a m a x \text a_{max} amax(假设 a m a x = γ a m a x , 0 < γ < 1 \text a_{max}=\gamma \text a_{max},0<\gamma<1 amax=γamax,0<γ<1),然后用(3.26a),(3.26b),(3.26c)计算各段时间,直到条件 T a > 2 T j , T d > 2 T j T_a>2T_j,T_d>2T_j Ta>2TjTd>2Tj都成立。但是该方式就有一个缺点,需要循环计算以找到合适的 a m a x \text a_{max} amax,计算量有所增加。如何确定 γ \gamma γ以达到合理的计算量和精度是需要考虑的,本博客提供的程序中取 γ = 0.99 \gamma=0.99 γ=0.99

    例子Example3.11:下面是该情形的一个例子。

    边界条件: q 0 = 0 , q 1 = 10 , v 0 = 7 , v 1 = 0 , q_0=0,q_1=10,v_0=7,v_1=0, q0=0,q1=10,v0=7,v1=0,

    约束条件: v m a x = 10 , a m a x = 10 , j m a x = 30. v_{max}=10,\text a_{max}=10,j_{max}=30. vmax=10,amax=10,jmax=30.

    规划得到轨迹的结果为(注意 γ \gamma γ取不同的值,结果会有所差异,以下结果中取 γ = 0.99 \gamma=0.99 γ=0.99)

          Tj1: 0.2321
           Ta: 0.4666
           Tv: 0
          Tj2: 0.2321
           Td: 1.4718
            T: 1.9384
           q0: 0
           q1: 10
           v0: 7
           v1: 0
         vlim: 8.6329
        alima: 6.9641
        alimd: -6.9641
         jmax: 30
    

    轨迹的图形如下图所示

    在这里插入图片描述

    另外,在计算(3.26a),(3.26b),(3.26c)时可能会出现 T a < 0 T_a<0 Ta<0 T d < 0 T_d<0 Td<0。这时候只有加速段或只有减速段,这取决于初速度的大小。

    T a < 0 T_a<0 Ta<0时(只有 v 0 > v 1 v_0>v_1 v0>v1时出现),令 T a = 0 T_a=0 Ta=0,减速段参数按下式计算
    T d = 2 q 1 − q 0 v 0 + v 1 (3.28a) T_d=2\frac{q_1-q_0}{v_0+v_1}\tag{3.28a}\\ Td=2v0+v1q1q0(3.28a)

    T j 2 = j m a x ( q 1 − q 0 ) − j m a x ( j m a x ( q 1 − q 0 ) 2 + ( v 1 + v 0 ) 2 ( v 1 − v 0 ) ) j m a x ( v 1 + v 0 ) (3.28b) T_{j2}=\frac{j_{max}(q_1-q_0)-\sqrt{j_{max}(j_{max}(q_1-q_0)^2+(v_1+v_0)^2(v_1-v_0))}}{j_{max}(v_1+v_0)} \tag{3.28b} Tj2=jmax(v1+v0)jmax(q1q0)jmax(jmax(q1q0)2+(v1+v0)2(v1v0)) (3.28b)

    T d < 0 T_d<0 Td<0时(只有 v 0 < v 1 v_0<v_1 v0<v1时出现),令 T d = 0 T_d=0 Td=0,加速段参数按下式计算
    T a = 2 q 1 − q 0 v 0 + v 1 (3.29a) T_a=2\frac{q_1-q_0}{v_0+v_1}\tag{3.29a}\\ Ta=2v0+v1q1q0(3.29a)

    T j 2 = j m a x ( q 1 − q 0 ) − j m a x ( j m a x ( q 1 − q 0 ) 2 + ( v 1 + v 0 ) 2 ( v 1 − v 0 ) ) j m a x ( v 1 + v 0 ) (3.29b) T_{j2}=\frac{j_{max}(q_1-q_0)-\sqrt{j_{max}(j_{max}(q_1-q_0)^2+(v_1+v_0)^2(v_1-v_0))}}{j_{max}(v_1+v_0)} \tag{3.29b} Tj2=jmax(v1+v0)jmax(q1q0)jmax(jmax(q1q0)2+(v1+v0)2(v1v0)) (3.29b)

    例子Example3.12:下面是 T a = 0 T_a=0 Ta=0的一个例子。

    边界条件: q 0 = 0 , q 1 = 10 , v 0 = 7.5 , v 1 = 0 , q_0=0,q_1=10,v_0=7.5,v_1=0, q0=0,q1=10,v0=7.5,v1=0,

    约束条件: v m a x = 10 , a m a x = 10 , j m a x = 30. v_{max}=10,\text a_{max}=10,j_{max}=30. vmax=10,amax=10,jmax=30.

    规划得到轨迹的结果为(注意 γ \gamma γ取不同的值,结果会有所差异,以下结果中取 γ = 0.99 \gamma=0.99 γ=0.99)

          Tj1: 0
           Ta: 0
           Tv: 0
          Tj2: 0.0973
           Td: 2.6667
            T: 2.6667
           q0: 0
           q1: 10
           v0: 7.5000
           v1: 0
         vlim: 7.5000
        alima: 0
        alimd: -2.9190
         jmax: 30
    

    轨迹的图形如下图所示

    在这里插入图片描述

    计算 q 1 > q 0 q_1>q_0 q1>q0的轨迹

    一旦各段的时间长度和相关参数确定后,就可以使用下面的方程计算S型轨迹。这里假设 t 0 = 0 t_0=0 t0=0

    加速段

    a) t ∈ [ 0 , T j 1 ] t\in[0,T_{j1}] t[0,Tj1]
    { q ( t ) = q 0 + v 0 t + j m a x t 3 6 q ˙ ( t ) = v 0 + j m a x t 2 2 q ¨ ( t ) = j m a x t q ( 3 ) ( t ) = j m a x (3.30a) \begin{cases} q(t)=q_0+v_0t+j_{max}\frac{t^3}{6}\\ \dot q(t)=v_0+j_{max}\frac{t^2}{2}\\ \ddot q(t)=j_{max}t\\ q^{(3)}(t)=j_{max} \end{cases} \tag{3.30a} q(t)=q0+v0t+jmax6t3q˙(t)=v0+jmax2t2q¨(t)=jmaxtq(3)(t)=jmax(3.30a)
    b) t ∈ [ T j 1 , T a − T j 1 ] t\in[T_{j1},T_a-T_{j1}] t[Tj1,TaTj1]
    { q ( t ) = q 0 + v 0 t + a l i m a 6 ( 3 t 3 − 3 T j 1 t + T j 1 2 ) q ˙ ( t ) = v 0 + a l i m a ( t − T j 1 2 ) q ¨ ( t ) = j m a x T j 1 = a l i m a q ( 3 ) ( t ) = 0 (3.30b) \begin{cases} q(t)=q_0+v_0t+\frac{\text a_{lima}}{6}(3t^3-3T_{j1}t+T_{j1}^2)\\ \dot q(t)=v_0+\text a_{lima}(t-\frac{T_{j1}}{2})\\ \ddot q(t)=j_{max}T_{j1}=\text a_{lima}\\ q^{(3)}(t)=0 \end{cases} \tag{3.30b} q(t)=q0+v0t+6alima(3t33Tj1t+Tj12)q˙(t)=v0+alima(t2Tj1)q¨(t)=jmaxTj1=alimaq(3)(t)=0(3.30b)
    c) t ∈ [ T a − T j 1 , T a ] t\in[T_a-T_{j1},T_a] t[TaTj1,Ta]
    { q ( t ) = q 0 + ( v l i m + v 0 ) T a 2 − v l i m ( T a − t ) − j m a x ( T a − t ) 3 6 q ˙ ( t ) = v l i m + j m i n ( T a − t ) 2 2 q ¨ ( t ) = − j m i n ( T a − t ) q ( 3 ) ( t ) = j m i n = − j m a x (3.30c) \begin{cases} q(t)=q_0+(v_{lim}+v_0)\frac{T_a}{2}-v_{lim}(T_a-t)-j_{max}\frac{(T_a-t)^3}{6}\\ \dot q(t)=v_{lim}+j_{min}\frac{(T_a-t)^2}{2}\\ \ddot q(t)=-j_{min}(T_a-t)\\ q^{(3)}(t)=j_{min}=-j_{max} \end{cases} \tag{3.30c} q(t)=q0+(vlim+v0)2Tavlim(Tat)jmax6(Tat)3q˙(t)=vlim+jmin2(Tat)2q¨(t)=jmin(Tat)q(3)(t)=jmin=jmax(3.30c)

    匀速段

    a) t ∈ [ T a , T a + T v ] t\in[T_a,T_a+T_v] t[Ta,Ta+Tv]
    { q ( t ) = q 0 + ( v l i m + v 0 ) T a 2 + v l i m ( t − T a ) q ˙ ( t ) = v l i m q ¨ ( t ) = 0 q ( 3 ) ( t ) = 0 (3.30d) \begin{cases} q(t)=q_0+(v_{lim}+v_0)\frac{T_a}{2}+v_{lim}(t-T_a)\\ \dot q(t)=v_{lim}\\ \ddot q(t)=0\\ q^{(3)}(t)=0 \end{cases} \tag{3.30d} q(t)=q0+(vlim+v0)2Ta+vlim(tTa)q˙(t)=vlimq¨(t)=0q(3)(t)=0(3.30d)

    减速段

    a) t ∈ [ T − T d , T − T d + T j 2 ] t\in[T-T_d,T-T_d+T_{j2}] t[TTd,TTd+Tj2]
    { q ( t ) = q 1 − ( v l i m + v 1 ) T d 2 + v l i m ( t − T + T d ) − j m a x ( t − T + T d ) 3 6 q ˙ ( t ) = v l i m − j m a x ( t − T + T d ) 2 2 q ¨ ( t ) = − j m a x ( t − T + T d ) q ( 3 ) ( t ) = j m a x = − j m a x (3.30e) \begin{cases} q(t)=q_1-(v_{lim}+v_1)\frac{T_d}{2}+v_{lim}(t-T+T_d)-j_{max}\frac{(t-T+T_d)^3}{6}\\ \dot q(t)=v_{lim}-j_{max}\frac{(t-T+T_d)^2}{2}\\ \ddot q(t)=-j_{max}(t-T+T_d)\\ q^{(3)}(t)=j_{max}=-j_{max} \end{cases} \tag{3.30e} q(t)=q1(vlim+v1)2Td+vlim(tT+Td)jmax6(tT+Td)3q˙(t)=vlimjmax2(tT+Td)2q¨(t)=jmax(tT+Td)q(3)(t)=jmax=jmax(3.30e)
    b) t ∈ [ T − T d + T j 2 , T − T j 2 ] t\in[T-T_{d}+T_{j2},T-T_{j2}] t[TTd+Tj2,TTj2]
    { q ( t ) = q 1 − ( v l i m + v 1 ) T d 2 + v l i m ( t − T + T d ) + a l i m d 6 ( 3 ( t − T + T d ) 2 − 3 T j 2 ( t − T + T d ) + T j 2 2 ) q ˙ ( t ) = v l i m + a l i m d ( t − T + T d − T j 2 2 ) q ¨ ( t ) = − j m a x T j 2 = a l i m d q ( 3 ) ( t ) = 0 (3.30f) \begin{cases} q(t)=q_1-(v_{lim}+v_1)\frac{T_d}{2}+v_{lim}(t-T+T_d)+ \frac{\text a_{limd}}{6}(3(t-T+T_d)^2-3T_{j2}(t-T+T_d)+T_{j2}^2)\\ \dot q(t)=v_{lim}+\text a_{limd}(t-T+T_d-\frac{T_{j2}}{2})\\ \ddot q(t)=-j_{max}T_{j2}=\text a_{limd}\\ q^{(3)}(t)=0 \end{cases} \tag{3.30f} q(t)=q1(vlim+v1)2Td+vlim(tT+Td)+6alimd(3(tT+Td)23Tj2(tT+Td)+Tj22)q˙(t)=vlim+alimd(tT+Td2Tj2)q¨(t)=jmaxTj2=alimdq(3)(t)=0(3.30f)
    c) t ∈ [ T − T j 2 , T ] t\in[T-T_{j2},T] t[TTj2,T]
    { q ( t ) = q 1 − v 1 ( T − t ) − j m a x ( T − t ) 3 6 q ˙ ( t ) = v 1 + j m a x ( t − T ) 2 2 q ¨ ( t ) = − j m a x ( T − t ) q ( 3 ) ( t ) = j m a x (3.30g) \begin{cases} q(t)=q_1-v_1(T-t)-j_{max}\frac{(T-t)^3}{6}\\ \dot q(t)=v_1+j_{max}\frac{(t-T)^2}{2}\\ \ddot q(t)=-j_{max}(T-t)\\ q^{(3)}(t)=j_{max} \end{cases} \tag{3.30g} q(t)=q1v1(Tt)jmax6(Tt)3q˙(t)=v1+jmax2(tT)2q¨(t)=jmax(Tt)q(3)(t)=jmax(3.30g)

    计算 q 1 < q 0 q_1<q_0 q1<q0的轨迹

    q 1 < q 0 q_1<q_0 q1<q0,轨迹的参数按照上面的步骤计算,只是需要考虑始末的位置/速度符号相反,并且计算后需要对位置曲线、速度曲线、加速度曲线和加加速度曲线进行反转。

    更一般地,设给定的始末位置和速度为 ( q ^ 0 , q ^ 1 , v ^ 0 , v ^ 1 ) (\hat q_0,\hat q_1,\hat v_0,\hat v_1) (q^0,q^1,v^0,v^1),为了计算轨迹,需要把这些值转化为
    q 0 = σ q ^ 0 , q 1 = σ q ^ 1 , v 0 = σ v ^ 0 , v 1 = σ v ^ 1 (3.31) q_0=\sigma \hat q_0,q_1=\sigma \hat q_1,v_0=\sigma \hat v_0,v_1=\sigma\hat v_1 \tag{3.31} q0=σq^0,q1=σq^1,v0=σv^0,v1=σv^1(3.31)
    这里 σ = sign ( q ^ 1 − q ^ 0 ) \sigma=\text {sign}(\hat q_1-\hat q_0) σ=sign(q^1q^0)。相似的,对速度,加速度,加加速度 ( v ^ m a x , v ^ m i n , a ^ m a x , a ^ m i n , j ^ m a x , j ^ m i n ) (\hat v_{max},\hat v_{min},\hat {\text a}_{max},\hat {\text a}_{min},\hat j_{max},\hat j_{min}) (v^max,v^min,a^max,a^min,j^max,j^min)的约束也需要进行转换
    { v m a x = ( σ + 1 ) 2 v ^ m a x + ( σ − 1 ) 2 v ^ m i n v m i n = ( σ + 1 ) 2 v ^ m i n + ( σ − 1 ) 2 v ^ m a x a m a x = ( σ + 1 ) 2 a ^ m a x + ( σ − 1 ) 2 a ^ m i n a m i n = ( σ + 1 ) 2 a ^ m i n + ( σ − 1 ) 2 a ^ m a x j m a x = ( σ + 1 ) 2 j ^ m a x + ( σ − 1 ) 2 j ^ m i n j m i n = ( σ + 1 ) 2 j ^ m i n + ( σ − 1 ) 2 j ^ m a x (3.32) \begin{cases} v_{max}=\frac{(\sigma+1)}{2}\hat v_{max}+\frac{(\sigma-1)}{2}\hat v_{min}\\ v_{min}=\frac{(\sigma+1)}{2}\hat v_{min}+\frac{(\sigma-1)}{2}\hat v_{max}\\ \text a_{max}=\frac{(\sigma+1)}{2}\hat {\text a}_{max}+\frac{(\sigma-1)}{2}\hat {\text a}_{min}\\ \text a_{min}=\frac{(\sigma+1)}{2}\hat {\text a}_{min}+\frac{(\sigma-1)}{2}\hat {\text a}_{max}\\ j_{max}=\frac{(\sigma+1)}{2}\hat j_{max}+\frac{(\sigma-1)}{2}\hat j_{min}\\ j_{min}=\frac{(\sigma+1)}{2}\hat j_{min}+\frac{(\sigma-1)}{2}\hat j_{max}\\ \end{cases} \tag{3.32} vmax=2(σ+1)v^max+2(σ1)v^minvmin=2(σ+1)v^min+2(σ1)v^maxamax=2(σ+1)a^max+2(σ1)a^minamin=2(σ+1)a^min+2(σ1)a^maxjmax=2(σ+1)j^max+2(σ1)j^minjmin=2(σ+1)j^min+2(σ1)j^max(3.32)
    最后,计算的曲线也需要转换为
    { q ^ ( t ) = σ q ( t ) q ^ ˙ ( t ) = σ q ˙ ( t ) q ^ ¨ ( t ) = σ q ¨ ( t ) q ^ ( 3 ) ( t ) = σ q ( 3 ) ( t ) (3.33) \begin{cases} \hat q(t)=\sigma q(t)\\ \dot{\hat q}(t)=\sigma \dot q(t)\\ \ddot {\hat q}(t)=\sigma \ddot q(t)\\ \hat q^{(3)}(t)=\sigma q^{(3)}(t) \end{cases} \tag{3.33} q^(t)=σq(t)q^˙(t)=σq˙(t)q^¨(t)=σq¨(t)q^(3)(t)=σq(3)(t)(3.33)

    流程图

    综上所述,S型加减速的流程图如下

    在这里插入图片描述

    算法程序实现

    前面进行了分类讨论,终于到程序实现这激动人心的时刻了。不过别高兴太早,下面只提供matlab的实现程序,不过该程序是按照c/c++的风格编写,可以轻易改写为c/c++程序。

    **欢迎读者对程序进行无情的测试,让bug的暴风雨来得更猛烈些! **

    S型速度规划的算法子函数:CalcFun.m

    %{
    CalcFun.m
    S型速度规划的算法子函数
    %}
    %%
    %子函数声明,可供外部文件调用
    function fun=CalcFun
    fun.displacement=@displacement;
    fun.velocity=@velocity;
    fun.acceleration=@acceleration;
    fun.jerk=@jerk;
    fun.CalcSProfile=@CalcSProfile;
    fun.InitialParam=@InitialParam;
    end
    %%
    %子函数定义
    %{
    根据式(3.30a)-(3.30g)计算任意时刻的位移
    %}
    function q=displacement(t,Param)
    q0=Param.q0;
    q1=Param.q1;
    v0=Param.v0;
    v1=Param.v1;
    vlim=Param.vlim;
    alima=Param.alima;
    alimd=Param.alimd;
    jmax=Param.jmax;
    Tj1=Param.Tj1;
    Ta=Param.Ta;
    Tv=Param.Tv;
    Tj2=Param.Tj2;
    Td=Param.Td;
    T=Param.T;
    jmin=Param.jmin;
    if(t>=0 && t<Tj1)
        q=q0+v0*t+jmax*t^3/6;
    elseif(t>=Tj1 && t<Ta-Tj1)   
        q=q0+v0*t+alima/6*(3*t^2-3*Tj1*t+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>=T-Td && t<T-Td+Tj2)
        q=q1-(vlim+v1)*Td/2+vlim*(t-T+Td)-jmax*(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*(3*(t-T+Td)^2-3*Tj2*(t-T+Td)+Tj2^2);
    else
        q=q1-v1*(T-t)-jmax*(T-t)^3/6;
    end
    %计算(3.33)
        q=Param.sigma*q;
    end
    
    
    %{
    根据式(3.30a)-(3.30g)计算任意时刻的速度
    %}
    function dq=velocity(t,Param)
    
    v0=Param.v0;
    v1=Param.v1;
    vlim=Param.vlim;
    alima=Param.alima;
    alimd=Param.alimd;
    jmax=Param.jmax;
    Tj1=Param.Tj1;
    Ta=Param.Ta;
    Tv=Param.Tv;
    Tj2=Param.Tj2;
    Td=Param.Td;
    T=Param.T;
    jmin=Param.jmin;
    if(t>=0 && t<Tj1)
        dq=v0+jmax*t^2/2;
    elseif(t>=Tj1 && t<Ta-Tj1)   
        dq=v0+alima*(t-Tj1/2);
    elseif(t>=Ta-Tj1 && t<Ta)
        dq=vlim+jmin*(Ta-t)^2/2;
    elseif(t>=Ta && t<Ta+Tv)
        dq=vlim;
    elseif(t>=T-Td && t<T-Td+Tj2)
        dq=vlim-jmax*(t-T+Td)^2/2;
    elseif(t>=T-Td+Tj2 && t<T-Tj2)
        dq=vlim+alimd*(t-T+Td-Tj2/2);
    else
        dq=v1+jmax*(T-t)^2/2;
    end
    %计算(3.33)
    dq=Param.sigma*dq;
    end
    
    %{
    根据式(3.30a)-(3.30g)计算任意时刻的加速度
    %}
    function ddq=acceleration(t,Param)
    
    alima=Param.alima;
    alimd=Param.alimd;
    jmax=Param.jmax;
    Tj1=Param.Tj1;
    Ta=Param.Ta;
    Tv=Param.Tv;
    Tj2=Param.Tj2;
    Td=Param.Td;
    T=Param.T;
    jmin=Param.jmin;
    if(t>=0 && t<Tj1)
        ddq=jmax*t;
    elseif(t>=Tj1 && t<Ta-Tj1)   
        ddq=alima;
    elseif(t>=Ta-Tj1 && t<Ta)
        ddq=-jmin*(Ta-t);
    elseif(t>=Ta && t<Ta+Tv)
        ddq=0;
    elseif(t>=T-Td && t<T-Td+Tj2)
       ddq=-jmax*(t-T+Td);
    elseif(t>=T-Td+Tj2 && t<T-Tj2)
        ddq=alimd;
    else
        ddq=-jmax*(T-t);
    end
    %计算(3.33)
    ddq=Param.sigma*ddq;
    end
    
    %{
    根据式(3.30a)-(3.30g)计算任意时刻的加加速度
    %}
    function jt=jerk(t,Param)
    
    jmax=Param.jmax;
    Tj1=Param.Tj1;
    Ta=Param.Ta;
    Tv=Param.Tv;
    Tj2=Param.Tj2;
    Td=Param.Td;
    T=Param.T;
    jmin=Param.jmin;
    if(t>=0 && t<Tj1)
        jt=jmax;
    elseif(t>=Tj1 && t<Ta-Tj1)   
        jt=0;
    elseif(t>=Ta-Tj1 && t<Ta)
        jt=jmin;
    elseif(t>=Ta && t<Ta+Tv)
        jt=0;
    elseif(t>=T-Td && t<T-Td+Tj2)
       jt=jmin;
    elseif(t>=T-Td+Tj2 && t<T-Tj2)
        jt=0;
    else
        jt=jmax;
    end
    %计算(3.33)
    jt=Param.sigma*jt;
    end
    
    %{
    (1)参数合法性检查
    (2)初始化参数结构体
    %}
    function IniParam=InitialParam(q0,q1,v0,v1,v_max,a_max,j_max)
    %参数检查,绝对值小于1.0E-8的数值认为是0.
    if(abs(v_max)<1.0E-8)
        disp('参数错误。最大速度不能为0!程序退出。');
        return;
    end
    if(abs(a_max)<1.0E-8)
        disp('参数错误。最大加速度不能为0!程序退出。');
        return;
    end
    if(abs(j_max)<1.0E-8)
        disp('参数错误。最大加加速度不能为0!程序退出。');
        return;
    end
    if(abs(q1-q0)<1.0E-8)
        disp('参数错误。输入位移不能为0!程序退出。');
        return;
    end
    %%
    %定义并初始化输入参数的结构体
    if(q1-q0>0)
        sigma=1;
    else
        sigma=-1;
    end
    v_min=-v_max;
    j_min=-j_max;
    a_min=-a_max;
    %计算(3.31),
    hq0=sigma*q0;
    hq1=sigma*q1;
    hv0=sigma*v0;
    hv1=sigma*v1;
    %计算(3.32)
    hv_max=(sigma+1)/2*v_max+(sigma-1)/2*v_min;
    hv_min=(sigma+1)/2*v_min+(sigma-1)/2*v_max;
    ha_max=(sigma+1)/2*a_max+(sigma-1)/2*a_min;
    ha_min=(sigma+1)/2*a_min+(sigma-1)/2*a_max;
    hj_max=(sigma+1)/2*j_max+(sigma-1)/2*j_min;
    hj_min=(sigma+1)/2*j_min+(sigma-1)/2*j_max;
    IniParam=struct(...
    'q0',{hq0},...
    'q1',{hq1},...
    'v0',{hv0},...
    'v1',{hv1},...
    'vmax',{hv_max},...
    'vmin',{hv_min},...
    'amax',{ha_max},...
    'amin',{ha_min},...
    'jmax',{hj_max},...
    'jmin',{hj_min},...
    'sigma',{sigma}...
    );
    end
    
    %{
    根据输入参数进行S型速度规划,输出结果Param
    %}
    function Param=CalcSProfile(q0,q1,v0,v1,v_max,a_max,j_max)
    
    %初始化参数
    InParam=InitialParam(q0,q1,v0,v1,v_max,a_max,j_max);
    %定义并初始化存储规划结果的结构体
    Param=struct(...
     'Tj1',{0},'Ta',{0},'Tv',{0},'Tj2',{0},'Td',{0},'T',{0},...
     'q0',{0},...
     'q1',{0},...
    'v0',{0},...
    'v1',{0},...
    'vlim',{0},...
    'alima',{0},...
    'alimd',{0},...
    'jmax',{0},...
    'jmin',{0},...
    'sigma',{InParam.sigma}...
    );
    Param.q0=InParam.q0;
    Param.q1=InParam.q1;
    Param.v0=InParam.v0;
    Param.v1=InParam.v1;
    Param.jmax=InParam.jmax;
    Param.jmin=InParam.jmin;
    %%
    %计算(3.17)和(3.18)
    T1=sqrt(abs(InParam.v1-InParam.v0)/InParam.jmax);
    T2=InParam.amax/InParam.jmax;
    Tjs=min(T1,T2);
    if(T1<=T2)
        Dq=InParam.q1-InParam.q0;
        if(Dq<Tjs*(InParam.v0+InParam.v1))
            disp('位移过小,不存在满足始末速度的轨迹!程序退出。');
            return;
        end
    else
        Dq=InParam.q1-InParam.q0;
        if(Dq<0.5*(InParam.v0+InParam.v1)*(Tjs+abs(InParam.v1-InParam.v0)/InParam.amax))
            disp('位移过小,不存在满足始末速度的轨迹!程序退出。');
            return;
        end
    end
    %%
    %输入参数正确误(即轨迹存在),分类讨论
    if((InParam.vmax-InParam.v0)*InParam.jmax<InParam.amax^2)
        %(3.19)满足,amax不能达到
        Tj1=sqrt((InParam.vmax-InParam.v0)/InParam.jmax);
        Ta=2*Tj1;
        Param.alima=InParam.jmax*Tj1;
    else
        %(3.19)不满足,amax能达到
        Tj1=InParam.amax/InParam.jmax;
        Ta=Tj1+(InParam.vmax-InParam.v0)/InParam.amax;
        Param.alima=InParam.amax;
    end
    
    if((InParam.vmax-InParam.v1)*InParam.jmax<InParam.amax^2)
        %(3.20)满足,amin不能达到
        Tj2=sqrt((InParam.vmax-InParam.v1)/InParam.jmax);
        Td=2*Tj2;
        Param.alimd=-InParam.jmax*Tj2;
    else
         %(3.20)不满足,amin能达到
        Tj2=InParam.amax/InParam.jmax;
        Td=Tj2+(InParam.vmax-InParam.v1)/InParam.amax;
        Param.alimd=-InParam.amax;
    end
     
    %计算(3.25)
    Tv=(InParam.q1-InParam.q0)/InParam.vmax-Ta/2*(1+InParam.v0/InParam.vmax)-...
        Td/2*(1+InParam.v1/InParam.vmax);
    if(Tv>0)
        %case1,最大速度能达到
        Param.vlim=InParam.vmax;
        Param.Tj1=Tj1;
        Param.Ta=Ta;
        Param.Tj2=Tj2;
        Param.Td=Td; 
        Param.T=Ta+Tv+Td;
        Param.Tv=Tv;
        return;   
    else
       % case2,最大速度不能达到
       Tv=0;
       Param.Tv=Tv;
       %计算(3.26a),(3.27),(3.26b),(3.26c)
       Tj=InParam.amax/InParam.jmax;
       Tj1=Tj;
       Tj2=Tj;
       Delta=InParam.amax^4/InParam.jmax^2+2*(InParam.v0^2+InParam.v1^2)+InParam.amax*...
           (4*(InParam.q1-InParam.q0)-2*InParam.amax/InParam.jmax*(InParam.v0+InParam.v1));
       Ta=(InParam.amax^2/InParam.jmax-2*InParam.v0+sqrt(Delta))/(2*InParam.amax);
       Td=(InParam.amax^2/InParam.jmax-2*InParam.v1+sqrt(Delta))/(2*InParam.amax);
       if(Ta>2*Tj && Td>2*Tj)
           %加速段和减速段都能达到最大加速度
           Param.Tj1=Tj1;
           Param.Tj2=Tj2;
           Param.Ta=Ta;
           Param.Td=Td;
           Param.T=Ta+Tv+Td;
           Param.alima=InParam.amax;
           Param.alimd=-InParam.amax;
           Param.vlim=InParam.v0+(Ta-Tj1)*Param.alima;
           return;
       else
           %至少有一段不能达到最大加速度
           gamma=0.99;
           amax=InParam.amax;
           %逐渐减小最大加速度约束
           while(Ta<2*Tj || Td<2*Tj)
               if(Ta>0 && Td>0)
                   amax=gamma*amax;
                    %循环计算(3.26a),(3.27),(3.26b),(3.26c)
                   Tj=amax/InParam.jmax;
                   Tj1=Tj;
                   Tj2=Tj;
                   Delta=amax^4/InParam.jmax^2+2*(InParam.v0^2+InParam.v1^2)+amax*...
                       (4*(InParam.q1-InParam.q0)-2*amax/InParam.jmax*(InParam.v0+InParam.v1));
                   Ta=(amax^2/InParam.jmax-2*InParam.v0+sqrt(Delta))/(2*amax);
                   Td=(amax^2/InParam.jmax-2*InParam.v1+sqrt(Delta))/(2*amax);          
               else
                   %出现Ta或Td小于0
                   if(Ta<=0)
                       Ta=0;
                       Tj1=0;
                       %计算(3.28a)
                       Td=2*(InParam.q1-InParam.q0)/(InParam.v0+InParam.v1);
                       %计算(3.28b)
                       num=InParam.jmax*(InParam.q1-InParam.q0)-...
                           sqrt(InParam.jmax*(InParam.jmax*(InParam.q1-InParam.q0)^2+...
                           (InParam.v1+InParam.v0)^2*(InParam.v1-InParam.v0)));
                       den=InParam.jmax*(InParam.v1+InParam.v0);
                       Tj2=num/den;
                   elseif(Td<=0)
                       Td=0;
                       Tj2=0;
                       %计算(3.29a)
                       Ta=2*(InParam.q1-InParam.q0)/(InParam.v0+InParam.v1);
                       %计算(3.29b)
                        num=InParam.jmax*(InParam.q1-InParam.q0)-...
                           sqrt(InParam.jmax*(InParam.jmax*(InParam.q1-InParam.q0)^2-...
                           (InParam.v1+InParam.v0)^2*(InParam.v1-InParam.v0)));
                       den=InParam.jmax*(InParam.v1+InParam.v0);
                       Tj1=num/den;
                   end 
                   Param.Tj1=Tj1;
                   Param.Tj2=Tj2;
                   Param.Ta=Ta;
                   Param.Td=Td;
                   Param.T=Ta+Tv+Td;
                   Param.alima=InParam.jmax*Tj1;
                   Param.alimd=-InParam.jmax*Tj2;
                   Param.vlim=InParam.v0+(Ta-Tj1)* Param.alima;
                   return;             
               end   
           end
           Param.Tj1=Tj1;
           Param.Tj2=Tj2;
           Param.Ta=Ta;
           Param.Td=Td;
           Param.T=Ta+Tv+Td;
           Param.alima=InParam.jmax*Tj1;
           Param.alimd=-InParam.jmax*Tj2;
           Param.vlim=InParam.v0+(Ta-Tj1)* Param.alima;
           return;    
       end    
    end
    
    end
    

    算法测试及结果显示程序:SProfileTest.m

    %{
    %SProfileTest.m
    S型速度规划测试程序,2019.02.12,Brian
    (1)输入轨迹的边界条件及约束条件;
    (2)绘制速度规划的结果。
    %}
    clc
    clear
    close('all')
    %%
    %定义轨迹边界条件及约束条件
    q0=10;
    q1=0;
    v0=-7;
    v1=0;
    v_max=10;
    a_max=10;
    j_max=30;
    %%
    %调用算法子函数,进行速度规划,得到规划结果
    fun=CalcFun();
    Param=fun.CalcSProfile(q0,q1,v0,v1,v_max,a_max,j_max);
    %%
    %根据规划结果,调用算法子函数计算轨迹
    Ts=0.001;
    i=0;
    for t=0:Ts:Param.T
        i=i+1; 
        time(i)=i*Ts;
        dis(i)=fun.displacement(t,Param);
        vel(i)=fun.velocity(t,Param);
        acc(i)=fun.acceleration(t,Param);
        jerk(i)=fun.jerk(t,Param);
    end
    %%
    %绘图
    figure
    %位置
    subplot(4,1,1)
    plot(time,dis,'r','LineWidth',1.5);
    grid on
    ylabel('position')
    %速度
    subplot(4,1,2)
    plot(time,vel,'b','LineWidth',1.5);
    grid on
    hold on
    ylabel('velocity')
    v_ref1=v_max*ones(1,length(time));
    v_ref2=-v_max*ones(1,length(time));
    plot(time,v_ref1,'b--','LineWidth',1.5);
    plot(time,v_ref2,'b--','LineWidth',1.5);
    %加速度
    subplot(4,1,3)
    plot(time,acc,'g','LineWidth',1.5);
    grid on
    hold on
    ylabel('acceleration')
    a_ref1=a_max*ones(1,length(time));
    a_ref2=-a_max*ones(1,length(time));
    plot(time,a_ref1,'g--','LineWidth',1.5);
    plot(time,a_ref2,'g--','LineWidth',1.5);
    %加加速度
    subplot(4,1,4)
    plot(time,jerk,'m','LineWidth',1.5)
    grid on
    hold on
    ylabel('jerk')
    j_ref1=j_max*ones(1,length(time));
    j_ref2=-j_max*ones(1,length(time));
    plot(time,j_ref1,'m--','LineWidth',1.5);
    plot(time,j_ref2,'m--','LineWidth',1.5);
    %结束
    

    小结

    上述的S型速度规划参考文献[1]实现,算法实现的逻辑清晰,实现简单,可以应用在实际的数控系统中。但是其中存在两个缺点:

    (1)在case2中出现无法达到给定最大加速度时,通过减小最大加速度约束来规划轨迹,这使得所得的轨迹通常不是最优,即在指定的参数范围内,不是时间最短的轨迹。

    (2)在给定的位移较小,无法达到给定的始末速度时,即式(3.18)不成立,该算法不能进行处理。这需要用户进行额外的处理,重新给定参数,使得轨迹存在。

    特别是使用该算法进行速度前瞻时要处理好这两个问题,否则可能导致运动控制系统性能和稳定性下降。

    关于第(1)个问题,参考文献[2]-[4]有所涉及,综合这几篇文献,是可以解决缺点(1)的,使得规划的S曲线始终是最优,这当然会增加算法逻辑的复杂度。如何解决这两个问题,使得该算法始具有更好的性能和稳定性,且听下回分解。


    参考文献

    [1]Biagiotti L, Melchiorri C. Trajectory Planning for Automatic Machines and Robots[M]. Springer Berlin Heidelberg, 2009.

    [2]Erkorkmaz K, Altintas Y. High speed CNC system design. Part I: jerk limited trajectory generation and quintic spline interpolation[J]. International Journal of Machine Tools & Manufacture, 2001,41(9):1323-1345.

    [3]商允舜. CNC数控系统加减速控制方法研究与实现[D]. 浙江大学机械制造及自动化, 2006.

    [4]郭新贵, 李从心. S曲线加减速算法研究[J]. 机床与液压, 2002(05):60-62.

    [5]张碧陶, 高伟强, 沈列, 等. S曲线加减速控制新算法的研究[J]. 机床与液压, 2009,37(10):27-29.

    [6]何均, 游有鹏, 陈浩, 等. S形加减速的嵌套式前瞻快速算法[J]. 航空学报, 2010,31(4):842-851.

    展开全文
  • 车速控制, 规划车速变化轨迹, 通过反馈控制算法调整车速
  • dqdrvjjd.zip_速度规划

    2022-07-15 11:18:57
    一种流形学习算法(很好用),有小波分析的盲信号处理,计算加权加速度,构成不同频率的调制信号,利用matlab GUI实现的串口编程例子,是路径规划的实用方法。
  • 文章目录系列文章目录前言一、Double S曲线速度规划介绍1.介绍2.图例3.适合场景4.应用对象二、double S曲线原理及步骤1.系统参数定义2.给定曲线的系统参数(假设参数)3.给定曲线的边界参数4.曲线速度、加速度加加...

    系列文章目录

    提示:这里可以添加系列文章的所有文章的目录,目录需要自己手动添加
    TODO:写完再整理

    文章目录


    前言

    认知有限,望大家多多包涵,有什么问题也希望能够与大家多交流,共同成长!

    本文先对double S曲线进行机械臂轨迹和无人驾驶路径的速度都规划做个简单的介绍,具体内容后续再更,其他模块可以参考去我其他文章


    提示:以下是本篇文章正文内容

    路径-速度解耦规划

    路径-速度解耦规划将轨迹规划分为两步:路径规划和速度规划。

    路径规划:生成候选曲线,这是车辆可行驶的路径,使用代价函数对每条路径进行评估,该函数包括平滑度、安全性、与车道中心的偏离等等,按代价对路径进行排名,并选择代价最低的路径。

    速度规划:确定沿这条路径行进的速度,可以使用优化功能为路径选择受到各种限制约束的良好速度曲线

    一、Double S曲线速度规划介绍

    1.介绍

    Double S规划也叫S形曲线规划,S轨迹也称作七段轨迹曲线,因此主要由七段函数构成。,Point to Point的直线速度规划
    .

    2.图例

    位置曲线
    在这里插入图片描述
    速度曲线
    在这里插入图片描述
    加速度曲线
    在这里插入图片描述
    加加速度曲线
    在这里插入图片描述
    标记
    q0:起点
    q1:终点
    v0:初始速度
    v1:终点速度
    a0:初始加速度
    a1:终点加速度
    j0:初始加加速度
    j1:终止加加速度
    t0:开始时间【防盗标记–盒子君hzj】
    t1:终止时间
    T:总时间
    Vmax:最大速度
    amax:最大加速度
    jmax:最大加加速度
    .
    .

    3.适合场景

    直线段的梯形加减速的速度规划

    输出控制的速度、加速度、加加速度的指令(当然仅仅是输出其中一个也行,能控住其中一个,其他两个量也控制住了,主要是看机器人的底盘接收什么信号)【防盗标记–盒子君hzj】

    .

    4.应用对象

    机械臂常用的轨迹曲线主要是S曲线

    路径规划的速度规划【防盗标记–盒子君hzj】
    .

    二、double S曲线原理及步骤

    1.系统参数定义

    Tj1:在加速段中满足(jmax或者jmin)所需的时间周期时间
    Tj2:在减速段中满足(jmax或者jmin)所需的时间周期时间
    Ta:加速段周期
    Td:减速段周期【防盗标记–盒子君hzj】
    Tv:匀速时间
    T:整条曲线的总时间
    .
    .

    2.给定曲线的系统参数(假设参数)

    在这里插入图片描述
    通过给定最大速度、最大加速度、最大加加速度,通过合理改变曲线的速度、加速度、加加速度使得曲线位置在变化的时候更高效

    .
    .

    3.给定曲线的边界参数

    在这里插入图片描述
    .

    4.曲线速度、加速度加加速度变化分析及计算【通过控制时间实现】

    情况1 :曲线的终点大于起点,即曲线是向前进的(q1>q0)

    (1)判断double S曲线是否适合部署条件

    首先需要判断位移h=q1 -q0是否满足最大速度vmax、最大加速度amax、最大加加速度jmax 的限制,极限情况就是纯加速或者纯减速的情况【防盗标记–盒子君hzj】

    方法:
    定义曲线运行的总时间为:
    在这里插入图片描述

    如果在这里插入图片描述说明路径是可行的
    .
    .

    (2)满足最大速度的限制下,存在以下两种情况

    运动被分成三个阶段分析

    【按照加速度划分】
    加速段:加速度线性增加到amax,然后减小到0
    最大速度匀速段 :保持加速度为0
    减速段 : 加速度减小到-amax 然后增加到0【防盗标记–盒子君hzj】

    Case1:达到可以达到最大速度Vlim=Vmax

    在这里插入图片描述

    (1)先推测加速段、减速段时间

    【通过加速度推测】
    【加速段和减速段是对称的,时间也是一样的】

    1、判断加速度是否达到最大加速度(a=amax或者a=-amax)


    2、判断条件
    在这里插入图片描述
    就证明没有达到最大加速度amax


    若达到了最大加速度amax/amin
    加速段的时间公式【防盗标记–盒子君hzj】
    在这里插入图片描述
    减速段的时间公式
    在这里插入图片描述


    若未达到了最大加速度amax/amin

    加速段的时间公式
    在这里插入图片描述

    减速段的时间公式
    在这里插入图片描述


    控制输出
    【计算出时间和给定各个参数就可以计算】

    【加加速度控制】对加速度进行一次求导
    【加速度控制方法】加速度匀加速/匀减速Tj的时间(因为这时候加速度是线性量比较好控制)
    【速度控制方法】对加速度量进行一次积分
    【位置控制方法】对加速度进行两次积分

    (2)再推测出匀速段的时间

    【推测出是否达到最大速度】
    在这里插入图片描述
    若Tv>0, 说明我们实际达到了最大速度,反之则没能达到最大速度

    控制输出:
    【加加速度控制】对速度进行两次求导
    【加速度控制方法】对速度进行一次求导
    【速度控制】按照最大速度运行tv的时间(此时速度的控制量是线性的)
    【位置控制方法】对速度进行一次积分
    .
    .

    Case2:达到未可以达到最大速度Vlin < Vmax

    在这里插入图片描述

    (1)先推测出匀速段的时间

    在这个情况下,不存在匀速段,也就是匀速段的时间Tv = 0
    控制输出:没有这个阶段的控制【防盗标记–盒子君hzj】
    .
    .

    (2)再推测出加速段、减速段时间

    【通过加速度推测】
    【加速段和减速段是对称的,时间也是一样的】


    判断加速度是否达到最大加速度(a=amax或者a=-amax)


    判断条件
    在这里插入图片描述
    Ta:加速段周期
    Td:减速段周期
    如果 在这里插入图片描述,此时没能达到最大或者最小加速度【防盗标记–盒子君hzj】


    若达到了最大加速度amax,加/减速段的时间公式

    加速段和减速段是对称的,时间也是一样的
    加减速轨迹段的时间Tj公式:
    在这里插入图片描述


    若未达到了最大加速度amax,加/减速段的时间公式

    (1)有加速、减速的过程(Td>0且Ta>0时)
    由于加减速不一定刚好对分,这样的不确定因素对求解其中的参数会比较困难,这里通过引入变量delta,通过逐步缩减amax值的方法进行近似计算规划轨迹的加减速段的时间Ta,Td.【防盗标记–盒子君hzj】
    给定通过线性变化,慢慢减小a_max的值在这里插入图片描述

    这样就可以使用上面的公式计算时间,直到在这里插入图片描述

    加速段时间Ta,减速段时间Tb计算公式如下:
    在这里插入图片描述

    而在上述计算过程中,还可能会出现Ta,Td为负值的情况,这种情况下,则是该规划轨迹仅仅有加速段轨迹或者仅仅有减速度轨迹。【防盗标记–盒子君hzj】
    在这种情况下,速度给定的初始值V0和V1的关系:v0>v1是计算成立的一个必要条件
    .
    .
    (2)只有加速(v0>v1,Td<0时)
    在这里插入图片描述

    (3)只有减速(v0>v1,Ta<0时)
    在这里插入图片描述
    .
    .

    情况2 :曲线的起点大于终点,即曲线是向后退的(q0>q1)

    【通用的情况】

    (1)给定的曲线的位置、速度边界条件

    在这里插入图片描述
    .
    .

    (2)假设曲线方向

    在这里插入图片描述
    sign()函数功能是取某个数的符号(正或负): 当x>0,sign(x)=1;当x=0,sign(x)=0; 当x<0, sign(x)=-1
    在这里插入图片描述
    .
    .

    (3)把后退的方向转换到前进的方向,进而得到速度、加速度、加加速度的系统参数

    在这里插入图片描述

    (4)此时问题转化成了情况1,计算出结果之后再次转换,进而得到最终的位置、速度、加速度、加加速度控制输出

    在这里插入图片描述
    .
    .

    (4)控制输出

    加速段控制率

    【把对应时间计算出来就行】
    在这里插入图片描述

    匀速段控制率

    【把对应时间计算出来就行】
    在这里插入图片描述

    减速段控制率

    【把对应时间计算出来就行】
    在这里插入图片描述

    技巧
    在加/减速段用加速度控制,在匀速段用速度控制(因为这时候都是线性值)
    .
    .

    三、程序流程图

    在这里插入图片描述

    四、二维源代码示例

    在这里插入图片描述

    clc;clear all;
    %%
    %NOTE: 速度轨迹规划初步实现
    %① Vlimt=Vmax时,Tv>0%② Vlimt<Vmax时,Tv=0;Tv,velocity constant period
    %                1)加减速混合轨迹;
    %                2)仅加速轨迹 v0<v1;
    %                3)仅减速轨迹 v0>v1;
    
    %% 输入参数
    q0=0;q1=10;v0=0;v1=-5;
    vmax=5;amax=10;jmax=30;
    n=1000 %总点数
    
    %% 计算程序
    [ta,td,tv,tj1,tj2,vlim,alima,alimd,jmax]=myfunC(q0,q1,v0,v1,vmax,amax,jmax,8)
    duration=ta+td+tv;
    dt=duration/n;
    [uv,vel,acc,jerk]=DoubleS(n,q0,v0,q1,v1,ta,td,tv,tj1,tj2,vlim,alima,alimd,jmax,duration);
    
    %% 绘图
    myplot(uv,dt,duration,vel,vmax,acc,amax,jerk,jmax)
    
    %——————————————————————————————————————————————————————————————————%
    %% matlab 函数库
    %% 绘图
    function myplot(uv,dt,duration,vel,vmax,acc,amax,jerk,jmax)
    t=dt:dt:duration;
    subplot(4,1,1)
    plot(t,uv,'r-','linewidth',1.5);
    title('位移曲线');xlabel('时间t[s]');ylabel('位移[rad]');grid on;
    subplot(4,1,2)
    plot(t,vel,'b','linewidth',1.5);
    hold on;plot([0,duration],[vmax,vmax],'m--');plot([0,duration],[-vmax,-vmax],'g--');hold off;
    title('速度曲线');xlabel('时间t[s]');ylabel('速度[rad/s]');grid on;legend('vel','vmax','vmin')
    subplot(4,1,3)
    plot(t,acc,'g-','linewidth',1.5)
    hold on;plot([0,duration],[amax,amax],'m--');plot([0,duration],[-amax,-amax],'g--');hold off;
    title('加速度曲线');xlabel('时间t[s]');ylabel('加速度[rad/s^2]');grid on;legend('acc','amax','amin')
    subplot(4,1,4)
    plot(t,jerk,'k-.','linewidth',1.5)
    hold on;plot([0,duration],[jmax,jmax],'m--');plot([0,duration],[-jmax,-jmax],'g--');hold off;
    title('jerk曲线');xlabel('时间t[s]');ylabel('加加速[rad/s^3]');grid on;legend('jerk','jmax','jmin')
    
    end
    %%
    function  [uv,vel,acc,jerk]=DoubleS(n,q0,v0,q1,v1,ta,td,tv,tj1,tj2,vlim,alima,alimd,jmax,duration)
    uv=zeros(1,n);
    vel=zeros(1,n);
    acc=zeros(1,n);
    jerk=zeros(1,n);
    h=q1-q0;        %desired displacement h
    jmin=-jmax;% vlim
    % alimd=-alima;
    for l=1:1:n
        %t=(l*duration-1)/(n);
        t=duration*(l-1)/(n-1);
        % 1
        if t<=tj1
            uv(l)= q0 + v0*t + jmax/6*t^3;
            vel(l)=v0 + jmax/2*t^2;
            acc(l)=jmax*t;
            jerk(l)=jmax;
            % 2 (无匀速段时可省略)
        elseif  (t>tj1 )&&( t<ta-tj1)
            uv(l)= q0 + v0*t + alima/6*(3*t^2-3*tj1*t+tj1^2);
            vel(l)=v0 + alima*(t-tj1/2);
            acc(l)=alima;
            jerk(l)=0;
            % 3
        elseif  (t>ta-tj1) && (t<=ta )
            uv(l)= q0 +1.0/2*(vlim+v0)*ta - vlim*(ta-t) - 1.0/6*jmin*(ta-t)^3;
            vel(l)=vlim + 1.0/2*jmin*(ta-t)^2;
            acc(l)= - jmin*(ta-t);
            jerk(l)=jmin;
            % 4 ()
        elseif  (t>ta) && (t<=ta+tv )
            uv(l)= q0 + 1.0/2*(vlim+v0)*ta + vlim*(t-ta);
            vel(l)=vlim;
            acc(l)=0;
            jerk(l)=0;
            % 5
        elseif  (t>duration-td) &&  (t<=duration-td+tj2 )
            uv(l)= q1 - (vlim+v1)*td/2 + vlim*(t-duration+td) - jmax/6*(t-duration+td)^3;
            vel(l)=vlim - jmax/2*(t-duration+td)^2;
            acc(l)= - jmax*(t-duration+td);
            jerk(l)= - jmax;
            % 6
        elseif ( t>duration-td+tj2) && (t<=duration-tj2 )
            uv(l)= q1 - v1*(duration-t) + alimd/6*(3*(duration-t)^2-3*tj2*(duration-t)+tj2^2);
            vel(l)= vlim + alimd*(t-duration+td-tj2/2);
            acc(l)=- jmax*tj2;
            jerk(l)=0;
            % 7
        elseif  (t>duration-tj2) && (t<=duration )
            uv(l)= q1 - v1*(duration-t) - jmax/6*(duration-t)^3;
            vel(l)=v1 + jmax/2*(duration-t)^2;
            acc(l)=- jmax*(duration-t);
            jerk(l)=jmax;
        end
    end
    end
    
    %% 计算不同情况
    function[ta,td,tv,tj1,tj2,vlim,alima,alimd,jmax]=myfunC(q0,q1,v0,v1,vmax,amax,jmax,flag)
    if (vmax-v0)*jmax<amax^2    %amax is not reached
        tj1=sqrt((vmax-v0)/jmax)
        ta=2*tj1
    else                        %amax is  reached
        tj1=amax/jmax
        ta=tj1+(vmax-v0)/amax
    end
    
    if (vmax-v1)*jmax<amax^2    %amin is not reached
        tj2=sqrt((vmax-v1)/jmax)
        td=2*tj2
    else                       %amin is  reached
        tj2=amax/jmax
        td=tj2+(vmax-v1)/amax
    end
    
    tv=(q1-q0)/vmax-ta/2*(1+v0/vmax)-td/2*(1+v1/vmax)
    
    if tv>0
        vlim=vmax; % Case 1:vlimit=vmax
        alima=jmax*tj1
        alimd=-jmax*tj2
    elseif tv<0 % Case 2:vlimit<vmax
        tv=0
        tj1=amax/jmax
        tj2=tj1
        tj=tj1
        %     deat=amax^4/(jmax^2)+2*(v0^2+v1^2)+amax*(4*(q1-q0)-2*amax/jmax*(v0+v1))
    end
    
    switch flag
        case 0   %混合轨迹
            deat=amax^4/(jmax^2)+2*(v0^2+v1^2)+amax*(4*(q1-q0)-2*amax/jmax*(v0+v1))
            ta=(amax^2/jmax-2*v0+sqrt(deat))/(2*amax)%两个解
            %     ta=(amax^2/jmax-2*v0-sqrt(deat))/(2*amax)   %ta<0
            td=(amax^2/jmax-2*v1+sqrt(deat))/(2*amax)%两个解
            %     td=(amax^2/jmax-2*v1-sqrt(deat))/(2*amax)   %td<0
            
        case 1   %仅减速轨迹
            deat=amax^4/(jmax^2)+2*(v0^2+v1^2)+amax*(4*(q1-q0)-2*amax/jmax*(v0+v1))
            ta=(amax^2/jmax-2*v0-sqrt(deat))/(2*amax)   %ta<0
            td=(amax^2/jmax-2*v1+sqrt(deat))/(2*amax)%两个解
            
        case 2
            deat=amax^4/(jmax^2)+2*(v0^2+v1^2)+amax*(4*(q1-q0)-2*amax/jmax*(v0+v1))
            ta=(amax^2/jmax-2*v0+sqrt(deat))/(2*amax)%两个解
            td=(amax^2/jmax-2*v1-sqrt(deat))/(2*amax)   %td<0
        otherwise
            return;
    end
    
    vlim=(q1-q0)/td;
    alima=jmax*tj1;
    alimd=-jmax*tj2;
    vlim=v0+(ta-tj1)*alima;
    vlim=v1-(td-tj2)*alimd;
    
    %仅有加减速的情况
    if (ta<0 && v0>v1) % Case 2-1:ta<0 &v0>v1 只有减速段
        ta=0
        td=2*(q1-q0)/(v1+v0)
        tj1=0
        tj2=(jmax*(q1-q0)-sqrt(jmax*(jmax*(q1-q0)^2+(v1+v0)^2*(v1-v0))))/(jmax*(v1+v0))
        tv=0
        
        alima=jmax*tj1
        alimd=-jmax*tj2
        % vlim=v0+(ta-tj1)*alima
        vlim=v1-(td-tj2)*alimd
    end
    if (td<0 && v0<v1) % Case 2-1:td<0 &v0<v1 只有加速段
        tv=0
        td=0
        ta=2*(q1-q0)/(v1+v0)
        tj2=0
        tj1=(jmax*(q1-q0)-sqrt(jmax*(jmax*(q1-q0)^2-(v1+v0)^2*(v1-v0))))/(jmax*(v1+v0))
        
        alima=jmax*tj1
        alimd=-jmax*tj2
        vlim=v0+(ta-tj1)*alima
        %         vlim=v1-(td-tj2)*alimd
    end
    end
    

    五、三维的源代码解析示例

    double S曲线的类及其参数

    class DoubleSTrajectory3d {
     public:
      DoubleSTrajectory3d();
    
      // 调用的API
    //【核心--曲线计算】给定起始点、目标点、起始速度、终止速度,
    // 计算各个周期时间和控制率(加速段加速度a_lima_、减速段加速度a_limd_、匀速段速度v_lim_)
      bool setPath(const common::NavGoal3D& _start, const double& _initial_vel,  
                     const common::NavGoal3D& _target, const double& _final_vel);
    
    //获取double S曲线的运行总时间
      double getTime();   
    
    //【核心--调用】输入当前时间,获取曲线的当前位置点,曲线调用的方式和B样条如出一辙                                
      bool evaluate(double time, Eigen::Vector2d* current_point);               
    
     private:
      double t_;
      double a_max_;
      double a_lima_; //加速段加速度
      double a_limd_; //减速段加速度
      double v_max_;
      double v_lim_;  //匀速段速度
      double j_max_;
    
      double t_j1_;
      double t_j2_;
      double t_a_;
      double t_v_;
      double t_d_;
    
      Vector3d start_;
      Vector3d end_;
      Vector3d direction_vector_;
    
      double v0_;
      double v1_;
      double s_;
    };
    

    double S曲线计算【单段曲线】

    //【核心--曲线计算】给定起始点、目标点、起始速度、终止速度,
    // 计算各个周期时间和控制率(加速段加速度a_lima_、减速段加速度a_limd_、匀速段速度v_lim_)
    DoubleSTrajectory3d::DoubleSTrajectory3d() {}
    bool DoubleSTrajectory3d::setPath(NavGoal3D& start,
                                      initial_vel,
                                      NavGoal3D& target,
                                      const double& final_vel) {
      //(1)初始化参数
      start_ = start.position;
      end_ = target.position;
      v0_ = initial_vel;
      v1_ = final_vel;
      s_ = (start_ - end_).norm();//计算起点和终点的位移
      if (s_ <= 1e-4) {           //起点和终点的位移过短,退出
        t_j1_ = 0.0;
        t_j2_ = 0.0;
        t_a_ = 0.0;
        t_v_ = 0.0;
        t_d_ = 0.0;
        t_ = t_a_ + t_v_ + t_d_;
        return true;
      }
      v_max_ = target.max_speed;                  //设置速度
      a_max_ = target.max_acceleration;           //设置加速度
      j_max_ = DOUBLE_S_MAX_JERK;                 //设置加加速度
      direction_vector_ = (end_ - start_) / s_;   //计算三维方向向量
      
      //(2)计算double S曲线
      double d = v1_ - v0_;   //计算起点和终点的速度差值
    
      //1)判断double S曲线是否适合部署条件check feasibility
      //即判断位移h=q1 -q0是否满足最大速度vmax、最大加速度amax、最大加加速度jmax 的限制。tj = cri2才符合
      double cri1 = std::sqrt(std::fabs(v1_ - v0_) / j_max_);
      double cri2 = a_max_ / j_max_;
      double tj = std::min(cri1, cri2);
      if (tj < cri2) {
        if (s_ < (tj * (v1_ + v0_))) {
          return false;
        }
      } else {
        if (s_ < 0.5 * (v0_ + v1_) * (tj + std::fabs(v1_ - v0_) / a_max_)) {
          return false;
        }
      }
    
    
      //2)根据七段控制率开始计算曲线(加速阶段)
      // case1:假设 v_lim == v_max
      //加速段
      if ((v_max_ - v0_) * j_max_ < a_max_ * a_max_) {  // a_max not reached
        //若未达到了最大加速度amax,加速段的时间公式
        t_j1_ = std::sqrt((v_max_ - v0_) / j_max_);
        t_a_ = 2 * t_j1_;
      } else {
        //若达到了最大加速度amax,加速段的时间公式
        t_j1_ = a_max_ / j_max_;
        t_a_ = t_j1_ + (v_max_ - v0_) / a_max_;
      }
    
      //减速段
      if ((v_max_ - v1_) * j_max_ < a_max_ * a_max_) {  // a_min is not reached
        //若未达到了最大加速度amin,减速段的时间公式
        t_j2_ = std::sqrt((v_max_ - v1_) / j_max_);
        t_d_ = 2 * t_j2_;
      } else {
        //若达到了最大加速度amin,减速段的时间公式
        t_j2_ = a_max_ / j_max_;
        t_d_ = t_j2_ + (v_max_ - v1_) / a_max_;
      }
    
      //匀速段
      //计算时间公式
      t_v_ = s_ / v_max_ - 0.5 * t_a_ * (1 + v0_ / v_max_) - 0.5 * t_d_ * (1 + v1_ / v_max_);
    
      if (t_v_ > 0) { //若Tv>0, 说明我们实际达到了最大速度,反之则没能达到最大速度
        t_ = t_a_ + t_v_ + t_d_;    //总时间=加速段时间+匀速段时间+减速段时间
    
        //计算控制率输出
        a_lima_ = j_max_ * t_j1_;               //加速段加速度
        a_limd_ = -1 * j_max_ * t_j2_;          //减速段加速度
        v_lim_ = v0_ + (t_a_ - t_j1_) * a_lima_;//匀速速度
    
        return true;
      }
    /
    //case2:假设 v_lim < v_max
      // if not retured at this point, it means v_lim < v_max
      while (1) {
        //1)计算判断加速度是否达到最大加速度(a=amax或者a=-amax)的公式
        t_j1_ = a_max_ / j_max_;
        t_j2_ = t_j1_;
        double& t_j = t_j1_;
        double delta = std::pow(a_max_, 4) / std::pow(j_max_, 2) + 2 * (v0_ * v0_ + v1_ * v1_) +
                        a_max_ * (4 * s_ - 2 * a_max_ * (v0_ + v1_) / j_max_);
        t_a_ = 0.5 * (a_max_ * a_max_ / j_max_ - 2 * v0_ + std::sqrt(delta)) / a_max_;
        t_d_ = 0.5 * (a_max_ * a_max_ / j_max_ - 2 * v1_ + std::sqrt(delta)) / a_max_;
    
        //2)判断此时没能达到最大或者最小加速度,加/减速段的时间公式
        if (t_a_ > 2 * t_j && t_d_ > 2 * t_j) { 
          t_v_ = 0;
          t_ = t_a_ + t_d_;
    
          //计算控制率输出
          a_lima_ = j_max_ * t_j1_;
          a_limd_ = -1 * j_max_ * t_j2_;
          v_lim_ = v0_ + (t_a_ - t_j1_) * a_lima_;
          return true;
        }
    
        //3)若未达到了最大加速度amax,减速段的时间公式
        if (t_a_ < 0) {
          t_d_ = 2 * s_ / (v0_ + v1_);
          t_a_ = 0;
          t_j1_ = 0;
          t_j2_ = (j_max_ * s_ - std::sqrt(j_max_ * (j_max_ * s_ * s_ + std::pow(v1_ + v0_, 2) * d))) / (j_max_ * (v1_ + v0_));
          t_v_ = 0;
          t_ = t_a_ + t_v_ + t_d_;
    
          //计算控制率输出
          a_lima_ = j_max_ * t_j1_;
          a_limd_ = -1 * j_max_ * t_j2_;
          v_lim_ = v0_ + (t_a_ - t_j1_) * a_lima_;
          return true;
        }
    
        //3)若未达到了最大加速度amax,加速段的时间公式
        if (t_d_ < 0) {
          t_a_ = 2 * s_ / (v0_ + v1_);
          t_d_ = 0;
          t_j2_ = 0;
          t_j1_ = (j_max_ * s_ - std::sqrt(j_max_ * (j_max_ * s_ * s_ - std::pow(v1_ + v0_, 2) * d))) / (j_max_ * (v1_ + v0_));
          t_v_ = 0;
          t_ = t_a_ + t_v_ + t_d_;
    
          a_lima_ = j_max_ * t_j1_;
          a_limd_ = -1 * j_max_ * t_j2_;
          v_lim_ = v0_ + (t_a_ - t_j1_) * a_lima_;
          return true;
        }
       
        a_max_ *= 0.9;
      }
      return true;                                    
    }
    
    

    【核心–调用】输入当前时间,获取曲线的当前位置点,曲线调用的方式和B样条如出一辙

    bool DoubleSTrajectory3d::evaluate(double t, Eigen::Vector2d* current_point) {
      if (t < 0) {
        t = 0;
      }
      if (t > t_) {
        t = t_;
      }
      
      double s;
      double v;
      // 加速段,计算速度和位置
      if ( t <= t_j1_) {  //
        s = v0_ * t + j_max_ * std::pow(t, 3) / 6.0;
        v = v0_ + 0.5 * j_max_ * std::pow(t, 2);
      } 
      else if (t <= t_a_ - t_j1_) {
        s = v0_ * t + a_lima_ * (3 * std::pow(t, 2) - 3 * t_j1_ * t + std::pow(t_j1_, 2)) / 6.0;
        v = v0_ + a_lima_ * (t - 0.5 * t_j1_);
      } 
      else if (t <= t_a_) {
        s = 0.5 * (v_lim_ + v0_) * t_a_ - v_lim_ * (t_a_ - t) + j_max_ * std::pow(t_a_ - t, 3) / 6.0;
        v = v_lim_ - 0.5 * j_max_ * std::pow(t_a_ - t, 2);
      } 
      //匀速段,计算速度和位置
      else if (t <= t_a_ + t_v_) { 
        s = 0.5 * (v_lim_ + v0_) * t_a_ + v_lim_ * (t - t_a_);
        v = v_lim_;
      } 
      //减速段,计算速度和位置
      else if (t <= t_ - t_d_ + t_j2_) {
        s = s_ - 0.5 * (v_lim_ + v1_) * t_d_ + v_lim_ * (t - t_ + t_d_) - j_max_ * std::pow(t - t_ + t_d_, 3) / 6.0;
        v = v_lim_ - 0.5 * j_max_ * (t - t_ + t_d_) * (t - t_ + t_d_);
      } 
      else if (t <= t_ - t_j2_) {
        double temp = t - t_ + t_d_;
        s = s_ - 0.5 * (v_lim_ + v1_) * t_d_ + v_lim_ * temp + a_limd_ * (3 * std::pow(temp, 2) - 3 * t_j2_ * temp + std::pow(t_j2_, 2)) / 6.0;
        v = v_lim_ + a_limd_ * (temp - 0.5 * t_j2_);
      } 
      else {
        s = s_ - v1_ * (t_ - t) - j_max_ * std::pow(t_ - t, 3) / 6.0;
        v = v1_ + 0.5 * j_max_ * std::pow(t_ - t, 2);
      }
      //因为位置和速度是二维的,用三维向量计算出某一个方向的分量
      Eigen::Vector3d p = start_ + s * direction_vector_;
      Eigen::Vector3d vp = v * direction_vector_;
      //位置
      current_point[0][0] = p[0];
      current_point[1][0] = p[1];
      current_point[2][0] = p[2];
      //速度
      current_point[0][1] = vp[0];
      current_point[1][1] = vp[1];
      current_point[2][1] = vp[2];
      return true;
    }
    

    获取double S曲线的运行总时间

    double DoubleSTrajectory3d::getTime() {
      return t_;
    }
    

    总结

    (1)double S 曲线怎么从二维扩展到三维?

    (1)方法一:进行降维处理

    (2)方法二:不会进行降维处理,三维计算曲线,最后面用一个三维向量计算出xyz方向的分量

    .

    (2)单段double S 曲线的计算于调用方法

    double S 曲线输入的是曲线的边界条件,按照加速段、匀速段、减速段的周期计算出控制率(速度和加速度),通过输入 时间点就可以获取到曲线具体的位置、速度、加速度、加加速度信息【防盗标记–盒子君hzj】
    .

    (3)多段double S 曲线(全局航线)的计算于调用方法

    把全部曲线划分为一段一段进行double曲线规划的,方法是把航线划分为一小段一小段先进行double S曲线的拟合,在把每一小段拟合好的曲线push到一个wps_list里面,从而wps_list就表征了拟合好的全局航线

    .

    参考资料

    https://zhuanlan.zhihu.com/p/267501745

    https://blog.csdn.net/easy_R/article/details/103444453

    《Trajectory Planning for Automatic Machine and Robots》
    【防盗标记–盒子君hzj】

    展开全文
  • 文章目录1 无人驾驶的动作规划2 轨迹规划2.1 车辆模型2.2 道路定义2.3 候选轨迹生成2.4 根据轨迹点构建有向图并搜索3 速度规划3.1 S-T图算法3.2 设置Cost 1 无人驾驶的动作规划 动作规划的目的:是将上游行为决策...

    1 无人驾驶的动作规划

    动作规划的目的:是将上游行为决策(behavior)的宏观指令解释成一条带有时间信息的轨迹曲线(trajectory),最后给底层的反馈控制。

    一种简单的思路是,将动作规划问题拆分成两个问题:

    1. 轨迹规划(Trajectory Planning):二维平面上的优化轨迹问题。
    2. 速度规划(Speed Planning):选定轨迹后,用什么样的速度行驶这条轨迹。

    分开优化不一定能保证总体最优解,但在实际工程实践中,分开优化是更实际有效的解决方案。


    2 轨迹规划

    2.1 车辆模型

    车辆的姿态向量为 x ‾ = ( x , y , θ , κ , υ ) \overline{x}=(x,y,\theta,\kappa,\upsilon) x=(x,y,θ,κ,υ)

    符号含义
    ( x , y ) (x,y) (x,y)车辆在二维片面内的位置
    θ \theta θ车辆的朝向
    κ \kappa κ曲率,即朝向 θ \theta θ的变化率
    υ \upsilon υ车辆的速度,即轨迹任意点的切线速度

    这些姿态变量满足以下关系:
    x ˙ = v cos ⁡ θ y ˙ = v sin ⁡ θ θ ˙ = v κ \begin{aligned} \dot{x} &amp;=v \cos \theta \\ \dot{y} &amp;=v \sin \theta \\ \dot{\theta} &amp;=v \kappa \end{aligned} x˙y˙θ˙=vcosθ=vsinθ=vκ
    轨迹相对于车辆姿态的系统关系为:
    d x / d s = cos ⁡ ( θ ( s ) ) d y / d s = sin ⁡ ( θ ( s ) ) d θ / d s = κ ( s ) \begin{array}{l}{\mathrm{d} x / \mathrm{d} s=\cos (\theta(s))} \\ {\mathrm{d} y / \mathrm{d} s=\sin (\theta(s))} \\ {\mathrm{d} \theta / \mathrm{d} s=\kappa(s)}\end{array} dx/ds=cos(θ(s))dy/ds=sin(θ(s))dθ/ds=κ(s)

    2.2 道路定义

    轨迹规划算法非常依赖于地图中对于道路中心线(Center Line)的定义,用一个采样函数去定义道路,采样函数为 r ( s ) = [ r x ( s ) , r y ( s ) , r θ ( s ) , r κ ( s ) ] r(s)=\left[r_{x}(s), r_{y}(s), r_{\theta}(s), r_{\kappa}(s)\right] r(s)=[rx(s),ry(s),rθ(s),rκ(s)],车辆姿态点和道路坐标系以及道路采样函数的关系满足:
    x r ( s , l ) = r x ( s ) + l cos ⁡ ( r θ ( s ) + π / 2 ) y r ( s , l ) = r y ( s ) + l sin ⁡ ( r θ ( s ) + π / 2 ) θ r ( s , l ) = r θ ( s ) κ r ( s , l ) = ( r κ ( s ) − 1 − l ) − 1 \begin{array}{l}{x_{r}(s, l)=r_{x}(s)+l \cos \left(r_{\theta}(s)+\pi / 2\right)} \\ {y_{r}(s, l)=r_{y}(s)+l \sin \left(r_{\theta}(s)+\pi / 2\right)} \\ {\theta_{r}(s, l)=r_{\theta}(s)} \\ {\kappa_{r}(s, l)=\left(r_{\kappa}(s)^{-1}-l\right)^{-1}}\end{array} xr(s,l)=rx(s)+lcos(rθ(s)+π/2)yr(s,l)=ry(s)+lsin(rθ(s)+π/2)θr(s,l)=rθ(s)κr(s,l)=(rκ(s)1l)1

    符号含义
    s s s纵向位移,道路中心线切线方向位移
    l l l横向位移

    假设对于某条道路Lane(k),纵向宽度 l k l_k lk不变。那么该道路可以标示为一个随着中心线横向位移的点集 { p ( s , l k ) : s ∈ R + } \left\{p\left(s, l_{k}\right) : s \in \boldsymbol{R}^{+}\right\} {p(s,lk):sR+},这样的一个坐标系统为SL坐标系统。
    在这里插入图片描述

    2.3 候选轨迹生成

    首先,定义车辆的轨迹(Trajectory)为一个从[0,1]区间到车辆姿态向量集合 C = { x ⃗ } C=\{\vec{x}\} C={x }的连续映射: ρ : [ 0 , 1 ] → C \rho :[0,1] \rightarrow C ρ:[0,1]C,或者理解为一系列车辆姿态的序列。

    符号含义
    ρ 1 ( 0 ) \rho_{1}(0) ρ1(0)第一条道路的起点姿态
    ρ 1 ( 1 ) \rho_{1}(1) ρ1(1)第一条道路的终点姿态
    q init q_{\text {init}} qinit广义上的,初始姿态向量, q  init  = ( x I , y I , θ l , κ l ) q_{\text { init }}=\left(x_{I}, y_{I}, \theta_{l}, \kappa_{l}\right) q init =(xI,yI,θl,κl)
    q e n d q_{\mathrm{ end}} qend广义上的,目标姿态向量, q g o a l = ( x G , y G , θ G , k G ) q_{\mathrm{goal}}=\left(x_{G}, y_{G}, \theta_{G}, k_{G}\right) qgoal=(xG,yG,θG,kG)

    轨迹优化的目标,就是在所有可能轨迹中,筛选出满足边界条件的轨迹曲线,再寻找一条最平滑,Cost最低的曲线。候选轨迹曲线用类似路由寻径模块中的“撒点”采样生成,这样,一条轨迹就可以看成是沿着Lane的中心线纵向位移方向,连接不同Trajectory Point的平滑曲线。
    在这里插入图片描述

    这里,采用多项式螺旋线连接轨迹点,生成候选曲线。多项式螺旋线,代表了一类曲率用弧长(对应这里的s)多项式函数来表示的曲线簇。一般使用三阶或者五阶多项式螺旋线,曲率 κ \kappa κ和轨迹弧长 s s s的关系为:
    三 阶 : k ( s ) = k 0 + k 1 s + k 2 s 2 + k 3 s 3 三阶:k(s)=k_{0}+k_{1} s+k_{2} s^{2}+k_{3} s^{3} k(s)=k0+k1s+k2s2+k3s3

    五 阶 : k ( s ) = k 0 + k 1 s + k 2 s 2 + k 3 s 3 + k 4 s 4 + k 5 s 5 五阶:k(s)=k_{0}+k_{1} s+k_{2} s^{2}+k_{3} s^{3}+k_{4} s^{4}+k_{5} s^{5} k(s)=k0+k1s+k2s2+k3s3+k4s4+k5s5

    三阶和五阶的区别为:三阶多项式会导致曲率的二阶导数 d κ 2 / d s 2 \mathrm{d} \kappa^{2} / \mathrm{d} s^{2} dκ2/ds2(对应方向盘转速)不连续,而五阶可以同事保持曲率的一阶导数和二阶导数的连续性。
    在这里插入图片描述

    最后,最优利用梯度下降方法搜索,得到三阶(五阶)螺旋线连接的轨迹。

    2.4 根据轨迹点构建有向图并搜索

    在车辆模型,道路模型,轨迹点模型,和多项式螺旋曲线的设定下,轨迹规划问题可以简化为: ∣ l  total  / Δ l ∣ × ∣ s  total  / Δ s ∣ \left|l_{\text { total }} / \Delta l\right| \times\left|s_{\text { total }} / \Delta s\right| l total /Δl×s total /Δs个轨迹点连接成的 ∣ l  total  / Δ l ∣ ∣ s  ual  / Δ r ∣ \left|l_{\text { total }} / \Delta l\right|^{\left|s_{\text { ual }} / \Delta r\right|} l total /Δls ual /Δr条候选曲线的搜索问题。

    所有的轨迹点构成了一个图 G = ( V , E ) G=(V, E) G=(V,E)

    符号含义
    l t o t a l l_{total} ltotal道路总宽度
    Δ l \Delta l Δl宽度的最小采样长度
    s t o t a l s_{total} stotal道路总长度
    Δ s \Delta s Δs长度的最小采样长度
    V V V轨迹点, v ∈ V , v = ( x , y , s , l ) v \in V, v=(x, y, s, l) vV,v=(x,y,s,l)
    E E E三阶(五阶)多项式螺旋曲线

    3 速度规划

    3.1 S-T图算法

    速度规划的任务,是在动作规划的轨迹上,考虑下游执行限制和行为决策结果,在每个轨迹点上,加入速度和加速度信息。

    速度规划主要考虑对动态障碍物的规避,这里引入S-T(纵向位移 - 时间)的概念,并把无人车的速度规划问题,抽象到在S-T图上搜索的问题进行求解。

    任何一个S-T图都基于一条已经给定的轨迹曲线,根据预测模块对动态障碍物的轨迹预测,每个动态障碍物都会在这条给定的轨迹上有所投影,从而产生一定S-T区域的覆盖,这里举一个S-T图速度优化算法的例子。
    在这里插入图片描述

    图中,无人车正在思考轨迹规划要选取的换道轨迹,目标车道有前车b,后车a,则a和b在轨迹上的投影就是平行于s轴的线段,随着t的增加,这个投影平行四边形也会不断向右延伸。

    模仿轨迹规划的地图划分,把地图分成小网格(Lattice Grid),对每个网格赋予Cost,最终速度规划问题就可以归纳为:从起点 ( s = 0 , t = 0 ) (s=0,t=0) (s=0,t=0)到终点 ( s = S e n d , t &gt; 0 ) (s=S_{end},t&gt;0) (s=Send,t>0)在网格上寻找最小路径Cost的搜索问题。

    图中3种方案分别为:

    方案说明
    Speed Plan 3加速直接超过a和b
    Speed Plan 2超过a(Overtake),然后让b先过(Yield)
    Speed Plan 1让车a和b,一直跟在后面

    3.2 设置Cost

    根据决策信息,可以灵活调整障碍物周边的Cost,来达到速度调整的目的。例如,

    上游决定对a超车,就在S-T图上将a运动轨迹上方网格的Cost调小;
    上游决定对b让车,就在S-T图上将b运动轨迹下方网格的Cost调小;
    为了避免碰撞,所有动态障碍物轨迹经过的网格Cost都需要调大。

    因此,设置Cost是速度规划中S-T图算法的关键,在这个基础上,利用A*或者Dijkstra搜索算法,就能得到最小Cost轨迹。在得到了轨迹之后,通过计算任意轨迹点处的一阶导数和二阶倒数,就能计算出速度和加速度,从而完成速度规划的计算。


    参考文章

    《第一本无人驾驶技术书》刘少山

    在Typora中输入公式

    Typora开启行内公式

    常用数学符号的 LaTeX 表示方法

    一份不太简短的LATEX2ε介绍【精华】

    展开全文
  • 针对一类严格反馈形式的非线性二阶多输入多输出系统,提出一种带有加速度规划的输出跟踪动态控制策略.引入一个代替时间变量的路径参数用以规划路径跟踪时的加速度,回避了设计内环加速度控制回路的常规方法,简化了...
  • 计算加权加速度,微分方程组数值解方法,是路径规划的实用方法。
  • 柔性加减速规划,主要目的是保证加减速期间能够实现加加速度的连续
  • S形速度规划算法

    万次阅读 2020-01-14 19:19:41
    S形速度规划相对于梯形速度规划其速度曲线会更加平滑,电机运行会更加平稳。常见的S曲线包括7段式加速度曲线,这种曲线计算量大,而且规划起来困难。一种简单的方法是采用Sigmoid对称函数加减速曲线规划法。典型的...
  • 文章目录一、7种速度规划对比1、梯形速度规划2、余弦速度规划3、多项式速度规划4、7段S型速度规划5、7段修正S型速度规划6、15段S型速度规划7、31段S型速度规划二、7种速度规划示例演示 一、7种速度规划对比   一般...
  • 单轴任意速度规划PVT_单轴_PVT规划_pvt_pvt速度规划_Singlemotion_源码.zip
  • S形曲线速度规划--5段式

    千次阅读 2020-05-15 13:32:09
    加加速度 j(t)={J0≤t≤t1−Jt1≤t≤t20t2≤t≤t3−Jt3≤t≤t4Jt4≤t≤t5 j(t)=\begin{cases} J & 0\leq t \leq t_1\\ -J & t_1\leq t \leq t_2\\ 0 & t_2\leq t \leq t_3\\ -J & t_3\leq t \leq ...
  • 归一化5次多项式速度规划

    千次阅读 2020-03-07 21:51:40
      给定插补总长度LLL,速度限制vlimv_{lim}vlim​,加速度限制alima_{lim}alim​,计算总运动时间TTT,规划归一化5次多项式s(u)s(u)s(u) (u=t/Tu=t/Tu=t/T为归一化变量),使其满足以下条件: {s(0)=0s′(0)=0s′′...
  • apollo学习之—planning理论到实践(1)综合 apollo学习之—planning理论到实践(2)路径规划 apollo学习之—planning理论到实践(3)路径规划 apollo学习之—planning理论到实践(4)—速度规划 文章目录 1、参考...
  • 简要说明 支持起末速度、加速度的...《(一)双S型速度规划 原理 》链接 关于最终的效果可以参考另一篇: 《(二)双S型速度规划 曲线形状的讨论》链接 最后,本人技术水平有限,如有(肯定有)问题随时私信我。 链接 ...
  • 为了实现数控系统连续轨迹的速度规划,本文提出了混合进给速度的规划方法。此方法通过建立转弯处加工路径和加工误差的数学模型,实现了转弯处进给速度的控制。实验结果表明,这种方法不仅可以极大的提高机床的加工...