在ROS上快速验证PID算法

在ROS上快速验证PID算法

前言

最近有在外面出差授课的工作任务，其中有一个环节是给大家讲述PID相关的内容，在制作相关PPT的时候查询了很多资料，但是写着写着突然意识到一个问题，PID已经在控制专业学习过程以及工程开发时间中那么常见了，继续去照本宣科的讲解其理论是否还有必要，随即开始想不如开发一个小DEMO，在ROS环境里验证PID算法实例。这也是这篇文章的由来了。

PID算法简介

  • 比例（Proportional）(P)：

    计算当前偏差（设定值与实际值的差距），并乘以比例增益（Kp）。

    输出对系统的当前状态进行纠正。

  • 积分（Integral)（I）：

    累积偏差随时间的总和，并乘以积分增益（Ki）。

    解决由于比例控制引入的静态误差，确保系统最终能够达到设定值。

  • 微分（Derivative）（D）：

    计算偏差变化率，并乘以微分增益（Kd）。

    抑制系统的振荡，提高系统的稳定性。

这里有一张比较形象的图可以用于简述其工作状态。

算法实现

首先实现其基类接口

    // pid.h#ifndef PID_H#define PID_Hclass PIDImpl;class PID {public:PID(double dt, double max, double min, double Kp, double Kd, double Ki);double calculate(double setPoint, double processValue);~PID();private:PIDImpl* pimpl;};#endif //PID_H

再实现其算法计算

#include "pid.h"#include <iostream>#include <cmath>class PIDImpl {public:PIDImpl(double dt, double max, double min, double Kp, double Kd, double Ki);~PIDImpl();double calculate(double setPoint, double processValue);private:double _dt;double _max;double _min;double _Kp;double _Kd;double _Ki;double _pre_error;double _integral;};PID::PID(double dt, double max, double min, double Kp, double Kd, double Ki) {pimpl = new PIDImpl(dt, max, min, Kp, Kd, Ki);}double PID::calculate(double setPoint, double processValue) {return pimpl->calculate(setPoint, processValue);}PID::~PID() {delete pimpl;}PIDImpl::PIDImpl(double dt, double max, double min, double Kp, double Kd, double Ki) :_dt(dt),_max(max),_min(min),_Kp(Kp),_Kd(Kd),_Ki(Ki),_pre_error(0),_integral(0) {}/*- 比例（Proportional）(P)：计算当前偏差（设定值与实际值的差距），并乘以比例增益（Kp）。输出对系统的当前状态进行纠正。- 积分（Integral)（I）：累积偏差随时间的总和，并乘以积分增益（Ki）。解决由于比例控制引入的静态误差，确保系统最终能够达到设定值。- 微分（Derivative）（D）：计算偏差变化率，并乘以微分增益（Kd）。抑制系统的振荡，提高系统的稳定性。*/double PIDImpl::calculate(double setPoint, double processValue) {double error = setPoint - processValue;double Pout = _Kp * error;_integral += error * _dt;double Iout = _Ki * _integral;double derivative = (error - _pre_error) / _dt;double Dout = _Kd * derivative;double output = Pout + Iout + Dout;if (output > _max)output = _max;else if (output < _min)output = _min;_pre_error = error;return output;}PIDImpl::~PIDImpl() {}

