速度规划: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;
}

在这里插入图片描述

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/671969.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

uniCloud ---- schema2code

目录 schema2code有两种方式 label属性 component属性 group属性 应用 DB Schema里有大量的信息&#xff0c;其实有了这些信息&#xff0c;前端将无需自己开发表单维护界面&#xff0c;uniCloud可以自动生成新增、修改、列表、详情的前端页面&#xff0c;以及admin端的列…

电脑小白必看:如何备份电脑系统?

电脑系统坏了怎么办&#xff1f;扛着自己的笔记本或是台式机的主机去电脑修理店花几十上百重装系统&#xff1f;或是自己用光盘或是U盘启动盘花费数小时重新安装系统&#xff1f;还在为系统问题发愁吗&#xff1f;今天&#xff0c;小编给大家一个简单的方法&#xff0c;让你免去…

ERP 系统架构的设计与实践总结

企业资源计划&#xff08;ERP&#xff09;系统是一种集成多个业务功能的综合性软件解决方案。在设计和实践 ERP 系统架构时&#xff0c;需要考虑诸多因素&#xff0c;以确保系统能够满足企业的需求&#xff0c;并提供高效、可靠、安全的服务。本文将介绍一些关键的设计原则和实…

2024Node.js零基础教程(小白友好型),nodejs新手到高手,(六)NodeJS入门——http模块

047_http模块_获取请求行和请求头 hello&#xff0c;大家好&#xff0c;那第二节我们来介绍一下如何在这个服务当中来提取 HTT 请求报文的相关内容。首先先说一下关于报文的提取的方法&#xff0c;我在这个文档当中都已经记录好了&#xff0c;方便大家后续做一个快速的查阅。 …

图像处理之《生成隐写流》论文阅读

一、文章摘要 生成隐写术(GS)是一种新的数据隐藏方式&#xff0c;其特点是直接从秘密数据生成隐写介质。现有的GS方法通常因性能差而受到批评。本文提出了一种新的基于流的GS方法——生成隐写流(GSF)&#xff0c;该方法可以直接生成隐写图像而不需要封面图像。我们将隐写图像生…

简单实验 spring cloud gateWay 路由实验 实验

1.概要 1.1 说明 微服务统一网关实验&#xff0c;这里简单实验一下路由的功能 1.2 实验步骤&#xff0c;使用下面这个工程作为基础工程添加了一个gateWay做如下使用 简单实践 spring cloud nacos nacos-server-2.3.0-CSDN博客 2 代码 2.1 工程文件 <?xml version&quo…

Qt程序设计-导出PDF

本文讲解如何实现导出PDF,包含如何使用HTML格式和添加图片。 实例如下: 创建项目,添加两个按钮,并在D盘提前准备好图片。 窗体的头文件 #ifndef MAINWINDOW_H #define MAINWINDOW_H#include <QMainWindow>#include <QPrinter> #include <QPainter> #i…

泰雷兹和Quantinuum推出入门套件,帮助企业为未来的后量子加密变革做好准备

•新的解决方案——PQC入门套件(PQC Starter Kit)为用户提供了一种快速、简便的方法&#xff0c;用于测试和衡量其在后量子时代防范量子计算攻击的准备情况 •随着量子计算逐渐成熟&#xff0c;企业为后量子时代做好准备并培养加密灵活性&#xff0c;对于降低数据泄露风险至关重…

Win32 SDK Gui编程系列之--ListView自绘OwnerDraw

ListView自绘OwnerDraw 1.ListView自绘OwnerDraw 正在试错是否使用了列表视图,尽量制作出智能的表格编辑器。本页显示了业主抽签的表格数据(二维数组数据)的显示方法。 显示画面和整个程序如下所示。使用ListView_GetSubItemRect宏的话,就不需要getRect函数了。 当nCol的…

10-树-从中序与后序遍历序列构造二叉树

这是树的第10篇算法&#xff0c;力扣链接。 给定两个整数数组 inorder 和 postorder &#xff0c;其中 inorder 是二叉树的中序遍历&#xff0c; postorder 是同一棵树的后序遍历&#xff0c;请你构造并返回这颗 二叉树 。 示例 1: 输入&#xff1a;inorder [9,3,15,20,7], po…

