devbox
小蓝xlanll
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

ros之pid_ros参数p i d参数分别是

作者：小蓝xlanll | 2024-06-04 11:22:53

赞

踩

ros参数p i d参数分别是

PID口诀

参数整定找最佳，从小到[大顺序查 先是比例后积分，最后再把微分加 曲线振荡很频繁，比例度盘要放大 曲线漂浮绕大湾，比例度盘往小扳 曲线偏离回复慢，积分时间往下降 曲线波动周期长，积分时间再加长 曲线振荡频率快，先把微分降下来 动差大来波动慢。微分时间应加长 理想曲线两个波，前高后低4比1 一看二调多分析，调节质量不会低

MainWindow.h

  1. //
  2. // Created by wt on 2020/7/2.
  3. //
  4.  
  5. #ifndef DEMO_SWEEPING_ROBOT_MAINWINDOW_H
  6. #define DEMO_SWEEPING_ROBOT_MAINWINDOW_H
  7. #include <QWidget>
  8. #include <QLineEdit>
  9. #include <QPushButton>
  10. #include <QFormLayout>
  11. #include <ros/ros.h>
  12. #include <QLabel>
  13. #include <geometry_msgs/Twist.h>
  14. #include <turtlesim/Pose.h>
  15. #include <std_msgs/Float64.h>
  16. #include <thread>
  17.  
  18. class MainWindow: public QWidget {
  19. private:
  20.     QFormLayout layout;
  21.     QLineEdit xEdit;
  22.     QLineEdit yEdit;
  23.     QLabel xLabel;
  24.     QLabel yLabel;
  25.     QLabel thetaLabel;
  26.     QPushButton btn;
  27.     QPushButton sweepBtn;
  28.     QLineEdit kpEdit;
  29.     QLineEdit kdEdit;
  30.     QLineEdit kiEdit;
  31.     //发布者
  32.     ros::Publisher publisher;
  33.     //plot发布者
  34.     ros::Publisher plotPublisher;
  35.     //订阅者
  36.     ros::Subscriber subscriber;
  37.     //保存的当前x和y
  38.     float curx;
  39.     float cury;
  40.     //小乌龟的角度
  41.     float curTheta;
  42. public:
  43.     MainWindow(ros::NodeHandle node,QWidget* parent = Q_NULLPTR);
  44.  
  45.     ~MainWindow();
  46.     //点击
  47.     void click();
  48.     //扫地
  49.     void sweep();
  50.     //纵向扫地
  51.     void vSweep();
  52.     //求距离
  53.     float distance(float curx,float cury,float dstx,float dsty);
  54.     //回调
  55.     void callBack(const turtlesim::Pose::ConstPtr & pose);
  56.     //控制小乌龟
  57.     void controlTurtle(float dstx,float dsty);
  58.     //计算需要转的角度
  59.     float caclAngle(float dstx,float dsty,float curx,float cury,float theta);
  60. };
  61.  
  62.  
  63. #endif //DEMO_SWEEPING_ROBOT_MAINWINDOW_H

MainWindow.cpp

  1. //
  2. // Created by wt on 2020/7/2.
  3. //
  4.  
  5. #include "MainWindow.h"
  6.  
  7. MainWindow::MainWindow(ros::NodeHandle node, QWidget *parent) : QWidget(parent),
  8. btn("执行"),sweepBtn("扫地") {
  9.     //发布者
  10.     publisher = node.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 5);
  11.     //订阅者
  12.     subscriber = node.subscribe("/turtle1/pose", 5, &MainWindow::callBack, this);
  13.     //plot发布者
  14.     plotPublisher = node.advertise<std_msgs::Float64>("/tutle/speed", 5);
  15.  
  16.     setLayout(&layout);
  17.     //默认值
  18.     xEdit.setText("5.544444561");
  19.     yEdit.setText("5.544444561");
  20.     layout.addRow("目标x:", &xEdit);
  21.     layout.addRow("目标y:", &yEdit);
  22.     layout.addRow("当前x:", &xLabel);
  23.     layout.addRow("当前y:", &yLabel);
  24.     layout.addRow("当前theta:", &thetaLabel);
  25.     kpEdit.setText("0.1");
  26.     kdEdit.setText("0.1");
  27.     kiEdit.setText("0.1");
  28.     layout.addRow("kp:",&kpEdit);
  29.     layout.addRow("kd:",&kdEdit);
  30.     layout.addRow("ki:",&kiEdit);
  31.     layout.addRow("", &btn);
  32.     layout.addRow("", &sweepBtn);
  33.  
  34.     //信号和槽绑定
  35.     connect(&btn, &QPushButton::clicked, this, &MainWindow::click);
  36.     connect(&sweepBtn, &QPushButton::clicked, this, &MainWindow::sweep);
  37. }
  38.  
  39. MainWindow::~MainWindow() {
  40.  
  41. }
  42.  
  43. //真实的速度
  44. void MainWindow::click() {
  45.     //当前的x和y
  46.     //目标的x和y
  47.     float dstx = xEdit.text().toFloat();
  48.     float dsty = yEdit.text().toFloat();
  49.     //开启线程执行耗时操作
  50.     new std::thread(&MainWindow::controlTurtle, this, dstx, dsty);
  51. }
  52.  
  53. //转向
  54. void MainWindow::controlTurtle(float dstx, float dsty) {
  55.     //走的距离
  56.     float dst = distance(curx, cury, dstx, dsty);
  57.     //走的时间
  58.     float time = 5.0;
  59.     //调头时间
  60.     float turnTime = time/12;
  61.     //频率
  62.     float hz = 10;
  63.     //频率
  64.     ros::Rate rate(hz);
  65.     //当前速度
  66.     float curSpeed = 0;
  67.     //记录上一次误差
  68.     float lastError = 0;
  69.     //总误差
  70.     float totalError = 0;
  71.     //kp系数
  72.     float kp = kpEdit.text().toFloat();
  73.     //kd系数
  74.     float kd = kdEdit.text().toFloat();
  75.     //ki系数
  76.     float ki = kiEdit.text().toFloat();
  77.  
  78.     geometry_msgs::Twist data;
  79.     while (distance(curx, cury, dstx, dsty) > 0.1) {
  80.         //目标速度
  81.         float tarSpeed = distance(curx, cury, dstx, dsty) / time;
  82.         /*-------------------------- p --------------------------*/
  83.         //误差= 目标速度 - 当前速度
  84.         float pError = tarSpeed - curSpeed;
  85.         //调节速度  当前速度=当前速度 + 误差*kp系数(比例系数)
  86.         curSpeed += pError * kp;
  87.         /*-------------------------- d --------------------------*/
  88.         // dError =  当前误差-记录的上一次误差
  89.         float dError = pError - lastError;
  90.         //   
  91.         curSpeed += dError * kd;
  92.         //记录当前误差
  93.         lastError = pError;
  94.         /*-------------------------- i --------------------------*/
  95.         // 总误差 = 当前误差累加
  96.         totalError += pError;
  97.         curSpeed+=totalError*ki;
  98.  
  99.         //数据
  100.         data.linear.x = curSpeed;
  101.         //角速度
  102.         data.angular.z = caclAngle(dstx,dsty,curx,cury,curTheta)/turnTime;
  103.         //移动 (只会按照当前这个速度走1秒钟)
  104.         publisher.publish(data);
  105.  
  106.         //数据
  107.         std_msgs::Float64 sp;
  108.         sp.data = curSpeed;
  109.         //发送速度给plot展示
  110.         plotPublisher.publish(sp);
  111.         //剩下的时间
  112.         time -= 1 / hz;
  113.         //睡眠
  114.         rate.sleep();
  115.     }
  116.     //把速度设置为0
  117.     //数据
  118.     data.linear.x = 0;
  119.     data.angular.z = 0;
  120.     //移动 (只会按照当前这个速度走1秒钟)
  121.     publisher.publish(data);
  122. }
  123.  
  124. //D算法
  125. //void MainWindow::controlTurtle(float dstx, float dsty) {
  126. //    //走的距离
  127. //    float dst = distance(curx, cury, dstx, dsty);
  128. //    //走的时间
  129. //    float time = 5.0;
  130. //    //频率
  131. //    float hz = 10;
  132. //    //频率
  133. //    ros::Rate rate(hz);
  134. //    //当前速度
  135. //    float curSpeed = 0;
  136. //    //记录上一次误差
  137. //    float lastError = 0;
  138. //    //总误差
  139. //    float totalError = 0;
  140. //    //kp系数
  141. //    float kp = kpEdit.text().toFloat();
  142. //    //kd系数
  143. //    float kd = kdEdit.text().toFloat();
  144. //    //ki系数
  145. //    float ki = kiEdit.text().toFloat();
  146. //
  147. //    geometry_msgs::Twist data;
  148. //    while (distance(curx, cury, dstx, dsty) > 0.1) {
  149. //        //目标速度
  150. //        float tarSpeed = distance(curx, cury, dstx, dsty) / time;
  151. //        /*-------------------------- p --------------------------*/
  152. //        //误差
  153. //        float pError = tarSpeed - curSpeed;
  154. //        //调节速度
  155. //        curSpeed += pError * kp;
  156. //        /*-------------------------- d --------------------------*/
  157. //        float dError = pError - lastError;
  158. //        curSpeed += dError * kd;
  159. //        lastError = pError;
  160. //        /*-------------------------- i --------------------------*/
  161. //        totalError += pError;
  162. //        curSpeed+=totalError*ki;
  163. //
  164. //        //数据
  165. //        data.linear.x = curSpeed;
  166. //        data.angular.z = 0;
  167. //        //移动 (只会按照当前这个速度走1秒钟)
  168. //        publisher.publish(data);
  169. //
  170. //        //数据
  171. //        std_msgs::Float64 sp;
  172. //        sp.data = curSpeed;
  173. //        //发送速度给plot展示
  174. //        plotPublisher.publish(sp);
  175. //        //剩下的时间
  176. //        time -= 1 / hz;
  177. //        //睡眠
  178. //        rate.sleep();
  179. //    }
  180. //    //把速度设置为0
  181. //    //数据
  182. //    data.linear.x = 0;
  183. //    data.angular.z = 0;
  184. //    //移动 (只会按照当前这个速度走1秒钟)
  185. //    publisher.publish(data);
  186. //}
  187.  
  188.  
  189. //P算法
  190. //void MainWindow::controlTurtle(float dstx, float dsty) {
  191. //    //走的距离
  192. //    float dst = distance(curx,cury,dstx,dsty);
  193. //    //走的时间
  194. //    float time = 5.0;
  195. //    //频率
  196. //    float hz = 10;
  197. //    //频率
  198. //    ros::Rate rate(hz);
  199. //    //当前速度
  200. //    float curSpeed = 0;
  201. //    //kp系数
  202. //    float kp = 0.1;
  203. //
  204. //    geometry_msgs::Twist data;
  205. //    while (distance(curx,cury,dstx,dsty)>0.1){
  206. //        //目标速度
  207. //        float tarSpeed = distance(curx,cury,dstx,dsty)/time;
  208. //        //误差
  209. //        float pError = tarSpeed-curSpeed;
  210. //        //调节速度
  211. //        curSpeed += pError*kp;
  212. //
  213. //        //数据
  214. //        data.linear.x = curSpeed;
  215. //        data.angular.z = 0;
  216. //        //移动 (只会按照当前这个速度走1秒钟)
  217. //        publisher.publish(data);
  218. //
  219. //        //数据
  220. //        std_msgs::Float64 sp;
  221. //        sp.data = curSpeed;
  222. //        //发送速度给plot展示
  223. //        plotPublisher.publish(sp);
  224. //        //剩下的时间
  225. //        time -= 1/hz;
  226. //        //睡眠
  227. //        rate.sleep();
  228. //    }
  229. //    //把速度设置为0
  230. //    //数据
  231. //    data.linear.x = 0;
  232. //    data.angular.z = 0;
  233. //    //移动 (只会按照当前这个速度走1秒钟)
  234. //    publisher.publish(data);
  235. //}
  236.  
  237. //计算速度
  238. //void MainWindow::controlTurtle(float dstx, float dsty) {
  239. //    //走的距离
  240. //    float dst = distance(curx,cury,dstx,dsty);
  241. //    //走的时间
  242. //    float time = 5.0;
  243. //    //频率
  244. //    float hz = 10;
  245. //    //频率
  246. //    ros::Rate rate(hz);
  247. //    //速度
  248. //    float speed = dst/time;
  249. //    geometry_msgs::Twist data;
  250. //    while (distance(curx,cury,dstx,dsty)>0.1){
  251. //        speed = distance(curx,cury,dstx,dsty)/time;
  252. //        //数据
  253. //        data.linear.x = speed;
  254. //        data.angular.z = 0;
  255. //        //移动 (只会按照当前这个速度走1秒钟)
  256. //        publisher.publish(data);
  257. //
  258. //        //数据
  259. //        std_msgs::Float64 sp;
  260. //        sp.data = speed;
  261. //        //发送速度给plot展示
  262. //        plotPublisher.publish(sp);
  263. //        //剩下的时间
  264. //        time -= 1/hz;
  265. //        //睡眠
  266. //        rate.sleep();
  267. //    }
  268. //    //把速度设置为0
  269. //    //数据
  270. //    data.linear.x = 0;
  271. //    data.angular.z = 0;
  272. //    //移动 (只会按照当前这个速度走1秒钟)
  273. //    publisher.publish(data);
  274. //}
  275.  
  276. //指定时间走完
  277. //void MainWindow::click() {
  278. //    //当前的x和y
  279. //    //目标的x和y
  280. //    float dstx = xEdit.text().toFloat();
  281. //    float dsty = yEdit.text().toFloat();
  282. //    //走的距离
  283. //    float dst = distance(curx,cury,dstx,dsty);
  284. //    //走的时间
  285. //    float time = 5.0;
  286. //    //频率
  287. //    float hz = 10;
  288. //    //频率
  289. //    ros::Rate rate(hz);
  290. //    //速度
  291. //    float speed = dst/time;
  292. //    geometry_msgs::Twist data;
  293. //    while (distance(curx,cury,dstx,dsty)>0.1){
  294. //        //数据
  295. //        data.linear.x = speed;
  296. //        data.angular.z = 0;
  297. //        //移动 (只会按照当前这个速度走1秒钟)
  298. //        publisher.publish(data);
  299. //
  300. //        //数据
  301. //        std_msgs::Float64 sp;
  302. //        sp.data = speed;
  303. //        //发送速度给plot展示
  304. //        plotPublisher.publish(sp);
  305. //        //睡眠
  306. //        rate.sleep();
  307. //    }
  308. //    //把速度设置为0
  309. //    //数据
  310. //    data.linear.x = 0;
  311. //    data.angular.z = 0;
  312. //    //移动 (只会按照当前这个速度走1秒钟)
  313. //    publisher.publish(data);
  314. //}
  315.  
  316. //指定时间走完
  317. //void MainWindow::click() {
  318. //    //当前的x和y
  319. //    //目标的x和y
  320. //    float dstx = xEdit.text().toFloat();
  321. //    float dsty = yEdit.text().toFloat();
  322. //    //走的距离
  323. //    float dst = distance(curx,cury,dstx,dsty);
  324. //    //走的时间
  325. //    float time = 5.0;
  326. //    //频率
  327. //    float hz = 10;
  328. //    //频率
  329. //    ros::Rate rate(hz);
  330. //    //速度
  331. //    float speed = dst/time;
  332. //    //走的距离
  333. //    float runDst = 0;
  334. //    geometry_msgs::Twist data;
  335. //    while (runDst<dst){
  336. //        //数据
  337. //        data.linear.x = speed;
  338. //        data.angular.z = 0;
  339. //        //移动 (只会按照当前这个速度走1秒钟)
  340. //        publisher.publish(data);
  341. //        //增加走的距离
  342. //        runDst += speed/hz;
  343. //        //睡眠
  344. //        rate.sleep();
  345. //    }
  346. //    //把速度设置为0
  347. //    //数据
  348. //    data.linear.x = 0;
  349. //    data.angular.z = 0;
  350. //    //移动 (只会按照当前这个速度走1秒钟)
  351. //    publisher.publish(data);
  352. //}
  353.  
  354. //指定时间走完
  355. //void MainWindow::click() {
  356. //    //当前的x和y
  357. //    //目标的x和y
  358. //    float dstx = xEdit.text().toFloat();
  359. //    float dsty = yEdit.text().toFloat();
  360. //    //走的距离
  361. //    float dst = distance(curx,cury,dstx,dsty);
  362. //    //走的时间
  363. //    float time = 5.0;
  364. //    //频率
  365. //    float hz = 10;
  366. //    //频率
  367. //    ros::Rate rate(hz);
  368. //    //速度
  369. //    float speed = dst/time;
  370. //    geometry_msgs::Twist data;
  371. //    for (int i = 0; i < (int)(time*hz); ++i) {
  372. //        //数据
  373. //        data.linear.x = speed;
  374. //        data.angular.z = 0;
  375. //        //移动 (只会按照当前这个速度走1秒钟)
  376. //        publisher.publish(data);
  377. //        //睡眠
  378. //        rate.sleep();
  379. //    }
  380. //    //把速度设置为0
  381. //    //数据
  382. //    data.linear.x = 0;
  383. //    data.angular.z = 0;
  384. //    //移动 (只会按照当前这个速度走1秒钟)
  385. //    publisher.publish(data);
  386. //}
  387.  
  388. //5秒钟走完(问题)
  389. //void MainWindow::click() {
  390. //    //当前的x和y
  391. //    //目标的x和y
  392. //    float dstx = xEdit.text().toFloat();
  393. //    float dsty = yEdit.text().toFloat();
  394. //    //走的距离
  395. //    float dst = distance(curx,cury,dstx,dsty);
  396. //    //走的时间
  397. //    float time = 5;
  398. //    //频率
  399. //    ros::Rate rate(1);
  400. //    //速度
  401. //    float speed = dst/time;
  402. //    for (int i = 0; i < (int)time; ++i) {
  403. //        //数据
  404. //        geometry_msgs::Twist data;
  405. //        data.linear.x = speed;
  406. //        data.angular.z = 0;
  407. //        //移动 (只会按照当前这个速度走1秒钟)
  408. //        publisher.publish(data);
  409. //        //睡眠
  410. //        rate.sleep();
  411. //    }
  412. //}
  413.  
  414. //1秒钟走完
  415. //void MainWindow::click() {
  416. //    //当前的x和y
  417. //    //目标的x和y
  418. //    float dstx = xEdit.text().toFloat();
  419. //    float dsty = yEdit.text().toFloat();
  420. //    //走的距离
  421. //    float dst = distance(curx,cury,dstx,dsty);
  422. //    //走的时间
  423. //    float time = 1;
  424. //    //速度
  425. //    float speed = dst/time;
  426. //    //数据
  427. //    geometry_msgs::Twist data;
  428. //    data.linear.x = speed;
  429. //    data.angular.z = 0;
  430. //    //移动
  431. //    publisher.publish(data);
  432. //}
  433.  
  434. float MainWindow::distance(float curx, float cury, float dstx, float dsty) {
  435.     return sqrt(pow(curx - dstx, 2) + pow(cury - dsty, 2));
  436. }
  437.  
  438. void MainWindow::callBack(const turtlesim::Pose_<std::allocator<void>>::ConstPtr &pose) {
  439.     curx = pose->x;
  440.     cury = pose->y;
  441.     curTheta = pose->theta;
  442.     xLabel.setText(QString::number(curx));
  443.     yLabel.setText(QString::number(cury));
  444.     thetaLabel.setText(QString::number(pose->theta));
  445. }
  446. /**
  447.  * 计算需要转的角度
  448.  * @param dstx
  449.  * @param dsty
  450.  * @param curx
  451.  * @param cury
  452.  * @param theta 小乌龟当前角度
  453.  * @return
  454.  */
  455. float MainWindow::caclAngle(float dstx,float dsty,float curx,float cury,float theta){
  456.     float angle = atan2(dsty-cury,dstx-curx)-theta;
  457.     //角度范围在0-180 -180-0
  458.     if(angle>M_PI){
  459.         angle -= 2*M_PI;
  460.     } else if (angle<-M_PI){
  461.         angle += 2*M_PI;
  462.     }
  463.     return angle;
  464. }
  465.  
  466. void MainWindow::sweep() {
  467.     new std::thread(&MainWindow::vSweep,this);
  468. }
  469.  
  470. void MainWindow::vSweep() {
  471.     for (int i = 1; i < 11; ++i) {
  472.         controlTurtle((float)i,1);
  473.         controlTurtle((float)i,10);
  474.     }
  475. }
  476.  
  477.  
  478.  
  479.  
  480.

main.cpp

  1. //
  2. // Created by wt on 2020/7/2.
  3. //
  4. #include <iostream>
  5. #include <ros/ros.h>
  6. #include <QApplication>
  7. #include "MainWindow.h"
  8. using namespace std;
  9.  
  10. int main(int argc,char *argv[]){
  11.     //节点名
  12.     string nodeName = "sweeping_node";
  13.     //初始化节点
  14.     ros::init(argc,argv,nodeName);
  15.     //创建节点
  16.     ros::NodeHandle node;
  17.     /*-------------------------- 异步spiner --------------------------*/
  18.     ros::AsyncSpinner spinner(1);
  19.     spinner.start();
  20.     /*-------------------------- qt --------------------------*/
  21.     QApplication app(argc,argv);
  22.     MainWindow w(node);
  23.     w.show();
  24.     return QApplication::exec();
  25. }

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/小蓝xlanll/article/detail/671617
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号