最后如何实现其算法调用呢？

    //control.cpp#include "ros/ros.h"#include <tf/transform_listener.h>#include <nav_msgs/Odometry.h>#include <visualization_msgs/Marker.h>#include <std_srvs/Empty.h>#include "geometry.h"#include "pid.h"tf::Point Odom_pos;double Odom_yaw, Odom_v, Odom_w;ros::Publisher cmd_vel_pub, marker_pub;ros::Subscriber odom_sub;int num_slice = 50;double maxSpeed = 0.5, distanceConst = 0.5;// double dt = 0.1, maxT = M_PI, minT = -M_PI, Kp = 0.3, Ki = 0.05, Kd = 0.01;double dt = 0.1, maxT = M_PI, minT = -M_PI, Kp = 1, Ki = 0.02, Kd = 0.1;// double dtS = 0.1, maxS = maxSpeed, minS = 0.0, KpS = 0.08, KiS = 0.01, KdS = 0.005;double dtS = 0.1, maxS = maxSpeed, minS = 0.0, KpS = 0.1, KiS = 0.02, KdS = 0.1;void odomCallback(const nav_msgs::Odometry odom_msg) {tf::pointMsgToTF(odom_msg.pose.pose.position, Odom_pos);Odom_yaw = tf::getYaw(odom_msg.pose.pose.orientation);Odom_v = odom_msg.twist.twist.linear.x;Odom_w = odom_msg.twist.twist.angular.z;}void displayLane(bool isTrajectoryPushed, Geometry &geometry) {static visualization_msgs::Marker path;path.type = visualization_msgs::Marker::LINE_STRIP;path.header.frame_id = "odom";path.header.stamp = ros::Time::now();path.ns = "odom";path.id = 0;path.action = visualization_msgs::Marker::ADD;path.lifetime = ros::Duration();path.color.b = 1.0;path.color.a = 1.0;path.scale.x = 0.02;path.pose.orientation.w = 1.0;static int slice_index = 0;VECTOR2D *prev = nullptr, *current = nullptr;while (path.points.size() <= num_slice) {geometry_msgs::Point p;float angle = slice_index * 2 * M_PI / num_slice;slice_index++;p.x = 4 * cos(angle) - 0.5;p.y = 4 * sin(angle) + 1.0;p.z = 0;path.points.push_back(p);if (!isTrajectoryPushed) {VECTOR2D *temp = new VECTOR2D(p.x, p.y);geometry.trajectory.push_back(*temp);current = temp;if (prev != nullptr) {geometry.path.push_back(geometry.getLineSegment(*prev, *current));}prev = current;}}if (prev != nullptr && current != nullptr && current != prev)geometry.path.push_back(geometry.getLineSegment(*prev, *current));marker_pub.publish(path);}int main(int argc, char **argv) {ros::init(argc, argv, "control");ros::NodeHandle n("~");tf::TransformListener m_listener;tf::StampedTransform transform;cmd_vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);odom_sub = n.subscribe("odom", 10, odomCallback);ros::Rate loop_rate(10);geometry_msgs::Twist tw_msg;Geometry geometry;int frame_count = 0;PID pidTheta(dt, maxT, minT, Kp, Kd, Ki);PID pidVelocity(dtS, maxS, minS, KpS, KdS, KiS);double omega = 0.0;while (ros::ok()) {if (frame_count == 0)displayLane(false, geometry);elsedisplayLane(true, geometry);double speed = maxSpeed;VECTOR2D current_pos;current_pos.x = Odom_pos.x();current_pos.y = Odom_pos.y();Geometry::LineSegment *lineSegment = geometry.getNearestLine(current_pos);Geometry::LineSegment lineSegmentPerpen = geometry.getMinimumDistanceLine(*lineSegment, current_pos);Geometry::LineSegment *nextLinesegment = geometry.getNextLineSegment(lineSegment);double targetDistanceWithEnd = geometry.getDistanceSquared(current_pos, lineSegment->endP);double targetDistanceWithStart = geometry.getDistanceSquared(current_pos, lineSegment->startP);double targetAngleWithPerpen = geometry.getGradient(current_pos, lineSegment->endP);double angleError = 0.0;VECTOR2D target = lineSegment->endP;double targetAngle = geometry.getGradient(current_pos, target);double distanceToClosestPath = abs(lineSegment->disatanceToAObj);double targetAnglePerpen = geometry.getGradient(current_pos, lineSegmentPerpen.endP);if (distanceToClosestPath < distanceConst) {double directional = targetAngle;double discripancy = targetAnglePerpen - directional;discripancy = geometry.correctAngle(discripancy);discripancy = 0.5* discripancy / distanceConst * abs(distanceToClosestPath);double combined = targetAngle + discripancy;angleError = combined - Odom_yaw;} else {angleError = targetAnglePerpen - Odom_yaw;}if (targetDistanceWithEnd < 0.5) {double futureAngleChange = nextLinesegment->gradient - lineSegment->gradient;futureAngleChange = geometry.correctAngle(futureAngleChange);futureAngleChange = futureAngleChange / distanceConst * abs(targetDistanceWithEnd);;double combined = targetAngle + futureAngleChange;angleError = combined - Odom_yaw;} else {angleError = geometry.getGradient(current_pos, lineSegmentPerpen.endP) - Odom_yaw;}double speedError = 0.0;if (targetDistanceWithStart < 0.7 || targetDistanceWithEnd < 0.7) {double targetDistance = std::min(targetDistanceWithStart, targetDistanceWithEnd);speedError = 0.3 * maxSpeed * exp(-abs(targetDistance));speed = pidVelocity.calculate(maxSpeed, -speedError);}angleError = geometry.correctAngle(angleError);double omega = pidTheta.calculate(0, -angleError);ROS_INFO("Odom_yaw %f, Angle Error: %f , omega: %f Speed %f", Odom_yaw, angleError, omega, Odom_v);tw_msg.linear.x = speed;tw_msg.angular.z = omega;cmd_vel_pub.publish(tw_msg);frame_count++;ros::spinOnce();loop_rate.sleep();}return 0;}

