速度规划:s形曲线应用(变速 停车)opencv c++显示(3)

news2024/11/27 10:41:16

理论篇

先看该篇,这里沿用了里面的变量。

应用推导篇

分为变速和停车两部分(字迹潦草,可结合代码看)
请添加图片描述

请添加图片描述
请添加图片描述
请添加图片描述

代码篇

变速函数入口:

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;
}

在这里插入图片描述

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

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

相关文章

挑战杯 python+大数据校园卡数据分析

0 前言 &#x1f525; 优质竞赛项目系列&#xff0c;今天要分享的是 &#x1f6a9; 基于yolov5的深度学习车牌识别系统实现 &#x1f947;学长这里给一个题目综合评分(每项满分5分) 难度系数&#xff1a;4分工作量&#xff1a;4分创新点&#xff1a;3分 该项目较为新颖&am…

23.HarmonyOS App(JAVA)堆叠布局StackLayout使用方法

不常用 StackLayout直接在屏幕上开辟出一块空白的区域&#xff0c;添加到这个布局中的视图都是以层叠的方式显示&#xff0c;而它会把这些视图默认放到这块区域的左上角&#xff0c;第一个添加到布局中的视图显示在最底层&#xff0c;最后一个被放在最顶层。上一层的视图会覆盖…

mysql入门到精通005-基础篇-约束

1、概述 1.1 概念 约束是作用于表中字段上的规则&#xff0c;用于限制储存在表中的数据。 1.2 目的 保证数据库中数据的正确性、有效性和完整性。 1.3 常见的约束分类 一旦谈到外键&#xff0c;则至少涉及2张表约束是作用于表中字段上的&#xff0c;可以在创建表/修改表的…

机器学习-梯度下降法

不是一个机器学习算法是一种基于搜索的最优化方法作用&#xff1a;最小化一个损失函数梯度上升法&#xff1a;最大化一个效用函数 并不是所有函数都有唯一的极值点 解决方法&#xff1a; 多次运行&#xff0c;随机化初始点梯度下降法的初始点也是一个超参数 代码演示 impor…

【语音合成】中文-多情感领域-16k-多发音人

模型介绍 语音合成-中文-多情感领域-16k-多发音人 框架描述 拼接法和参数法是两种Text-To-Speech(TTS)技术路线。近年来参数TTS系统获得了广泛的应用&#xff0c;故此处仅涉及参数法。 参数TTS系统可分为两大模块&#xff1a;前端和后端。 前端包含文本正则、分词、多音字预…

在angular12中proxy.conf.json中配置详解

