加速度连续的s型加减速算法程序

该博客详细介绍了加速度连续的7段S型加减速规划算法,包括加加速度、匀加速、减加速、匀速、加减速、匀减速和减减速等阶段的数学模型和程序实现。算法考虑了速度限制、加速度限制和加加速度限制,并通过Qt Creator进行仿真验证。

在这里插入图片描述
此算法为加速度连续的7段s型加减速规划实现,理论参考:https://blog.csdn.net/Septembernine/article/details/53125828

(1)加加速段
加加速度为 jmaxjmax,加速度线性增加至设定值或最大值amaxamax。
(2)匀加速段
加加速度为0,加速度恒定。
(3)减加速段
当速度接近设定的值或最大值vmaxvmax时,加加速度突变为反向的jmaxjmax,进入加速度线性减小的变减速运动阶段。
(4)匀速段
当速度增至vmaxvmax后,加加速和加速度均变为0,进入匀速运动阶段。
(5)加减速段
加加速度突变为反向的jmaxjmax,加速度反向线性增加至−amax−amax。
(6)匀减速段
加加速度和为0,减加速度恒定。
(7)减减速段
加速度突变为jmaxjmax, 加速度由负向的amaxamax线性减小至0。

程序实现如下:

int sign(float q0, float q1);
// S曲线参数计算
void STrajectoryPara(float q0, float q1, float v0, float v1, float vmax, float amax, float jmax);
// 计算位置
float S_position(float);
// 计算速度
float S_velocity(float t);
// 计算加速度
float S_acceleration(float t);
// 计算加加速度
float S_jerk(float t);

int sSpeed::sign(float q0, float q1)
{
    int a = 0;
    if (q1 - q0 > 0)
        a = 1;
    else
        a = -1;
    return a;
}

