前往小程序，Get更优阅读体验！
立即前往
文档建议反馈控制台
首页
学习
活动
专区
工具
TVP
最新优惠活动
文章/答案/技术大牛
发布
首页
学习
活动
专区
工具
TVP最新优惠活动
返回腾讯云官网
社区首页 >专栏 >从开环到闭环的旅程-CoCube

从开环到闭环的旅程-CoCube

作者头像
zhangrelay
发布于 2022-12-10 14:56:01
2670
发布于 2022-12-10 14:56:01
举报
文章被收录于专栏：机器人课程与技术机器人课程与技术

差动驱动机器人轨迹-CoCube

​

迷宫逃离的问题-CoCube

自由运动和环境限制-CoCube

001，自由运动

002，引出环境

003，对比差异

ROS机器人从起点到终点（四）蓝桥云实践复现

https://live.csdn.net/v/embed/261670

cocube自由运动

机器人也需要一个目标，没有目标就没有方向感。

有了目标，如何从起点运动到终点呢？误差如何呢？

这时候就需要算法啦。

tutorials/move.cpp

代码语言：javascript
复制
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "geometry_msgs/Twist.h"
#include "math.h"
 
#include <sstream>
 
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<geometry_msgs::Twist>("/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：

代码语言：javascript
复制
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 move
  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

然后运行即可。

代码语言：javascript
复制
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

https://live.csdn.net/v/embed/261723

cocube稳定到失控

不能达到目标点，高速振荡！！！复现2、

https://live.csdn.net/v/embed/261725

​

cocube稳定到振荡

不用再复现了，就是个～垃～圾～代码啊……

就这么一个简单的小程序，就能出现如此严重的bug。

测试是非常重要的，并且不可缺少！

创建文件move.cpp（或想要的任何名称）并将其粘贴到源目录中。

了解代码

TurtleBot类

TurtleBot类将包含机器人的所有方面，例如位置姿态、发布者和订阅者、订阅者回调函数和“移动到目标”函数。

订阅者

订阅者将订阅主题“/turtle1/pose”，这是发布实际机器人位置的主题。当收到消息时调用函数update_pose，并将实际位置保存在名为pose的类属性中。

欧氏位置法

该方法将使用先前保存的海龟位置（即自身位置姿势）和参数（即数据）来计算海龟和目标之间的点对点（欧几里得）距离。

比例控制器

为了让机器人移动，将对线速度和角速度使用比例控制。线性速度将由常数乘以机器人和目标位置之间的距离组成，角速度将取决于y轴距离的反正切乘以x轴距离乘以常数。

误差容忍度

必须在=目标点周围创建一个公差区，因为精确达到目标需要非常高的精度。在这段代码中，如果使用一个非常小的精度，海龟会发疯（你可以试试！）。换句话说，代码和模拟器被简化了，所以它不能以完全精确的方式工作。

参考案例Python PID：

代码语言：javascript
复制
#!/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 subscriber
        rospy.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 received
    def callback(self, data):
        self.pose = data
        self.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 distance

    def 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 radians
	goal_pose.theta = goal_theta * PI / 180
	theta_tolerance = theta_tol * PI / 180
	 
        vel_msg = Twist()

	#PID Controller

	#rotate delta_1
	e_theta1_old = 0
	ei_theta1 = 0
	while 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.theta
	    ed_theta1 = en_theta1 - e_theta1_old
	    ei_theta1 = ei_theta1 + en_theta1 
	    #linear velocity 
            vel_msg.linear.x = 0
            vel_msg.linear.y = 0
            vel_msg.linear.z = 0

            #angular velocity in the z-axis:
            vel_msg.angular.x = 0
            vel_msg.angular.y = 0
            vel_msg.angular.z = p_r * en_theta1 + i_r * ei_theta1 + d_r * ed_theta1

            #Publishing our vel_msg
            self.velocity_publisher.publish(vel_msg)
            self.rate.sleep()
	    
	    e_theta1_old = en_theta1
        #Stopping our robot after the movement is over
        vel_msg.angular.z = 0
        self.velocity_publisher.publish(vel_msg)


	#delta translate
	e_translatn_old = 0
	ei_translatn = 0
        while 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_old
	    ei_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_translatn
            vel_msg.linear.y = 0
            vel_msg.linear.z = 0

            #angular velocity in the z-axis:
            vel_msg.angular.x = 0
            vel_msg.angular.y = 0
            vel_msg.angular.z = 0

            #Publishing our vel_msg
            self.velocity_publisher.publish(vel_msg)
            self.rate.sleep()
	    e_translatn_old = en_translatn	    

        #Stopping our robot after the movement is over
        vel_msg.linear.x = 0
        self.velocity_publisher.publish(vel_msg)

	#delta rotation 2
	e_theta2_old = 0
	ei_theta2 = 0
	while abs(goal_pose.theta - self.pose.theta) >= theta_tolerance:
	    en_theta2 = goal_pose.theta - self.pose.theta
	    ed_theta2 = en_theta2 - e_theta2_old
	    ei_theta2 = ei_theta2 + en_theta2
	    
	    #linear velocity 
            vel_msg.linear.x = 0
            vel_msg.linear.y = 0
            vel_msg.linear.z = 0

            #angular velocity in the z-axis:
            vel_msg.angular.x = 0
            vel_msg.angular.y = 0
            vel_msg.angular.z = p_r * en_theta2 + i_r * ei_theta2 + d_r * ed_theta2

            #Publishing our vel_msg
            self.velocity_publisher.publish(vel_msg)
            self.rate.sleep()
	    e_theta2_old = en_theta2
        #Stopping our robot after the movement is over
        vel_msg.angular.z = 0
        self.velocity_publisher.publish(vel_msg)

        rospy.spin()

if __name__ == '__main__':
    try:
        #Testing our function
        x = turtlebot()
        x.move2goal()

    except rospy.ROSInterruptException: pass
本文参与 腾讯云自媒体分享计划，分享自作者个人站点/博客。
原始发表：2022-12-09，如有侵权请联系 cloudcommunity@tencent.com 删除
机器人
编程算法
https
网络安全
c++

本文分享自 作者个人站点/博客 前往查看

如有侵权，请联系 cloudcommunity@tencent.com 删除。

本文参与 腾讯云自媒体分享计划  ，欢迎热爱写作的你一起参与！

机器人
编程算法
https
网络安全
c++
评论
登录后参与评论
0 条评论
热度
最新
登录 后参与评论
推荐阅读
LV.
文章
0
获赞
0
相关产品与服务
云开发 CloudBase
云开发（Tencent CloudBase，TCB）是腾讯云提供的云原生一体化开发环境和工具平台，为200万+企业和开发者提供高可用、自动弹性扩缩的后端云服务，可用于云端一体化开发多种端应用（小程序、公众号、Web 应用等），避免了应用开发过程中繁琐的服务器搭建及运维，开发者可以专注于业务逻辑的实现，开发门槛更低，效率更高。
免费体验产品介绍产品文档
精选特惠 用云无忧
领券

腾讯云开发者

扫码关注腾讯云开发者

扫码关注腾讯云开发者

领取腾讯云代金券

热门产品

热门推荐

更多推荐

Copyright © 2013 - 2024 Tencent Cloud. All Rights Reserved. 腾讯云 版权所有 

深圳市腾讯计算机系统有限公司 ICP备案/许可证号：粤B2-20090059 深公网安备号 44030502008569

腾讯云计算（北京）有限责任公司 京ICP证150476号 |  京ICP备11018762号 | 京公网安备号11010802020287

问题归档专栏文章快讯文章归档关键词归档开发者手册归档开发者手册 Section 归档

Copyright © 2013 - 2024 Tencent Cloud.

All Rights Reserved. 腾讯云 版权所有

登录 后参与评论