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

理论篇

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

应用推导篇

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

代码篇

变速函数入口：

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

停车函数入口：

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

头文件：SpeedPlan.h

#ifndef SPEEDPLAN_H
#define SPEEDPLAN_H


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

#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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71

主文件SpeedPlan.cpp

#include <iostream>
#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;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246