·

此处首先利用ROS中Marker工具绘制了一个半径为4的圆，其是有50个点组成的。为了验证PID，此处将50个点全部存储起来，用于当作基于小车当前位置的最近点。所以可以看到此处还调用了odom话题去获取当前位置。

剩下的就是将目标点和当前位置计算一个相对合适的速度和角速度了。实现上非常简单，但是可以在仿真环境下快速验证PID算法对小车控制的作用。

附件

具体内容请参考

- 在ROS上快速验证PID算法 - 古月居 (guyuehome.com)

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

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

相关文章

Electron 读取本地配置 增加缩放功能（ctrl+scroll）

Electron 读取本地配置 增加缩放功能（ctrl+scroll）

最近&#xff0c;一个之前做的electron桌面应用&#xff0c;需要增加两个功能&#xff1b;第一是读取本地的配置文件&#xff0c;然后记载配置文件中的ip地址&#xff1b;第二就是增加缩放功能&#xff1b; 第一&#xff0c;配置本地文件 首先需要在vue工程根目录中&#xff0…
阅读更多...
创建第一个51文件

创建第一个51文件

1.找一个文件目录创建一个main.c 比如我的 F:\my_project\project_of_51\0.first_of_51 什么你不会&#xff1f; 先把这里的文件扩展名打开; 再创建一个文本文件&#xff0c;重新命名为main.c // 修改.c 后弹出一个确认修改的框&#xff0c;选确认即可 2.kei操作: 1&#xf…
阅读更多...
与鲸同行，智领未来！和鲸科技“人工智能+X”学科建设合作交流会（北京站）圆满结束！

与鲸同行，智领未来！和鲸科技“人工智能+X”学科建设合作交流会（北京站）圆满结束！

在国家加快发展新质生产力的大背景下&#xff0c;3月25日下午&#xff0c;和鲸科技 2024 年“人工智能X”学科建设合作交流会&#xff08;北京站&#xff09;暨“AIX”实验室建设与供应商选型座谈会顺利召开。为提供更为集中和专业的讨论环境&#xff0c;本次会议特别采取闭门审…
阅读更多...
fastdfs-通信协议-自定义指令码拓展

fastdfs-通信协议-自定义指令码拓展

文章目录 一、fasdfs 通信协议1. 公共命令码2. 发送给storage server的命令码二、fastdfs storage自定义指令码1. 自定义storage server的命令码：计算文件md5三、参考一、fasdfs 通信协议 官方参考：https://mp.weixin.qq.com/s/lpWEv3NCLkfKmtzKJ5lGzQ FastDFS采用二进制TC…
阅读更多...
c语言例题，逐个打印数字

c语言例题，逐个打印数字

