S型速度曲线

S型速度曲线

介绍

输入距离，算法计算，输出加加速段、匀加速段、减加速段、匀速段、加减速段、匀减速段、减减速段时间。考虑实际距离与设置参数（运行速度、加速度、加加速度、速度峰值）之间的关系，存在以下三种情况：

• 可达最大运行速度
• 不可达最大运行速度，可达最大加速度
• 不可达最大运行速度，不可达最大加速度

代码实现

#include <stdio.h>
#include <math.h>

int main()
{
    // input
    float distance = 15;

    // parameter setting
    float vel = 10;
    float acc = 10;
    float jerk = 100;
    float vel_peak = 0;

    float acc_distance = 0;
    float total_distance = 0;

    float t1 = 0;
    float t2 = 0;
    float t3 = 0;
    float t4 = 0;
    float t5 = 0;
    float t6 = 0;
    float t7 = 0;

    if(vel >= (acc * acc / jerk))
    {
        t1 = acc / jerk;
        t2 = vel / acc - acc / jerk;
        t3 = t1;

        acc_distance = vel / 2 * (t1 + t2 + t3);
    }
    else if(vel < (acc * acc / jerk))
    {
        t1 = sqrt(vel / jerk);
        t2 = 0;
        t3 = t1;

        acc_distance = vel * t1;
    }

    total_distance = 2 * acc_distance;

    if(distance >= total_distance)
    {
        t4 = (distance - total_distance) / vel;
        t5 = t1;
        t6 = t2;
        t7 = t1;
    }
    else if(distance < total_distance)
    {
        t4 = 0;
        if(distance >= 2 * acc * acc * acc / jerk / jerk)
        {
            t2 = (-3 * t1 + sqrt(t1 * t1 + 4 * distance / acc)) / 2;

            t5 = t1;
            t6 = t2;
            t7 = t1;

            vel_peak = acc * (t1 + t2);
            printf("vel_peak = %f\n", vel_peak);
        }
        else if(distance < 2 * acc * acc * acc / jerk / jerk)
        {
            t1 = pow(distance / (2 * jerk), 1.0/3.0);   // div operator need float / float, not int / int

            t3 = t1;
            t5 = t1;
            t7 = t1;

            t2 = 0;
            t4 = 0;
            t6 = 0;

            vel_peak = jerk * t1 * t1;
            printf("vel_peak = %f\n", vel_peak);
        }
    }

    // output
    printf("t1 = %f\n", t1);
    printf("t2 = %f\n", t2);
    printf("t3 = %f\n", t3);
    printf("t4 = %f\n", t4);
    printf("t5 = %f\n", t5);
    printf("t6 = %f\n", t6);
    printf("t7 = %f\n", t7);

    return 0;
}

结果验证

• 可达最大运行速度

distance = 15

t1 = 0.100000
t2 = 0.900000
t3 = 0.100000
t4 = 0.400000
t5 = 0.100000
t6 = 0.900000
t7 = 0.100000

• 不可达最大运行速度，可达最大加速度

distance = 10

vel_peak = 9.512492
t1 = 0.100000
t2 = 0.851249
t3 = 0.100000
t4 = 0.000000
t5 = 0.100000
t6 = 0.851249
t7 = 0.100000

• 不可达最大运行速度，不可达最大加速度

distance = 0.1

vel_peak = 0.629960
t1 = 0.079370
t2 = 0.000000
t3 = 0.079370
t4 = 0.000000
t5 = 0.079370
t6 = 0.000000
t7 = 0.079370