理论篇

应用篇

实现变速规划

#include <iostream>
#include <opencv2/opencv.hpp> // 包含OpenCV头文件
#include <chrono>
#include <thread>

using namespace std;


#define _CRT_SECURE_NO_WARNINGS
#define a_max  1.0
#define J  0.2
#define v_max  4.0


class VelocityPlanner
{
public:
    VelocityPlanner();
    ~VelocityPlanner();

    double GetSpeed(double t);
    void SetParameters(double robot, double target);

private:

    double time0;
    double lasttime;
    double T1, T2;
    double t1, t2, t3;

    double v0, v1, v2, v3;
    double targetspeed0;
    //double targetspeed;
    //double lasttargetspeed;
    double robotspeed0;
    //double robotspeed;
    //double lastrobotspeed;

    double j;

};

void VelocityPlanner::SetParameters(double robot, double target) {
    robotspeed0 = robot;
    targetspeed0 = target;
    //lastrobotspeed = robot;
    //lasttargetspeed = target;

    double errorspeed = target - robot;
    double T1_max = abs(a_max / J);
    bool trilogy = abs(errorspeed) > J * T1_max * T1_max ? true : false;

    //三段S形
    if (trilogy) {
        //计算时间T1 T2
        T1 = T1_max;
        T2 = abs(errorspeed) / a_max - T1;
    }
    //两段S形
    else {
        T1 = pow(abs(errorspeed) / J, 0.5);
        T2 = 0;
    }

    t1 = T1;
    t2 = t1 + T2;
    t3 = t2 + T1;

    if (errorspeed < 0) {
        j = -J;
    }
    else {
        j = J;
    }

    auto currentTime = std::chrono::system_clock::now();
    auto currentTime_ms = std::chrono::time_point_cast<std::chrono::milliseconds>(currentTime);//毫秒
    auto valueMS = currentTime_ms.time_since_epoch().count();
    time0 = valueMS * 0.001;
    lasttime = time0;

    std::cout << "Milliseconds: " << "    " << typeid(valueMS).name() << "  " << valueMS << std::endl;
    std::cout << "errortime: " << "    " << typeid(time0).name() << "  " << time0 << std::endl;

    v0 = robot;
    v1 = v0 + j * T1 * T1 * 0.5;
    v2 = v1 + j * T1 * T2;
    //v3 = target;
    v3 = v2 + j * T1 * T1 * 0.5;
}

double VelocityPlanner::GetSpeed(double t) {
    double period = t - time0;
    double temp = 0.0;
    if (period < 0) {
        return v0;
    }
    else if (period < t1) {
        return v0 + j * period * period * 0.5;
    }
    else if (period < t2) {
        temp = period - t1;
        return v1 + j * T1 * temp;
    }
    else if (period < t3) {
        temp = period - t2;
        return v2 + j * T1 * temp - j * temp * temp * 0.5;
    }
    else {
        return v3;
    }
    
}
VelocityPlanner::VelocityPlanner() {
}
VelocityPlanner::~VelocityPlanner()
{
}

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

    cv::Mat canvas(600, 600, 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)

    VelocityPlanner vp;
    vp.SetParameters(20, 10);

    auto currentTime = std::chrono::system_clock::now();
    auto currentTime_ms = std::chrono::time_point_cast<std::chrono::milliseconds>(currentTime);//毫秒
    auto valueMS = currentTime_ms.time_since_epoch().count();
    double time = valueMS * 0.001;

    bool flag = false;
    double last_vt = 0, last_t = 0;
    int kx = 30, ky = 10;
    double period;
    for (double t = time; t <= time+tf; t += 0.01) {
        //double s_t = PathCurve(t);
        period = t - time;
        double v_t = vp.GetSpeed(t);
        if (!flag) {
            cv::circle(canvas, cv::Point(period * kx, v_t * ky), 2, cv::Scalar(0, 0, 255), -1);
        }
        else {
            cv::circle(canvas, cv::Point(period * kx, v_t * ky), 2, cv::Scalar(0, 0, 255), -1);
            cv::line(canvas, cv::Point(last_t * kx, last_vt * ky), cv::Point(period * kx, v_t * ky), cv::Scalar(255, 0, 0), 1);//y周  (x,y)

        }
        last_vt = v_t;
        last_t = period;

        std::cout << period << "时刻速度：" << "    " << v_t << std::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;
}

运行结果