速度规划：s形曲线应用（变速 停车）opencv c++显示（3）

理论篇

先看该篇，这里沿用了里面的变量。

应用推导篇

分为变速和停车两部分（字迹潦草，可结合代码看）

代码篇

变速函数入口：

velocityPlanner vp;
vp.SetParameters(0, 1);

停车函数入口：

ParkingVelocityPlanner vp;
vp.SetDistance(1, 0.4);

头文件：SpeedPlan.h

#ifndef SPEEDPLAN_H
#define SPEEDPLAN_H


#include  // 包含OpenCV头文件
#include 
#include 

#define _CRT_SECURE_NO_WARNINGS
#define a_max  1.0
#define J_s  0.5
#define v_max  4.0


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

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

    //private:

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

    double v0, v1, v2, v3;
    double targetspeed0;
    double robotspeed0;

    double j, J;
};

VelocityPlanner::VelocityPlanner() {
    J = J_s;
}
VelocityPlanner::~VelocityPlanner() {
}

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

    double GetSpeed(double t) override ;
    void SetDistance(double robot, double distance);
    void SetJ(double j);

    double S0, S1, S2, S3;
    double s_min, s_s;
    double distance0;

private:
    double CalculateFitJ(double robot, double distance);

};

ParkingVelocityPlanner::ParkingVelocityPlanner()
{
}

ParkingVelocityPlanner::~ParkingVelocityPlanner()
{
}

#endif

主文件SpeedPlan.cpp

#include 
#include "SpeedPlan.h"

using namespace std;




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

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

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

    T0 = 0;
    t0 = T0;
    t1 = t0 + 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 < t0) {
        return v0;
    }
    else if (period < t1) {
        temp = period - t0;
        return v0 + j * temp * temp * 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;
    }

}

void ParkingVelocityPlanner::SetDistance(double robot,double distance)
{
    distance0 = distance;

    //急刹段内
    T2 = robot / a_max;
    s_min = 0.5 * a_max * T2 * T2;
    if (distance < s_min) {
        cout << "急刹，速度规划失效！" << endl;
        return;
    }

    //s形规划
    SetParameters(robot, 0);
    S1 = v0 * T1 + j * pow(T1, 3)/6;
    S2 = v1 * T2 + 0.5 * j * T1 * T2 * T2;
    S3 = v2 * T1 + j * pow(T1, 3) / 3;
    s_s = S1 + S2 + S3;
    T0 = (distance - s_s) / robot;
    t0 = T0;
    t1 = t0 + T1;
    t2 = t1 + T2;
    t3 = t2 + T1;
    if (distance >= s_s) {
        cout << "s形速度规划！" << endl;
        cout << "j: " << j << endl;
        cout << "a_max  a: " << a_max << " " << j * T1 << endl;
        cout << "s_s: " <<s_s<<" "<<distance << endl;
        cout << "t0-3: " <<t3<<" "<<t0<<" " << t3 - t0 << endl;
        return;
    }

    //拟合规划
    double j_adaptive = CalculateFitJ(robot, distance);
    SetJ(j_adaptive);
    SetParameters(robot, 0);
    cout << "拟合速度规划！" << endl;
    cout << "j: " << j << endl;
    cout << "s_s: " << distance << endl;
    cout << "T0-3: " << t3-t0<< endl;
    cout << "a_max  a: " << a_max << " " << j * T1 << endl;

}

double ParkingVelocityPlanner::CalculateFitJ(double robot, double distance) {
    //两段式
    T2 = 0;
    T1 = distance / robot;
    double j_temp = robot / T1 / T1;
    if (j_temp * T1 <= a_max) {
        return j_temp;
    }

    //三段式
    T1 = 2 * distance / robot - robot / a_max;
    T2 = robot / a_max - T1;
    j_temp = a_max / T2;
    return j_temp;
}

void ParkingVelocityPlanner::SetJ(double j) {
    J = j;
}

