速度规划：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 <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.0class 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 <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 T2T1 = 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;
}