一、proxy.conf.json文件的目录 二、proxy.conf.json文件中的配置 "/xxx/api": {"target": "地址/api","secure": false,"logLevel": "debug","changeOrigin": true,"pathRewrite": {"…

【华为 ICT HCIA eNSP 习题汇总】——题目集13

1、以下在项目规划阶段中需要完成的工作是&#xff08;&#xff09;。 A、确定技术方案 B、了解项目背景 C、选择网络产品 D、规划 IP 地址 考点&#xff1a;网络规划与设计 解析&#xff1a;&#xff08;B&#xff09; 确定技术方案是在网络规划的设计阶段完成的工作&#xff…

团队管理-如何组织好一场会议

一、不同维度分析 1、按照时间维度 可分为 会前、会中、会后 会前 1、确定会议时间 尽可能选择参与者都空闲的时间&#xff0c;确保参与者都有时间可以参加&#xff0c;可以提前询问大家有空的时间&#xff0c;如果部分人没有时间但是会议比较紧急&#xff0c;可以让其选择…

2024:AI 大冒险

2024&#xff1a;AI 大冒险 2023 年就像一场疯狂的过山车&#xff0c;现在让我们一起系好安全带&#xff0c;来预测一下 2024 年的五大惊心动魄事件吧&#xff01; 一、AI 惹祸升级 嘿&#xff0c;2024 年可要小心了&#xff01;AI 这家伙可能会变得更调皮捣蛋。人们可能会用…

Bootstrap5 导航组件和面包屑

Bootstrap5 导航组件和面包屑 Bootstrap5 提供了一种简单快捷的方法来创建基本导航&#xff0c;它提供了非常灵活和优雅的选项卡和Pills等组件。 Bootstrap5 的所有导航组件&#xff0c;包括选项卡和Pills&#xff0c;都通过基本的 .nav 类共享相同的基本标记和样式。 使用 B…

springboot162基于SpringBoot的体育馆管理系统的设计与实现

体育馆管理系统 摘 要 现代经济快节奏发展以及不断完善升级的信息化技术&#xff0c;让传统数据信息的管理升级为软件存储&#xff0c;归纳&#xff0c;集中处理数据信息的管理方式。本体育馆管理系统就是在这样的大环境下诞生&#xff0c;其可以帮助管理者在短时间内处理完毕…

搜索引擎DuckDuckGo代理指南

DuckDuckGo作為一款搜索引擎&#xff0c;同時擁有自己的流覽器&#xff0c;高度保護用戶隱私&#xff0c;使其有別於其他收集和利用用戶數據進行定向廣告的搜索引擎。然而&#xff0c;單獨使用DuckDuckGo並不能保證線上完全匿名。如果你想進一步保護隱私&#xff0c;那就需要使…

深度解析源码,Spring 如何使用三级缓存解决循环依赖

目录 一. 前言 二. 基础知识 2.1. 什么是循环依赖&#xff1f; 2.2. 三级缓存 2.3. 原理执行流程 三. 源码解读 3.1. 代码入口 3.2. 第一层 3.3. 第二层 3.4. 第三层 3.5. 返回第二层 3.6. 返回第一层 四. 原理深度解读 4.1. 什么要有三级缓存&#xff1f; 4.2.…

深入解析 Spring 事务机制

当构建复杂的企业级应用程序时&#xff0c;数据一致性和可靠性是至关重要的。Spring 框架提供了强大而灵活的事务管理机制&#xff0c;成为开发者处理事务的首选工具。本文将深入探讨 Spring 事务的使用和原理&#xff0c;为大家提供全面的了解和实际应用的指导。 本文概览 首…

ELAdmin 的 CRUD

数据表结构 弄个测试的数据表&#xff0c;不同类型的几个字段&#xff0c;表名位 mp_reply。 生成代码 ELAdmin 可以自动生成代码。 左侧目录系统工具–代码生成&#xff0c;点开以后可以看到上面创建的数据表mp_reply&#xff0c;点击配置。 进入的页面内容有两部分&#…

【Py/Java/C++三种语言详解】LeetCode每日一题240207【二叉树BFS】LeetCode2641、二叉树的堂兄弟节点II

有华为OD考试扣扣交流群可加948025485 可上全网独家的 欧弟OJ系统 练习华子OD、大厂真题 绿色聊天软件戳 od1336了解算法冲刺训练 文章目录 题目链接题目描述解题思路代码PythonJavaC时空复杂度 华为OD算法/大厂面试高频题算法练习冲刺训练 题目链接 LeetCode2641、二叉树的堂…

C++——stack与queue与容器适配器

1.stack和queue的使用 1.1stack的使用 栈这种数据结构我们应该挺熟了&#xff0c;先入后出&#xff0c;只有一个出口(出口靠栈顶近)嘛 stack的底层容器可以是任何标准的容器类模板或者一些其他特定的容器类&#xff0c;这些容器类应该支持以操作&#xff1a; empty&#xff1…

6-3、T型加减速单片机程序【51单片机+L298N步进电机系列教程】

↑↑↑点击上方【目录】&#xff0c;查看本系列全部文章 摘要&#xff1a;根据前两节内容&#xff0c;已完成所有计算工作&#xff0c;本节内容介绍具体单片机程序流程及代码 一、程序流程图 根据前两节文章内容可知&#xff0c;T型加减速的关键内容是运动类型的判断以及定时…

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

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

【AWS】step-functions服务编排

文章目录 step-functionsState machine typeStandard workflowsExpress workflows design skillsError handlingsaga Transaction processing控制分布式系统中的并发性 收费 作为AWS Serverless无服务器的一个重要一环 使用step-functions方法将 AWS 服务链接在一起 step-funct…