void sSpeed::STrajectoryPara(float q0, float q1, float v0, float v1, float vmax, float amax, float jmax)
{
    float vmin, amin, jmin;
    int sigma = 0;
    float Tj = 0, delta = 0;
    vmin = -vmax;
    amin = -amax;
    jmin = -jmax;

    // 計算实际的q_0、q_1、v_max、a_max
    sigma = sign(q0, q1);
    Input_data.q0 = sigma * q0;
    Input_data.q1 = sigma * q1;
    Input_data.v0 = sigma * v0;
    vmax = ((sigma+1)/2)*vmax + ((sigma-1)/2)*vmin;
    vmin = ((sigma+1)/2)*vmin + ((sigma-1)/2)*vmax;
    Input_data.amax = ((sigma+1)/2)*amax + ((sigma-1)/2)*amin;
    Input_data.amin = ((sigma+1)/2)*amin + ((sigma-1)/2)*amax;
    Input_data.jmax = ((sigma+1)/2)*jmax + ((sigma-1)/2)*jmin;
    Input_data.jmin = ((sigma+1)/2)*jmin + ((sigma-1)/2)*jmax;

    // 判断是否达到最大速度
    if ((vmax - Input_data.v0)*Input_data.jmax < pow(Input_data.amax, 2)){
        // 达不到amax
        Input_data.Tj1 = sqrt((vmax - Input_data.v0)/Input_data.jmax);
        Input_data.Ta = 2*Input_data.Tj1;
        Input_data.alima = Input_data.jmax * Input_data.Tj1;
    }else{
        // 能够达到amax
        Input_data.Tj1 = Input_data.amax/Input_data.jmax;
        Input_data.Ta = Input_data.Tj1 + (vmax - Input_data.v0 )/Input_data.amax;
        Input_data.alima = Input_data.amax;
    }

    if ((vmax - Input_data.v1)*Input_data.jmax < pow(Input_data.amax, 2)){
        // 达不到amin
        Input_data.Tj2 = sqrt((vmax - Input_data.v1)/Input_data.jmax);
        Input_data.Td = 2 * Input_data.Tj2;
        Input_data.alimd = -Input_data.jmax * Input_data.Tj2;
    }else{
        // 能够达到amin
        Input_data.Tj2 = Input_data.amax/Input_data.jmax;
        Input_data.Td = Input_data.Tj2 + (vmax - Input_data.v1)/Input_data.amax;
        Input_data.alimd = -Input_data.amax;
    }
    // 计算匀速段时间
    Input_data.Tv = (Input_data.q1 - Input_data.q0)/vmax - (Input_data.Ta/2)*(1 + Input_data.v0/vmax)
            - (Input_data.Td/2)*(1 + Input_data.v1/vmax);

    // 对Tv进行讨论
    if (Input_data.Tv > 0)
        // 能够达到给定的最大速度vmax, 即存在匀速阶段
        Input_data.vlim = vmax;
    else{
        // 达不到最大速度,即匀速阶段Tv=0
        // 假设最大加速度和最小加速度均能达到
        Input_data.Tv = 0;
        Tj = Input_data.amax / Input_data.jmax;
        Input_data.Tj1 = Tj;
        Input_data.Tj2 = Tj;
        delta = (pow(Input_data.amax, 4)/pow(Input_data.jmax, 2)) + 2*(pow(Input_data.v0, 2) + pow(Input_data.v1, 2))
                + Input_data.amax*(4*(Input_data.q1 - Input_data.q0) - 2*(Input_data.amax/Input_data.jmax)*(Input_data.v0 + Input_data.v1));
        Input_data.Ta = ((pow(Input_data.amax, 2)/Input_data.jmax) - 2*Input_data.v0 + sqrt(delta)) / (2*Input_data.amax);
        Input_data.Td = ((pow(Input_data.amax, 2)/Input_data.jmax) - 2*Input_data.v1 + sqrt(delta)) / (2*Input_data.amax);
        // 对Ta和Td进行讨论
        if (Input_data.Ta < 0 || Input_data.Td < 0){
            if (Input_data.Ta < 0){
                // 没有加速段,只有减速段
                Input_data.Ta = 0;
                Input_data.Tj1 = 0;
                Input_data.Td = 2*(Input_data.q1 - Input_data.q0) / (Input_data.v0 + Input_data.v1);
                Input_data.Tj2 = (Input_data.jmax*(Input_data.q1 - Input_data.q0) - sqrt(Input_data.jmax*(Input_data.jmax*pow(Input_data.q1 - Input_data.q0, 2)
                                                                                                               + pow(Input_data.v1 + Input_data.v0, 2)*(Input_data.v1 - Input_data.v0)))) / (Input_data.jmax*(Input_data.v1 + Input_data.v0));
                Input_data.alima = 0;
                Input_data.alimd = -Input_data.jmax * Input_data.Tj2;
                Input_data.vlim = v0;
            }else if(Input_data.Td < 0){
                // 没有减速段,只有加速段
                Input_data.Td = 0;
                Input_data.Tj2 = 0;
                Input_data.Ta = 2*(Input_data.q1 - Input_data.q0) / (Input_data.v0 + Input_data.v1);
                Input_data.Tj1 = (Input_data.jmax*(Input_data.q1 - Input_data.q0) - sqrt(Input_data.jmax*(Input_data.jmax*pow(Input_data.q1 - Input_data.q0, 2)
                                                                                                               - pow(Input_data.v1 + Input_data.v0, 2)*(Input_data.v1 - Input_data.v0)))) / (Input_data.jmax*(Input_data.v1 + Input_data.v0));
                Input_data.alima = Input_data.jmax * Input_data.Tj1;
                Input_data.alimd = 0;
                Input_data.vlim = Input_data.jmax * Input_data.Tj1;
            }
        }else if(Input_data.Ta >= 2*Tj && Input_data.Td >= 2*Tj){
            // 加速段和减速段都能达到最大速度
            Input_data.alima = Input_data.amax;
            Input_data.alimd = -Input_data.amax;
            Input_data.vlim = v0 + Input_data.alima*(Input_data.Ta - Tj);
        }else{
            // 加速段和减速阶段至少有一段不能达到最大加速度
            float lambda = 0.99; // 系统取0<lambda<1
            while (Input_data.Ta < 2*Tj || Input_data.Td < 2*Tj){
                Input_data.amax = lambda * Input_data.amax;
                Input_data.Tv = 0;
                Tj = Input_data.amax / Input_data.jmax;
                Input_data.Tj1 = Tj;
                Input_data.Tj2 = Tj;
                delta = (pow(Input_data.amax, 4)/pow(Input_data.jmax, 2)) + 2*(pow(Input_data.v0, 2) + pow(Input_data.v1, 2))
                        + Input_data.amax*(4*(Input_data.q1 - Input_data.q0) - 2*(Input_data.amax/Input_data.jmax)*(Input_data.v0 + Input_data.v1));
                Input_data.Ta = ((pow(Input_data.amax, 2)/Input_data.jmax) - 2*Input_data.v0 + sqrt(delta)) / (2*Input_data.amax);
                Input_data.Td = ((pow(Input_data.amax, 2)/Input_data.jmax) - 2*Input_data.v1 + sqrt(delta)) / (2*Input_data.amax);
                if (Input_data.Ta < 0 || Input_data.Td < 0){
                    if (Input_data.Ta < 0){
                        // 没有加速段,只有减速段
                        Input_data.Ta = 0;
                        Input_data.Tj1 = 0;
                        Input_data.Td = 2*(Input_data.q1 - Input_data.q0) / (Input_data.v0 + Input_data.v1);
                        Input_data.Tj2 = (Input_data.jmax*(Input_data.q1 - Input_data.q0) - sqrt(Input_data.jmax*(Input_data.jmax*pow(Input_data.q1 - Input_data.q0, 2)
                                            + pow(Input_data.v1 + Input_data.v0, 2)*(Input_data.v1 - Input_data.v0)))) / (Input_data.jmax*(Input_data.v1 + Input_data.v0));
                        Input_data.alima = 0;
                        Input_data.alimd = -Input_data.jmax * Input_data.Tj2;
                        Input_data.vlim = v0;
                    }else if(Input_data.Td < 0){
                        // 没有减速段,只有加速段
                        Input_data.Td = 0;
                        Input_data.Tj2 = 0;
                        Input_data.Ta = 2*(Input_data.q1 - Input_data.q0) / (Input_data.v0 + Input_data.v1);
                        Input_data.Tj1 = (Input_data.jmax*(Input_data.q1 - Input_data.q0) - sqrt(Input_data.jmax*(Input_data.jmax*pow(Input_data.q1 - Input_data.q0, 2)
                                           - pow(Input_data.v1 + Input_data.v0, 2)*(Input_data.v1 - Input_data.v0)))) / (Input_data.jmax*(Input_data.v1 + Input_data.v0));
                        Input_data.alima = Input_data.jmax * Input_data.Tj1;
                        Input_data.alimd = 0;
                        Input_data.vlim = Input_data.jmax * Input_data.Tj1;
                    }
                }else if(Input_data.Ta >= 2*Tj && Input_data.Td >= 2*Tj){
                    // 加速段和减速段都能达到最大速度
                    Input_data.alima = Input_data.amax;
                    Input_data.alimd = -Input_data.amax;
                    Input_data.vlim = v0 + Input_data.alima*(Input_data.Ta - Tj);
                }
            }
        }
    }

    qDebug()<<Input_data.Ta;
}

