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

在这里插入图片描述

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

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！

相关文章

2.6：冒泡、简选、直插、快排，递归，宏

2.6：冒泡、简选、直插、快排，递归，宏

1.冒泡排序、简单选择排序、直接插入排序、快速排序(升序) 程序代码&#xff1a; 1 #include<stdio.h>2 #include<string.h>3 #include<stdlib.h>4 void Bubble(int arr[],int len);5 void simple_sort(int arr[],int len);6 void insert_sort(int arr[],in…
阅读更多...
ARP毒化

ARP毒化

ARP毒化虽然是一种比较老的渗透测试技术&#xff0c;但是在信息搜集方面能发挥出 很不错的效果。通过ARP毒化技术分析并提取内网流量中的敏感信息&#xff0c;往往会有 许多意外的“ 收获”。 9.2.1 工作原理 ARP&#xff08;地址解析协议&#xff09;是数据链路层的协议&am…
阅读更多...
【C中二三事】指针专题

【C中二三事】指针专题

指针专题 在 C 中&#xff0c;指针概念一直处于不佳而或缺的地位&#xff0c;本文就指针这一主题&#xff0c;记录下C语言在指针编程中的小细节。 文章目录 指针专题场景一解 场景二解 场景三解 场景四解 场景五解 场景六解 场景七解 场景一 ​ ∗ p *p ∗p 自增的是 p p p…
阅读更多...
工业互联网IoT物联网设备网络接入认证安全最佳实践

工业互联网IoT物联网设备网络接入认证安全最佳实践

制造业数字化转型过程中&#xff0c;产线物联网&#xff08;IoT&#xff09;设备、工控机的引入极大提高了生产效率的同时&#xff0c;也埋下了不容忽视的安全隐患。尤其制造业已成为勒索软件攻击的重灾区&#xff0c;利用物联网设备漏洞进行恶意攻击的事件不胜枚举&#xff0c…
阅读更多...
java---查找算法（二分查找，插值查找，斐波那契[黄金分割查找] ）-----详解 (ᕑᗢᓫ∗)˒

java---查找算法（二分查找，插值查找，斐波那契[黄金分割查找] ）-----详解 (ᕑᗢᓫ∗)˒

目录 一. 二分查找&#xff08;递归&#xff09;&#xff1a; 代码详解&#xff1a; 运行结果&#xff1a; 二分查找优化&#xff1a; 优化代码&#xff1a; 运行结果&#xff08;返回对应查找数字的下标集合&#xff09;&#xff1a; ​编辑 二分查找&#xff08;非递归…
阅读更多...
Nacos1.X源码解读（待完善）

Nacos1.X源码解读（待完善）

下载源码 1. 克隆git地址到本地 # 下载nacos源码 git clone https://github.com/alibaba/nacos.git 2. 切换分支到1.4.7, maven编译(3.5.1) 3. 找到启动类com.alibaba.nacos.Nacos 4. 启动VM参数设置单机模式, RUN 启动类 -Dnacos.standalonetrue 5. 启动本地服务注册到本…
阅读更多...
SpringFramework实战指南（六）

SpringFramework实战指南（六）

SpringFramework实战指南（六） 4.4 基于 配置类 方式管理 Bean4.4.1 完全注解开发理解4.4.2 实验一：配置类和扫描注解4.4.3 实验二：@Bean定义组件4.4.4 实验三：高级特性：@Bean注解细节4.4.5 实验四：高级特性：@Import扩展4.4.6 实验五：基于注解+配置类方式整合三层架构组…
阅读更多...
浅谈——开源软件的影响力

浅谈——开源软件的影响力

✅作者简介&#xff1a;2022年博客新星 第八。热爱国学的Java后端开发者&#xff0c;修心和技术同步精进。 &#x1f34e;个人主页&#xff1a;Java Fans的博客 &#x1f34a;个人信条&#xff1a;不迁怒&#xff0c;不贰过。小知识&#xff0c;大智慧。 ✨特色专栏&#xff1a…
阅读更多...
二叉树OJ题（1）

二叉树OJ题（1）

目录 1.相同的树 2.对称二叉树 3.翻转二叉树 4.另一颗树的子树 题目代码思路整体分析&注意事项易错点画图递归分析 树根左子树右子树 分支的思想 多情况考虑 1.相同的树 100. 相同的树 - 力扣&#xff08;LeetCode&#xff09;https://leetcode.cn/problems/same-…
阅读更多...
数据结构.树的线索化兄弟表示法哈夫曼树

数据结构.树的线索化兄弟表示法哈夫曼树

一、线索化 二、树的逻辑结构 三、哈夫曼树
阅读更多...
JSDoc 注释规范

JSDoc 注释规范

JSDoc 注释 在 前端项目中&#xff0c;注释格式包含了一些特殊标记&#xff0c;如 param、returns 等&#xff0c;这种注释通常是用来标记函数或方法的参数和返回值的数据类型和描述。 这种注释格式通常被称为 JSDoc 注释。在实际开发中&#xff0c;这样的注释可以被一些工具解…
阅读更多...
购物车商品数量为0判断是否删除