WebAssembly002 FFmpegWasmLocalServer项目

项目介绍 https://github.com/incubated-geek-cc/FFmpegWasmLocalServer.git可将音频或视频文件转换为其他可选的多媒体格式&#xff0c;并导出转码的结果 $ bash run.sh FFmpeg App is listening on port 3000!运行效果 相关依赖 Error: Cannot find module ‘express’ …

由vscode自动升级导致的“终端可以ssh服务器,但是vscode无法连接服务器”

问题描述 简单来说就是&#xff0c;ssh配置没动&#xff0c;前两天还可以用vscode连接服务器&#xff0c;今天突然就连不上了&#xff0c;但是用本地终端ssh可以顺利连接。 连接情况 我的ssh配置如下&#xff1a; Host gpu3HostName aaaUser zwx现在直接在终端中进行ssh&am…

ElastAlert 错误日志告警

文章目录 前言一、ElastAlert 概览1.1 简介1.2 ElastAlert 特性 二、ElastAlert 下载部署2.1 安装 Python3 环境2.2 下载 ElastAlert2.3 部署 ElastAlert 三、接入平台3.1 对外接口层3.2 服务层 前言 ElastAlert 是 Yelp 公司基于 python 开发的 ELK 日志告警插件&#xff0c;…

Stata学习(1)

一、五大窗口 Command窗口&#xff1a;实现人机交互 来导入一个自带数据&#xff1a; sysuse是导入系统自带的数据&#xff0c;auto导入该数据的名称&#xff0c;后面的clear是清除之前的数据 结果窗口&#xff1a;展示计算结果、查找功能 在Edit的find可以实现查找功能&#…

Nacos安装,服务注册,负载均衡配置,权重配置以及环境隔离

1. 安装 首先从官网下载 nacos 安装包&#xff0c;注意是下载 nacos-server Nacos官网 | Nacos 官方社区 | Nacos 下载 | Nacos 下载完毕后&#xff0c;解压找到文件夹bin&#xff0c;文本打开startup.cmd 修改配置如下 然后双击 startup.cmd 启动 nacos服务&#xff0c;默认…

【经验分享】如何高效管理Jmeter的接口测试用例?

Test Fragment 测试片段元素是控制器上的一个种特殊的线程组&#xff0c;它在测试树上与线程组处于一个层级。它与线程组有所不同&#xff0c;它不被执行&#xff0c;当它是一个模块控制器或者是被控制器所引用时才会被执行 添加Test Fragment的步骤&#xff1a; 目前接口的组…

微信小程序的图片色彩分析,窃取主色调,调色板

1、在微信小程序中创建包管理器 package.json npm init -y 2、安装 Mini App Color Thief 包 npm i --save miniapp-color-thief 3、构建 npm 4、wxml <canvas canvas-id"myCanvas"></canvas> <button bindtap"chooseImage">chooseIm…

MATLAB语音去噪系统

目录 一、背景 二、GUI页面 三、程序 3.1 LMS滤波程序 3.2 GUI程序 四、附录 一、背景 本文介绍了一种最佳的自适应滤波器结构&#xff0c;该结构采用最小均方差&#xff08;LMS&#xff09;作为判据&#xff0c;通过不断迭代自适应结构来调整得到最佳滤波器…

前端常用代码整理(不断更新中)— js,jquery篇

1.随机函数代码 function getRandom(min, max) {return Math.floor(Math.random() * (max - min 1)) min}2.倒计时代码 let now new Date()// 2. 得到指定时间的时间戳let last new Date(这里写想要达到的时间)// 3. &#xff08;计算剩余的毫秒数&#xff09; / 1000 剩余…

【Linux】缓冲区与缓冲区的刷新策略

目录 1.缓冲区基础 1.1缓冲区的刷新策略 1.1.1三种刷新策略 1.1.2.两种强制刷新策略 2.用户级语言层缓冲区 2.1.默认在显示器输出 2.2.重定向到文件输出 2.3.write调用没有显示两份的原因 3.模拟实现文件缓冲区 3.1 myFileBuffer.h 3.2 myFileBuffer.c 4.系统内核缓…