今天来分享个比较简单的程序例题&#xff0c;也是比较经典的一个新手例题&#xff0c;逐个打印输入的数字。我们直接从主函数看起&#xff0c;先定义一个num变量&#xff0c;同时变量的类型是unsigned int&#xff0c;这个类型的意思是无符号的整型变量&#xff0c;unsigned&am…
阅读更多...
代码随想录算法训练营Day39|LC62 不同路径LC63 不同路径II

代码随想录算法训练营Day39|LC62 不同路径LC63 不同路径II

一句话总结&#xff1a;不是太难&#xff0c;状态转移方程好想。 原题链接&#xff1a;62 不同路径 位置为(i, j)的点只能从上面或者左边过来&#xff0c;由此可列出状态转移方程。状态转移方程的初始化为所有第一排和第一列的点都初始化为1即可。 class Solution {public i…
阅读更多...
SD-WAN网络构建要点简述

SD-WAN网络构建要点简述

近年来&#xff0c;SD-WAN已成为企业网络优化的热门选择。SD-WAN代表软件定义广域网&#xff0c;是一种基于软件的网络解决方案&#xff0c;旨在提高企业网络连接的可靠性、安全性和性能。相比传统网络架构&#xff0c;SD-WAN技术通过虚拟化网络通信&#xff0c;利用智能软件和…
阅读更多...
数据结构进阶篇 之 【二叉树顺序存储（堆）】的整体实现讲解（赋完整实现代码）

数据结构进阶篇 之 【二叉树顺序存储（堆）】的整体实现讲解（赋完整实现代码）

做人要谦虚&#xff0c;多听听别人的意见&#xff0c;然后记录下来&#xff0c;看看谁对你有意见 一、二叉树的顺序&#xff08;堆&#xff09;结构及实现 1.二叉树的顺序结构 2.堆的概念及结构 3.堆的实现 3.1 向下调整算法 AdJustDown 3.2 向上调整算法 AdJustUP 3.3 …
阅读更多...
AndroidStudio出现类似 Could not create task ‘:app:ToolOperatorDemo.main()‘. 错误

AndroidStudio出现类似 Could not create task ‘:app:ToolOperatorDemo.main()‘. 错误

先看我们的报错 翻译过来大概意思是:无法创建任务:app:ToolOperatorDemo.main()。 没有找到名称为“main”的源集。 解决方法&#xff1a; 在.idea文件夹下的gradle.xml文件中 <GradleProjectSettings>标签下添加<option name"delegatedBuild" value"f…
阅读更多...
这样使用ChatGPT，效率翻倍不是梦！四大秘诀公开

这样使用ChatGPT，效率翻倍不是梦！四大秘诀公开

随着ChatGPT技术的不断革新&#xff0c;它在我们日常工作中扮演着越来越重要的角色。那么&#xff0c;我们该如何利用ChatGPT来解决工作难题呢&#xff1f; Q1&#xff1a;想要迅速获得ChatGPT的帮助&#xff0c;我们应如何提出问题&#xff1f; 以下是几条高效提问的建议&…
阅读更多...
前端学习＜二＞CSS基础——11-CSS3属性详解（一）

前端学习＜二＞CSS基础——11-CSS3属性详解（一）

前言 我们在上一篇文章中学习了CSS3的选择器&#xff0c;本文来学一下CSS3的一些属性。 本文主要内容&#xff1a; 文本 盒模型中的 box-sizing 属性 处理兼容性问题&#xff1a;私有前缀 边框 背景属性 渐变 文本 text-shadow&#xff1a;设置文本的阴影 格式举例&…
阅读更多...
FPGA芯片在通信基站中的作用

FPGA芯片在通信基站中的作用

基站作为移动通信系统的核心组成部分&#xff0c;承担着信号的发送和接收任务&#xff0c;包括天线、射频前端、数字信号处理和控制等功能。 随着通信技术不断进步和网络容量的提升&#xff0c;基站功能日益复杂&#xff0c;数量也在增加。 与此同时&#xff0c;FPGA芯片被广…
阅读更多...
@EnableWebMvc 导致自定义序列化器失效

