从开环到闭环的旅程-CoCube
创始人
2024-04-20 09:17:09
0次
差动驱动机器人轨迹-CoCube
迷宫逃离的问题-CoCube
自由运动和环境限制-CoCube
001，自由运动
002，引出环境
003，对比差异
ROS机器人从起点到终点（四）蓝桥云实践复现
cocube自由运动
机器人也需要一个目标，没有目标就没有方向感。
有了目标，如何从起点运动到终点呢？误差如何呢？
这时候就需要算法啦。
tutorials/move.cpp
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "geometry_msgs/Twist.h"
#include "math.h"#include ros::Subscriber sub;
ros::Publisher pub;
float goal_x = 2;
float goal_y = 2;void sendVel(const turtlesim::Pose::ConstPtr& data){ros::NodeHandle n;pub = n.advertise("/turtle1/cmd_vel",100);float curr_x = data->x;float curr_y = data->y;float curr_ang = data->theta;float dist = sqrt(pow(goal_x-curr_x,2) + pow(goal_y-curr_y,2));std::cout << "Distance = " << dist << std::endl;if(dist > 0.01){double ang = atan2((float)(goal_y-curr_y),(float)(goal_x-curr_x));std::cout << "Curr_ang = " << curr_ang << " | ang = " << ang << std::endl;geometry_msgs::Twist t_msg;t_msg.linear.x = 1.0*(dist);t_msg.angular.z = 4.0*(ang-curr_ang);pub.publish(t_msg);}else{std::cout << "Mission Completed" << std::endl;std::cout << "Please enter new coordinates" << std::endl;std::cout << "Please enter goal_x:" << std::endl;std::cin >> goal_x;std::cout << "Please enter goal_y:" << std::endl;std::cin >> goal_y;}}int main(int argc, char **argv){ros::init(argc,argv,"goToGoal");ros::NodeHandle n;sub = n.subscribe("/turtle1/pose",100,sendVel);ros::spin();return 0;
} 这里，有一个bug，后续解决，问题在一个突变点-pi和pi这个点，当然不止这一个bug。
然后修改CMakelist：
add_executable(move tutorials/move.cpp)
target_link_libraries(move ${catkin_LIBRARIES})
add_dependencies(move turtlesim_gencpp)install(TARGETS turtlesim_node turtle_teleop_key draw_square mimic moveRUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})然后运行即可。
Distance = 0.0793549
Curr_ang = 0.617521 | ang = 0.617525
Distance = 0.0780842
Curr_ang = 0.617522 | ang = 0.617518
Distance = 0.0768352
Curr_ang = 0.617521 | ang = 0.617522
Distance = 0.0756069
Curr_ang = 0.617521 | ang = 0.617519
Distance = 0.0743976
Curr_ang = 0.617521 | ang = 0.617523
Distance = 0.0732063
Curr_ang = 0.617521 | ang = 0.617512
Distance = 0.0720351
Curr_ang = 0.617521 | ang = 0.617528
Distance = 0.0708819
Curr_ang = 0.617521 | ang = 0.617531
Distance = 0.0697493
Curr_ang = 0.617522 | ang = 0.617525
Distance = 0.0686332
Curr_ang = 0.617522 | ang = 0.61752
Distance = 0.0675334
Curr_ang = 0.617522 | ang = 0.617515
Distance = 0.0664527
Curr_ang = 0.617521 | ang = 0.617517
Distance = 0.0653911
Curr_ang = 0.617521 | ang = 0.617527
Distance = 0.0643448
Curr_ang = 0.617522 | ang = 0.617513
Distance = 0.0633133
Curr_ang = 0.617521 | ang = 0.617518
Distance = 0.0623009
Curr_ang = 0.617521 | ang = 0.61753
Distance = 0.0613038
Curr_ang = 0.617521 | ang = 0.617519
Distance = 0.0603242
Curr_ang = 0.617521 | ang = 0.617533
Distance = 0.0593584
Curr_ang = 0.617522 | ang = 0.617541
Distance = 0.0584078
Curr_ang = 0.617523 | ang = 0.617524
Distance = 0.0574748
Curr_ang = 0.617523 | ang = 0.617534
Distance = 0.0565544
Curr_ang = 0.617524 | ang = 0.617509
Distance = 0.05565
Curr_ang = 0.617523 | ang = 0.617532
Distance = 0.0547598
Curr_ang = 0.617524 | ang = 0.6175
Distance = 0.0538829
Curr_ang = 0.617522 | ang = 0.617509
Distance = 0.0530208
Curr_ang = 0.617521 | ang = 0.61754
Distance = 0.0521729
Curr_ang = 0.617522 | ang = 0.617513
Distance = 0.0513383
Curr_ang = 0.617522 | ang = 0.617529
Distance = 0.0505164
Curr_ang = 0.617522 | ang = 0.617506
Distance = 0.0497078
Curr_ang = 0.617521 | ang = 0.617529
Distance = 0.0489129
Curr_ang = 0.617522 | ang = 0.617543
Distance = 0.0481306
Curr_ang = 0.617523 | ang = 0.617518
Distance = 0.0473606
Curr_ang = 0.617523 | ang = 0.617506
Distance = 0.0466027
Curr_ang = 0.617522 | ang = 0.617509
Distance = 0.0458571
Curr_ang = 0.617521 | ang = 0.617527
Distance = 0.0451225
Curr_ang = 0.617521 | ang = 0.617527
Distance = 0.0444017
Curr_ang = 0.617522 | ang = 0.617518
Distance = 0.0436904
Curr_ang = 0.617521 | ang = 0.617514
Distance = 0.0429913
Curr_ang = 0.617521 | ang = 0.617526
Distance = 0.0423032
Curr_ang = 0.617521 | ang = 0.617519
Distance = 0.0416274
Curr_ang = 0.617521 | ang = 0.617528
Distance = 0.0409611
Curr_ang = 0.617522 | ang = 0.617543
Distance = 0.0403059
Curr_ang = 0.617523 | ang = 0.617537
Distance = 0.0396603
Curr_ang = 0.617524 | ang = 0.617538
Distance = 0.0390257
Curr_ang = 0.617525 | ang = 0.617517
Distance = 0.0384018
Curr_ang = 0.617524 | ang = 0.617542
Distance = 0.0377878
Curr_ang = 0.617525 | ang = 0.617503
Distance = 0.0371829
Curr_ang = 0.617524 | ang = 0.617542
Distance = 0.0365881
Curr_ang = 0.617525 | ang = 0.617516
Distance = 0.0360027
Curr_ang = 0.617525 | ang = 0.617497
Distance = 0.0354253
Curr_ang = 0.617523 | ang = 0.617514
Distance = 0.034859
Curr_ang = 0.617522 | ang = 0.617508
Distance = 0.0343023
Curr_ang = 0.617521 | ang = 0.617509
Distance = 0.0337524
Curr_ang = 0.617521 | ang = 0.617504
Distance = 0.0332121
Curr_ang = 0.617519 | ang = 0.617506
Distance = 0.0326812
Curr_ang = 0.617519 | ang = 0.617515
Distance = 0.0321589
Curr_ang = 0.617518 | ang = 0.617485
Distance = 0.0316445
Curr_ang = 0.617516 | ang = 0.617496
Distance = 0.031138
Curr_ang = 0.617515 | ang = 0.617551
Distance = 0.0306389
Curr_ang = 0.617517 | ang = 0.617514
Distance = 0.0301494
Curr_ang = 0.617517 | ang = 0.617484
Distance = 0.0296662
Curr_ang = 0.617515 | ang = 0.617536
Distance = 0.0291931
Curr_ang = 0.617516 | ang = 0.617508
Distance = 0.0287252
Curr_ang = 0.617516 | ang = 0.617509
Distance = 0.0282654
Curr_ang = 0.617515 | ang = 0.617559
Distance = 0.0278128
Curr_ang = 0.617518 | ang = 0.617507
Distance = 0.0273683
Curr_ang = 0.617517 | ang = 0.617503
Distance = 0.0269306
Curr_ang = 0.617516 | ang = 0.617491
Distance = 0.0265009
Curr_ang = 0.617515 | ang = 0.61753
Distance = 0.0260754
Curr_ang = 0.617516 | ang = 0.617545
Distance = 0.0256583
Curr_ang = 0.617518 | ang = 0.61751
Distance = 0.0252477
Curr_ang = 0.617517 | ang = 0.617571
Distance = 0.0248444
Curr_ang = 0.617521 | ang = 0.617519
Distance = 0.0244464
Curr_ang = 0.617521 | ang = 0.617501
Distance = 0.0240552
Curr_ang = 0.617519 | ang = 0.617475
Distance = 0.0236705
Curr_ang = 0.617517 | ang = 0.617552
Distance = 0.0232916
Curr_ang = 0.617519 | ang = 0.617555
Distance = 0.0229196
Curr_ang = 0.617521 | ang = 0.61755
Distance = 0.0225517
Curr_ang = 0.617523 | ang = 0.617516
Distance = 0.0221918
Curr_ang = 0.617523 | ang = 0.617542
Distance = 0.0218362
Curr_ang = 0.617524 | ang = 0.617538
Distance = 0.0214874
Curr_ang = 0.617525 | ang = 0.617525
Distance = 0.0211429
Curr_ang = 0.617525 | ang = 0.617481
Distance = 0.0208047
Curr_ang = 0.617522 | ang = 0.617553
Distance = 0.0204724
Curr_ang = 0.617524 | ang = 0.617542
Distance = 0.0201442
Curr_ang = 0.617525 | ang = 0.617498
Distance = 0.0198214
Curr_ang = 0.617523 | ang = 0.617498
Distance = 0.0195054
Curr_ang = 0.617522 | ang = 0.617487
Distance = 0.0191921
Curr_ang = 0.617519 | ang = 0.6175
Distance = 0.0188857
Curr_ang = 0.617518 | ang = 0.617502
Distance = 0.0185835
Curr_ang = 0.617517 | ang = 0.617469
Distance = 0.0182866
Curr_ang = 0.617514 | ang = 0.617485
Distance = 0.0179939
Curr_ang = 0.617512 | ang = 0.617465
Distance = 0.0177066
Curr_ang = 0.617509 | ang = 0.617495
Distance = 0.0174219
Curr_ang = 0.617508 | ang = 0.617551
Distance = 0.0171429
Curr_ang = 0.617511 | ang = 0.617508
Distance = 0.0168693
Curr_ang = 0.617511 | ang = 0.617516
Distance = 0.0165999
Curr_ang = 0.617511 | ang = 0.617485
Distance = 0.0163343
Curr_ang = 0.617509 | ang = 0.617575
Distance = 0.0160718
Curr_ang = 0.617514 | ang = 0.617532
Distance = 0.0158161
Curr_ang = 0.617515 | ang = 0.617474
Distance = 0.0155616
Curr_ang = 0.617512 | ang = 0.617514
Distance = 0.0153139
Curr_ang = 0.617512 | ang = 0.617542
Distance = 0.0150678
Curr_ang = 0.617514 | ang = 0.617498
Distance = 0.014827
Curr_ang = 0.617513 | ang = 0.617513
Distance = 0.0145904
Curr_ang = 0.617513 | ang = 0.617484
Distance = 0.0143565
Curr_ang = 0.617511 | ang = 0.617485
Distance = 0.0141279
Curr_ang = 0.61751 | ang = 0.61755
Distance = 0.0139009
Curr_ang = 0.617512 | ang = 0.617538
Distance = 0.013678
Curr_ang = 0.617514 | ang = 0.617478
Distance = 0.013459
Curr_ang = 0.617512 | ang = 0.617564
Distance = 0.0132441
Curr_ang = 0.617515 | ang = 0.617604
Distance = 0.0130324
Curr_ang = 0.617521 | ang = 0.617476
Distance = 0.0128244
Curr_ang = 0.617518 | ang = 0.6175
Distance = 0.0126191
Curr_ang = 0.617517 | ang = 0.617561
Distance = 0.0124169
Curr_ang = 0.617519 | ang = 0.617446
Distance = 0.0122184
Curr_ang = 0.617515 | ang = 0.617491
Distance = 0.0120226
Curr_ang = 0.617513 | ang = 0.617575
Distance = 0.0118299
Curr_ang = 0.617517 | ang = 0.617476
Distance = 0.011641
Curr_ang = 0.617515 | ang = 0.617545
Distance = 0.0114537
Curr_ang = 0.617517 | ang = 0.61752
Distance = 0.0112716
Curr_ang = 0.617517 | ang = 0.617574
Distance = 0.0110912
Curr_ang = 0.61752 | ang = 0.617531
Distance = 0.0109133
Curr_ang = 0.617521 | ang = 0.617527
Distance = 0.0107397
Curr_ang = 0.617521 | ang = 0.617462
Distance = 0.0105672
Curr_ang = 0.617518 | ang = 0.617543
Distance = 0.0103989
Curr_ang = 0.617519 | ang = 0.617563
Distance = 0.0102322
Curr_ang = 0.617522 | ang = 0.617476
Distance = 0.0100681
Curr_ang = 0.617519 | ang = 0.617431
Distance = 0.00990781
Mission Completed
Please enter new coordinates
Please enter goal_x:
基于MSRDS机器人仿真平台的多机器人PID编队控制算法
bug点解决的办法如上这篇。
简单试一试看能不能复现出bug。
不能达到目标点，高速暴冲！！！复现1
cocube稳定到失控
不能达到目标点，高速振荡！！！复现2
cocube稳定到振荡
不用再复现了，就是个～垃～圾～代码啊……
就这么一个简单的小程序，就能出现如此严重的bug。
测试是非常重要的，并且不可缺少！
创建文件move.cpp（或想要的任何名称）并将其粘贴到源目录中。
了解代码
TurtleBot类
TurtleBot类将包含机器人的所有方面，例如位置姿态、发布者和订阅者、订阅者回调函数和“移动到目标”函数。
订阅者
订阅者将订阅主题“/turtle1/pose”，这是发布实际机器人位置的主题。当收到消息时调用函数update_pose，并将实际位置保存在名为pose的类属性中。
欧氏位置法
该方法将使用先前保存的海龟位置（即自身位置姿势）和参数（即数据）来计算海龟和目标之间的点对点（欧几里得）距离。
比例控制器
为了让机器人移动，将对线速度和角速度使用比例控制。线性速度将由常数乘以机器人和目标位置之间的距离组成，角速度将取决于y轴距离的反正切乘以x轴距离乘以常数。
误差容忍度
必须在=目标点周围创建一个公差区，因为精确达到目标需要非常高的精度。在这段代码中，如果使用一个非常小的精度，海龟会发疯（你可以试试！）。换句话说，代码和模拟器被简化了，所以它不能以完全精确的方式工作。
参考案例Python PID：
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
from math import pow,atan2,sqrt
PI = 3.1415926535897# PID parameters for rotation
p_r = 3
i_r = 0.00001
d_r = 1# PID parameters for translation
p_t = 1.3
i_t = 0.0001
d_t = 1
class turtlebot():def __init__(self):#Creating our node,publisher and subscriberrospy.init_node('turtlebot_controller', anonymous=True)self.velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)self.pose_subscriber = rospy.Subscriber('/turtle1/pose', Pose, self.callback)self.pose = Pose()self.rate = rospy.Rate(10)#Callback function implementing the pose value receiveddef callback(self, data):self.pose = dataself.pose.x = round(self.pose.x, 4)self.pose.y = round(self.pose.y, 4)def get_distance(self, goal_x, goal_y):distance = sqrt(pow((goal_x - self.pose.x), 2) + pow((goal_y - self.pose.y), 2))return distancedef move2goal(self):goal_pose = Pose()goal_pose.x = input("Set your x goal:")goal_pose.y = input("Set your y goal:")goal_theta = input("Set your theta goal:")distance_tolerance = input("Set your distance tolerance:")theta_tol = input("Set your theta tolerance:")while goal_theta > 360:goal_theta = goal_theta - 360# conerting angles from degrees to radiansgoal_pose.theta = goal_theta * PI / 180theta_tolerance = theta_tol * PI / 180vel_msg = Twist()#PID Controller#rotate delta_1e_theta1_old = 0ei_theta1 = 0while abs(atan2(goal_pose.y - self.pose.y, goal_pose.x - self.pose.x) - self.pose.theta) >= theta_tolerance:en_theta1 = atan2(goal_pose.y - self.pose.y, goal_pose.x - self.pose.x) - self.pose.thetaed_theta1 = en_theta1 - e_theta1_oldei_theta1 = ei_theta1 + en_theta1 #linear velocity vel_msg.linear.x = 0vel_msg.linear.y = 0vel_msg.linear.z = 0#angular velocity in the z-axis:vel_msg.angular.x = 0vel_msg.angular.y = 0vel_msg.angular.z = p_r * en_theta1 + i_r * ei_theta1 + d_r * ed_theta1#Publishing our vel_msgself.velocity_publisher.publish(vel_msg)self.rate.sleep()e_theta1_old = en_theta1#Stopping our robot after the movement is overvel_msg.angular.z = 0self.velocity_publisher.publish(vel_msg)#delta translatee_translatn_old = 0ei_translatn = 0while sqrt(pow((goal_pose.x - self.pose.x), 2) + pow((goal_pose.y - self.pose.y), 2)) >= distance_tolerance:en_translatn = sqrt(pow((goal_pose.x - self.pose.x), 2) + pow((goal_pose.y - self.pose.y), 2))ed_translatn = en_translatn - e_translatn_oldei_translatn = ei_translatn + en_translatn#linear velocity in the x-axis:vel_msg.linear.x = p_t * en_translatn + i_t * ei_translatn + d_t * ed_translatnvel_msg.linear.y = 0vel_msg.linear.z = 0#angular velocity in the z-axis:vel_msg.angular.x = 0vel_msg.angular.y = 0vel_msg.angular.z = 0#Publishing our vel_msgself.velocity_publisher.publish(vel_msg)self.rate.sleep()e_translatn_old = en_translatn #Stopping our robot after the movement is overvel_msg.linear.x = 0self.velocity_publisher.publish(vel_msg)#delta rotation 2e_theta2_old = 0ei_theta2 = 0while abs(goal_pose.theta - self.pose.theta) >= theta_tolerance:en_theta2 = goal_pose.theta - self.pose.thetaed_theta2 = en_theta2 - e_theta2_oldei_theta2 = ei_theta2 + en_theta2#linear velocity vel_msg.linear.x = 0vel_msg.linear.y = 0vel_msg.linear.z = 0#angular velocity in the z-axis:vel_msg.angular.x = 0vel_msg.angular.y = 0vel_msg.angular.z = p_r * en_theta2 + i_r * ei_theta2 + d_r * ed_theta2#Publishing our vel_msgself.velocity_publisher.publish(vel_msg)self.rate.sleep()e_theta2_old = en_theta2#Stopping our robot after the movement is overvel_msg.angular.z = 0self.velocity_publisher.publish(vel_msg)rospy.spin()if __name__ == '__main__':try:#Testing our functionx = turtlebot()x.move2goal()except rospy.ROSInterruptException: pass相关内容
热门资讯
监控摄像头接入GB28181平...
流程简介将监控摄像头的视频在网站和APP中直播，要解决的几个问题是：1&...
Windows10添加群晖磁盘...
在使用群晖NAS时，我们需要通过本地映射的方式把NAS映射成本地的一块磁盘使用。 通过...
protocol buffer...
目录 目录 什么是protocol buffer 1.protobuf 1.1安装 1.2使用...
在Word、WPS中插入AxM...
引言 我最近需要写一些文章，在排版时发现AxMath插入的公式竟然会导致行间距异常&#...
【PdgCntEditor】解...
一、问题背景 大部分的图书对应的PDF，目录中的页码并非PDF中直接索引的页码...
Fluent中创建监测点
1 概述某些仿真问题，需要创建监测点，用于获取空间定点的数据...
MySQL下载和安装（Wind...
前言：刚换了一台电脑，里面所有东西都需要重新配置，习惯了所...
MFC文件操作
MFC提供了一个文件操作的基类CFile，这个类提供了一个没有缓存的二进制格式的磁盘...
有效的括号
一、题目 给定一个只包括 '('，')'，'{'，'}'...