理论篇
先看该篇,这里沿用了里面的变量。
应用推导篇
分为变速和停车两部分(字迹潦草,可结合代码看)
代码篇
变速函数入口:
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.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 <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;
}