double ParkingVelocityPlanner::GetSpeed(double t) {
    //急刹
    if (distance0 < s_min) {
        return 0;
    }
    //拟合规划
    else{
        double period = t - time0;
        double temp = 0.0;
        if (period < t0) {
            return v0;
        }
        else if (period < t1) {
            temp = period - t0;
            return v0 + j * temp * temp * 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;
        }
    }
}


int main() {

    //VelocityPlanner vp;
    //vp.SetParameters(0, 1);
    //cout << "时间：" << vp.t3 - vp.t0 << endl;


    ParkingVelocityPlanner vp;
    vp.SetDistance(1, 0.4);

    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;

    cv::Mat canvas(600, 600, CV_8UC3, cv::Scalar(255, 255, 255)); // 创建一个300x300像素的画布
    cv::line(canvas, cv::Point(0, 0), cv::Point(0, 600), cv::Scalar(255, 0, 0), 4);//y周  (x,y)
    cv::line(canvas, cv::Point(0, 0), cv::Point(600, 0), cv::Scalar(255, 0, 0), 4);//x周  (x,y)
    double tf = vp.t3 * 1.25;// 总时间
    double kx = 500 / tf, ky = 300 / max(vp.v3, vp.v0);
    double period;
    double cyclicality = tf / 100;
    for (double t = time; t <= time + tf; t += cyclicality) {
        //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::line(canvas, cv::Point(vp.t0 * kx, vp.v0 * ky), cv::Point(vp.t0 * kx, 0), cv::Scalar(100, 0, 0), 1);//y周  (x,y)
    cv::circle(canvas, cv::Point(vp.t0 * kx, vp.v0 * ky), 5, cv::Scalar(0, 0, 255), -1);

    cv::line(canvas, cv::Point(vp.t1 * kx, vp.v1 * ky), cv::Point(vp.t1 * kx, 0), cv::Scalar(100, 0, 0), 1);//y周  (x,y)
    cv::circle(canvas, cv::Point(vp.t1 * kx, vp.v1 * ky), 5, cv::Scalar(0, 0, 255), -1);
    //cv::putText(canvas, "(" + std::to_string(vp.t1) + "," + std::to_string(vp.v1) + ")", cv::Point(vp.t1 * kx, vp.v1 * ky), cv::FONT_HERSHEY_SIMPLEX, 1.5, cv::Scalar(0, 255, 0), 2);

    cv::line(canvas, cv::Point(vp.t2 * kx, vp.v2 * ky), cv::Point(vp.t2 * kx, 0), cv::Scalar(100, 0, 0), 1);//y周  (x,y)
    cv::circle(canvas, cv::Point(vp.t2 * kx, vp.v2 * ky), 5, cv::Scalar(0, 0, 255), -1);
    //cv::putText(canvas, "(" + std::to_string(vp.t2) + "," + std::to_string(vp.v2) + ")", cv::Point(vp.t2 * kx, vp.v2 * ky), cv::FONT_HERSHEY_SIMPLEX, 1.5, cv::Scalar(0, 255, 0), 2);

    cv::line(canvas, cv::Point(vp.t3 * kx, vp.v3 * ky), cv::Point(vp.t3 * kx, 0), cv::Scalar(100, 0, 0), 1);//y周  (x,y)
    cv::circle(canvas, cv::Point(vp.t3 * kx, vp.v3 * ky), 5, cv::Scalar(0, 0, 255), -1);
    //cv::putText(canvas, "(" + std::to_string(vp.t3) + "," + std::to_string(vp.v3) + ")", cv::Point(vp.t3 * kx, vp.v3 * ky), cv::FONT_HERSHEY_SIMPLEX, 1.5, cv::Scalar(0, 255, 0), 2);

    

    // 创建镜像图像矩阵  
    cv::Mat mirror_img;
    cv::flip(canvas, mirror_img, 0);  // 水平镜像，flipCode=1  
    cv::imshow("Image", mirror_img);
    cv::waitKey(); // 等待10秒

    return 0;
}