float sSpeed::S_position(float t)
{
    float T = 0, q = 0;
    float Ta = Input_data.Ta;
    float Tv = Input_data.Tv;
    float Td = Input_data.Td;
    float Tj1 = Input_data.Tj1;
    float Tj2 = Input_data.Tj2;
    float q0 = Input_data.q0;
    float q1 = Input_data.q1;
    float v0 = Input_data.v0;
    float v1 = Input_data.v1;
    float vlim = Input_data.vlim;
    float alima = Input_data.alima;
    float alimd = Input_data.alimd;
    float jmax = Input_data.jmax;
    float jmin = Input_data.jmin;
    T = Ta + Tv + Td;
    // 加速段
    if (t >= 0 && t < Tj1)
        q = q0 + v0*t + jmax*pow(t, 3)/6;
    else if (t >= Tj1 && t < Ta - Tj1)
        q = q0 + v0*t +(alima/6)*(3*pow(t, 2) - 3*Tj1*t + pow(Tj1, 2));
    else if (t >= Ta - Tj1 && t < Ta)
        q = q0 + (vlim + v0)*(Ta/2) - vlim*(Ta - t) - jmin*(pow(Ta - t, 3)/6);
    // 匀速段
    else if (t >= Ta && t < Ta + Tv)
        q = q0 + (vlim + v0)*(Ta/2) + vlim*(t - Ta);
    // 减速段
    else if (t >= Ta + Tv && t < T - Td + Tj2)
        q = q1 - (vlim + v1)*(Td/2) + vlim*(t - T + Td) - jmax*(pow(t - T + Td, 3)/6);
    else if (t >= T - Td + Tj2 && t < T - Tj2)
        q = q1 - (vlim + v1)*(Td/2) + vlim*(t - T + Td) + (alimd/6)*(3*pow(t - T + Td, 2) - 3*Tj2*(t - T + Td) + pow(Tj2, 2));
    else if (t >= T - Tj2 && t <= T)
        q = q1 - v1*(T - t) - jmax*(pow(T - t, 3)/6);

    return q;
}