@EnableWebMvc 导致自定义序列化器失效

目录 前言 一. 自定义序列化器失效 1.1 EnableWebMvc 的作用 1.2 EnableWebMvc 带来了什么后果 1.3 原理分析 1.4 问题解决 二. 总结 前言 在使用Swagger的时候用 到了EnableWebMvc&#xff0c;发现之前为了解决Long类型、日期类型等自定义序列化器失效了 Configurati…
阅读更多...
【送书福利第六期】：《AI绘画教程：Midjourney使用方法与技巧从入门到精通》

【送书福利第六期】：《AI绘画教程：Midjourney使用方法与技巧从入门到精通》

文章目录 一、《AI绘画教程&#xff1a;Midjourney使用方法与技巧从入门到精通》二、内容介绍三、作者介绍&#x1f324;️粉丝福利 一、《AI绘画教程&#xff1a;Midjourney使用方法与技巧从入门到精通》 一本书读懂Midjourney绘画&#xff0c;让创意更简单&#xff0c;让设计…
阅读更多...
如何在Linux系统部署ONLYOFFICE协作办公利器并实现多人实时编辑文档

如何在Linux系统部署ONLYOFFICE协作办公利器并实现多人实时编辑文档

文章目录 1. 安装Docker2. 本地安装部署ONLYOFFICE3. 安装cpolar内网穿透4. 固定OnlyOffice公网地址 本篇文章讲解如何使用Docker在本地服务器上安装ONLYOFFICE&#xff0c;并结合cpolar内网穿透实现公网访问。 Community Edition允许您在本地服务器上安装ONLYOFFICE文档&…
阅读更多...
Makefile：运行流程解析（五）

Makefile：运行流程解析（五）

当输入make并且进行回车时&#xff0c;首先回去检查对应的目录下是否有Makefile或者makefile文件如果有文件那么执行指定的目标&#xff08;没有指定目标默认第一个目标为最终目标&#xff09;&#xff0c;如果没有对应的文件&#xff0c;报错执行结束对于最终的目标首选检查是…
阅读更多...
【回溯与邻里交换】纸牌三角

【回溯与邻里交换】纸牌三角

1.回溯算法 旋转有3种可能&#xff0c;镜像有2种 所以最后次数&#xff1a;counts/3/2 #include<iostream> using namespace std;int num[9]; int counts0; bool bools[9];//默认为false int dfs(int step){if(step9){//索引 if((num[0]num[1]num[2]num[3]num[3]num[4]n…
阅读更多...
【御控物联】JavaScript JSON结构转换（12）：对象To数组——键值互换

【御控物联】JavaScript JSON结构转换（12）：对象To数组——键值互换

文章目录 一、JSON结构转换是什么&#xff1f;二、核心构件之转换映射三、案例之《JSON对象 To JSON数组》四、代码实现五、在线转换工具六、技术资料 一、JSON结构转换是什么&#xff1f; JSON结构转换指的是将一个JSON对象或JSON数组按照一定规则进行重组、筛选、映射或转换…
阅读更多...
Java类与对象：从概念到实践的全景解析！

Java类与对象：从概念到实践的全景解析！

​ 个人主页&#xff1a;秋风起&#xff0c;再归来~ 文章专栏&#xff1a;javaSE的修炼之路 个人格言&#xff1a;悟已往之不谏&#xff0c;知来者犹可追 克心守己&#xff0c;律己则安&#xff01; 1、类的定义格式 在java中定义类时需要用到…
阅读更多...
LabVIEW车载轴承振动监测系统

LabVIEW车载轴承振动监测系统

LabVIEW车载轴承振动监测系统 随着汽车工业的快速发展&#xff0c;车用轴承的稳定性和可靠性对保障车辆安全运行越来越重要。目前&#xff0c;大多数车用轴承工作在恶劣的环境下&#xff0c;容易出现各种故障。开发了一种基于LabVIEW的车载轴承振动监测系统&#xff0c;提高车…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023