速度规划：s形曲线（opencv c++绘图）

理论篇

代码篇：

#include 
#include 

#include 

using namespace std;

double a_max = 1.5; // 最大加速度
double v_max = 6.0; // 最大速度
double J = 1; // 最大速度
double T4 = 2.0;

double T1 = a_max / J;
double T3 = T1; 
double T5 = T3;
double T7 = T5;

double T2 = v_max / a_max - T1; 
double T6 = T2;

double t1 = T1;
double t2 = t1 + T2;
double t3 = t2 + T3;
double t4 = t3 + T4;
double t5 = t4 + T5;
double t6 = t5 + T6;
double t7 = t6 + T7;

double a1 = J * T1;//该段最后时刻的速度
double a2 = a_max;
double a3 = 0;
double a4 = 0;
double a5 = a_max - J * T5;
double a6 = -a_max;
double a7 = 0;


double v1 = 0.5 * J * pow(T1, 2);//该段最后时刻的速度
double v2 = v1 + J * T1 * T2;
double v3 = v2 + 0.5 * J * pow(T1, 2);
double v4 = v3;
double v5 = v4 - 0.5 * J * pow(T5, 2);
double v6 = v5 - J * T5 * T6;
double v7 = v6 - 0.5 * J * pow(T5, 2);

double S1 = J * pow(T1, 3) / 6.0;
double S2 = S1 + v1 * T2 + J * T1 * pow(T2, 2) * 0.5;
double S3 = S2 + v2 * T3 + J * T1 * pow(T3, 2) * 0.5 - J * pow(T3, 3) / 6;
double S4 = S3 + v3 * T4;
double S5 = S4 + v4 * T5 - J * pow(T5, 3) / 6;
double S6 = S5 + v5 * T6 - J * T1 * pow(T6, 2) * 0.5;
double S7 = S6 + v6 * T7 - J * T1 * pow(T7, 2) * 0.5 + J * pow(T7, 3) / 6;


double VelocityCurve(double t) {
    double temp = 0;
    if (t < 0 || t >= t7) {
        return 0;
    }
    else if (t < t1) {
        return 0.5 * J * pow(t, 2);
    }
    else if (t < t2) {
        temp = t - t1;
        return v1 + J * T1 * temp;
    }
    else if (t < t3) {
        temp = t - t2;
        return v2 + J * T1 * temp - 0.5 * J * pow(temp, 2);
    }
    else if (t < t4) {
        temp = t - t3;
        return v3;
    }
    else if (t < t5) {
        //std::cout << "t5时刻的具体时间： " << t << "    ";
        temp = t - t4;
        return v4 - 0.5 * J * pow(temp, 2);
    }
    else if (t < t6) {
        //std::cout << "t6时刻的具体时间： " << t << "    ";
        std::cout << "ssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssss： " << t << "    ";
        temp = t - t5;
        return v5 - J * T5 * temp;
    }
    else if (t < t7) {
        //std::cout << "t7时刻的具体时间： " << t << "    ";
        temp = t - t6;
        return v6 - J * T5 * temp + 0.5 * J * pow(temp, 2);
    }
}




double PathCurve(double t) {
    double temp = 0;
    if (t < 0) {
        return 0;
    }
    else if (t >= t7) {
        return S7;
    }
    else if (t < t1) {
        return J * pow(t, 3) / 6.0;
    }
    else if (t < t2) {
        temp = t - t1;
        return S1 + v1 * temp + J * T1 * pow(temp, 2) * 0.5;
    }
    else if (t < t3) {
        temp = t - t2;
        return S2 + v2 * temp + J * T1 * pow(temp, 2) * 0.5 - J * pow(temp, 3) / 6;
    }else if (t < t4) {
        temp = t - t3;
        return S3 + v3 * temp;
    }
    else if (t < t5) {
        std::cout << "t5时刻的具体时间： " << t << std::endl;
        temp = t - t4;
        return S4 + v4 * temp - J * pow(temp, 3) / 6;
    }
    else if (t < t6) {
        std::cout << "t6时刻的具体时间： " << t << std::endl;
        temp = t - t5;
        return S5 + v5 * temp - J * T1 * pow(temp, 2) * 0.5;
    }
    else if (t < t7) {
        std::cout << "t7时刻的具体时间： " << t << std::endl;
        temp = t - t6;
        return S6 + v6 * temp - J * T1 * pow(temp, 2) * 0.5 + J * pow(temp, 3) / 6;
    }
}

int main() {
    double tf = 16.0; // 总时间

    cv::Mat canvas(400, 400, CV_8UC3, cv::Scalar(255, 255, 255)); // 创建一个300x300像素的画布
    
    // 显示第一个圆
    cv::line(canvas, cv::Point(0, 0), cv::Point(0, 400), cv::Scalar(255, 0, 0), 2);//y周  (x,y)
    cv::line(canvas, cv::Point(0, 0), cv::Point(400, 0), cv::Scalar(255, 0, 0), 2);//x周  (x,y)
    
    bool flag = false;
    double last_st = 0, last_t = 0;
    int kx = 30, ky = 10;
    for (double t = 0.0; t <= tf; t += 0.1) {
        //double s_t = PathCurve(t);
        double s_t = VelocityCurve(t);
        if (!flag) {
            cv::circle(canvas, cv::Point(t * kx, s_t * ky), 2, cv::Scalar(0, 0, 255), -1);
        }
        else {
            cv::circle(canvas, cv::Point(t * kx, s_t * ky), 2, cv::Scalar(0, 0, 255), -1);
            cv::line(canvas, cv::Point(last_t * kx, last_st * ky), cv::Point(t * kx, s_t * ky), cv::Scalar(255, 0, 0), 1);//y周  (x,y)
    
        }
        last_st = s_t;
        last_st = t;
    
        std::cout <<t<< "时刻的位置 = " << s_t << std::endl;
    }
    cout << "t5:" << t5 << endl;
    cout << "t6:" << t6 << endl;
    cout << "t7:" << t7 << endl;
    
    // 创建镜像图像矩阵  
    cv::Mat mirror_img;
    cv::flip(canvas, mirror_img, 0);  // 水平镜像，flipCode=1  
    
    // 显示原始图像和镜像图像  
    //cv::imshow("Original Image", canvas);
    cv::imshow("Image", mirror_img);
    //cv::waitKey(0);
    
    cv::waitKey(); // 等待10秒



    return 0;
}

运行结果：