float sSpeed::S_velocity(float t )
{
    float T = 0, qd = 0;
    float Ta = Input_data.Ta;
    float Tv = Input_data.Tv;
    float Td = Input_data.Td;
    float Tj1 = Input_data.Tj1;
    float Tj2 = Input_data.Tj2;
    float v0 = Input_data.v0;
    float v1 = Input_data.v1;
    float vlim = Input_data.vlim;
    float alima = Input_data.alima;
    float alimd = Input_data.alimd;
    float jmax = Input_data.jmax;
    float jmin = Input_data.jmin;
    T = Ta + Tv + Td;
    // 加速段
    if (t >= 0 && t < Tj1)
        qd = v0 + jmax*(pow(t, 2)/2);
    else if (t >= Tj1 && t < Ta - Tj1)
        qd = v0 + alima*(t - Tj1/2);
    else if (t >= Ta - Tj1 && t < Ta)
        qd = vlim + jmin*(pow(Ta - t, 2)/2);
    // 匀速段
    else if (t >= Ta && t < Ta + Tv)
        qd = vlim;
    // 减速段
    else if (t >= Ta + Tv && t < T - Td + Tj2)
        qd = vlim - jmax*(pow(t - T + Td, 2)/2);
    else if (t >= T - Td + Tj2 && t < T - Tj2)
        qd = vlim + alimd*(t - T + Td - Tj2/2);
    else if (t >= T - Tj2 && t <= T)
        qd = v1 + jmax*(pow(t - T, 2)/2);

    return qd;
}

float sSpeed::S_acceleration(float t)
{
    float T = 0, qdd = 0;
    float Ta = Input_data.Ta;
    float Tv = Input_data.Tv;
    float Td = Input_data.Td;
    float Tj1 = Input_data.Tj1;
    float Tj2 = Input_data.Tj2;
    float alima = Input_data.alima;
    float alimd = Input_data.alimd;
    float jmax = Input_data.jmax;
    float jmin = Input_data.jmin;
    T = Ta + Tv + Td;
    // 加速段
    if (t >= 0 && t < Tj1)
        qdd = jmax * t;
    else if (t >= Tj1 && t < Ta - Tj1)
        qdd = alima;
    else if (t >= Ta - Tj1 && t < Ta)
        qdd = -jmin * (Ta - t);
    // 匀速段
    else if (t >= Ta && t < Ta + Tv)
        qdd = 0;
    // 减速段
    else if (t >= Ta + Tv && t < T - Td + Tj2)
        qdd = -jmax * (t - T - Td);
    else if (t >= T - Td + Tj2 && t < T - Tj2)
        qdd = alimd;
    else if (t >= T - Tj2 && t <= T)
        qdd = -jmax * (T - t);

    return qdd;
}

float sSpeed::S_jerk(float t)
{
    float T = 0, qddd = 0;
    float Ta = Input_data.Ta;
    float Tv = Input_data.Tv;
    float Td = Input_data.Td;
    float Tj1 = Input_data.Tj1;
    float Tj2 = Input_data.Tj2;
    float jmax = Input_data.jmax;
    float jmin = Input_data.jmin;
    T = Ta + Tv + Td;
    // 加速段
    if (t >= 0 && t < Tj1)
        qddd = jmax;
    else if (t >= Tj1 && t < Ta - Tj1)
        qddd = 0;
    else if (t >= Ta - Tj1 && t < Ta)
        qddd = -jmin;
    // 匀速段
    else if (t >= Ta && t < Ta + Tv)
        qddd = 0;
    // 减速段
    else if (t >= Ta + Tv && t < T - Td + Tj2)
        qddd = -jmax;
    else if (t >= T - Td + Tj2 && t < T - Tj2)
        qddd = 0;
    else if (t >= T - Tj2 && t <= T)
        qddd = jmax;

    return qddd;
}

仿真结果如下:
在这里插入图片描述

完整工程Git下载链接:https://gitee.com/luck126/sspeed
工具:Qt Creater5.9

评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值