购物车商品数量为0判断是否删除

当编辑商品的数量为1&#xff0c;再减的话&#xff0c;我们搞个模态提示&#xff0c;让用户决定是否要删除这个商品&#xff1f; //商品数量的编辑功能handleItemNumEdit(e){const {operation,id}e.currentTarget.dataset;console.log(operation,id);let {cart}this.data;let …
阅读更多...
STM32 硬件随机数发生器(RNG)

STM32 硬件随机数发生器(RNG)

STM32 硬件随机数发生器 文章目录 STM32 硬件随机数发生器前言第1章 随机数发生器简介1.1 RNG主要特性1.2.RNG应用 第2章 RNG原理框图第3章 RNG相关寄存器3.1 RNG 控制寄存器 (RNG_CR)3.2 RNG 状态寄存器 (RNG_SR)3.3 RNG 数据寄存器 (RNG_DR) 第3章 RNG代码部分第4章 STM32F1 …
阅读更多...
多维时序 | MATLAB实现基于CNN-LSSVM卷积神经网络-最小二乘支持向量机多变量时间序列预测

多维时序 | MATLAB实现基于CNN-LSSVM卷积神经网络-最小二乘支持向量机多变量时间序列预测

多维时序 | MATLAB实现基于CNN-LSSVM卷积神经网络-最小二乘支持向量机多变量时间序列预测 目录 多维时序 | MATLAB实现基于CNN-LSSVM卷积神经网络-最小二乘支持向量机多变量时间序列预测预测效果基本介绍程序设计参考资料 预测效果 基本介绍 1.MATLAB实现基于CNN-LSSVM卷积神经…
阅读更多...
有趣的CSS - 多彩变化的按钮

有趣的CSS - 多彩变化的按钮

目录 整体效果核心代码html 代码css 部分代码 完整代码如下html 页面css 样式页面渲染效果 整体效果 这个按钮效果主要使用 :hover 、:active 伪选择器以及 animation 、transition 属性来让背景色循环快速移动形成视觉效果。 核心代码部分&#xff0c;简要说明了写法思路&…
阅读更多...
生存类游戏《幻兽帕鲁》从部署服务器到开始体验全过程

生存类游戏《幻兽帕鲁》从部署服务器到开始体验全过程

SteamDB数据显示&#xff0c;《幻兽帕鲁》上线24小时内&#xff0c;在线人数峰值便突破200万&#xff0c;跻身Steam历史排行榜第二位。随着热度进一步发酵&#xff0c;《幻兽帕鲁》官方发布推文称&#xff0c;游戏发售不到6天&#xff0c;销量已经突破了 800万份。欢迎大家在阿…
阅读更多...
问题：以下关于搜索OCPC说法错误的是（)？ #知识分享#知识分享#媒体

问题：以下关于搜索OCPC说法错误的是（)？ #知识分享#知识分享#媒体

问题&#xff1a;以下关于搜索OCPC说法错误的是&#xff08;)&#xff1f; A&#xff0e;OCPC进入第二阶段&#xff0c;不能随意更换转化目标和页面 B&#xff0e;OCPC可以直接跳过第一阶段&#xff0c;直接开始跑第二阶段 C&#xff0e;开启OCPC计划后&#xff0c;系统就会…
阅读更多...
零基础学编程从哪里入手，在学习中可以线上会议答疑解惑

零基础学编程从哪里入手，在学习中可以线上会议答疑解惑

一、前言 零基础学编程可以先从容易学的语言入手&#xff0c;比如中文编程&#xff0c;然后再学其他编程语言则会比较轻松&#xff0c;初步掌握编程思路。很多IT人士一般学2到3种编程语言。 今天给大家分享的中文编程开发语言工具资料如下&#xff1a; 编程入门视频教程链接…
阅读更多...
java内部类概述及使用方法

java内部类概述及使用方法

前言&#xff1a; 打好基础&#xff0c;daydayup! 内部类 内部类概述&#xff1a; 内部类是类的五大成分之一&#xff08;成员变量&#xff0c;方法&#xff0c;构造器&#xff0c;内部类&#xff0c;代码块&#xff09;&#xff0c;如果一个类定义在另一个类的内部&#xff…
阅读更多...
虚拟飞控计算机：飞行控制系统验证与优化的利器

虚拟飞控计算机：飞行控制系统验证与优化的利器

01.背景介绍 随着航空技术的飞速发展&#xff0c;飞行控制系统作为飞机的心脏&#xff0c;全面负责监测、调整和维持飞行器的姿态、航向、高度等参数&#xff0c;用以确保飞行的安全和稳定。为了满足这些要求&#xff0c;现代飞控系统通常采用先进的处理器和外设来确保其高效、…
阅读更多...
最新文章

Copyright @ 2022~2023