从开环到闭环的旅程-CoCube

时间：2022-12-10 12:00:03

差动驱动机器人轨迹-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 <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：

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

然后运行即可。

从开环到闭环的旅程-CoCube

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

标签：

相关文章