上海网站外包建设,百度关键词多少钱一个月,酒店网站设计模板,前端注册wordpress理论篇
先看该篇#xff0c;这里沿用了里面的变量。
应用推导篇
分为变速和停车两部分#xff08;字迹潦草#xff0c;可结合代码看#xff09; 代码篇
变速函数入口#xff1a;
velocityPlanner vp;
vp.SetParameters(0, 1);停车函数入口#xff1a;
ParkingVelo…理论篇
先看该篇这里沿用了里面的变量。
应用推导篇
分为变速和停车两部分字迹潦草可结合代码看 代码篇
变速函数入口
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.husing 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_caststd::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_caststd::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); // 水平镜像flipCode1 cv::imshow(Image, mirror_img);cv::waitKey(); // 等待10秒return 0;
}
