ROS环境下如何让TurtleBot围绕受控TurtleBot按指定参数做圆周运动？

一、线速度与角速度的计算逻辑 #

已知受控TurtleBot在待控制机器人坐标系下的位姿为(x, y, θ)，要实现待控制机器人以半径r、恒定角速度ω围绕目标运动，核心计算规则如下：

1. 基础切向线速度：圆周运动的切向线速度满足 v = r * ω，这是保持恒定角速度绕目标旋转的核心线速度值。
2. 距离修正与速度合成：
   • 先计算待控制机器人与目标的实际相对距离 d = sqrt(x² + y²)
   • 若当前距离d不等于目标半径r，需添加径向速度分量修正距离：径向速度 v_r = k_r * (r - d)，其中k_r为比例系数（需根据机器人实际调试，比如0.5）
   • 计算目标在待控制机器人坐标系下的相对角度 α = arctan2(y, x)，将切向速度和径向速度分解为机器人自身坐标系的速度分量：
     • x方向线速度：v_x = v * sin(α) + v_r * cos(α)
     • 若为差分驱动TurtleBot，y方向速度需设为0，此时需通过角速度调整保证路径
3. 角速度设置：
   • 待控制机器人的角速度需包含三部分：恒定圆周角速度ω、目标机器人自身的角速度（由θ的变化率θ_dot获取）、以及朝向切线方向的角度跟踪修正项：ω_robot = ω + θ_dot + k_ω * (α - π/2)，其中k_ω为角度跟踪比例系数（比如1.0），α - π/2是当前朝向与切线方向的偏差
   • 若目标机器人静止（θ_dot=0）且已处于目标半径r上，可简化为：ω_robot = ω + k_ω * (α - π/2)，稳定后角速度可保持为ω

二、ROS中的实现步骤 #

1. 获取相对位姿：
   通过TF变换监听待控制机器人与受控机器人的位姿关系，示例Python代码片段：

   import tf2_ros
   import geometry_msgs.msg
   import math
   import rospy
   
   tf_buffer = tf2_ros.Buffer()
   tf_listener = tf2_ros.TransformListener(tf_buffer)
   rospy.init_node('circle_follower')
   
   try:
       trans = tf_buffer.lookup_transform('tb_follower/base_link', 'tb_target/base_link', rospy.Time(0), rospy.Duration(1.0))
       x = trans.transform.translation.x
       y = trans.transform.translation.y
       # 四元数转欧拉角获取目标姿态θ
       q = trans.transform.rotation
       theta = math.atan2(2*(q.w*q.z + q.x*q.y), 1-2*(q.y*q.y + q.z*q.z))
   except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
       rospy.logwarn("Failed to get target transform")
   

2. 计算并发布速度指令：
   创建Twist消息，填充计算后的速度值并发布到待控制机器人的/cmd_vel话题：

   cmd_vel_pub = rospy.Publisher('/tb_follower/cmd_vel', geometry_msgs.msg.Twist, queue_size=10)
   twist = geometry_msgs.msg.Twist()
   
   # 代入前述公式计算v_x和ω_robot
   d = math.sqrt(x**2 + y**2)
   alpha = math.atan2(y, x)
   k_r = 0.5
   k_omega = 1.0
   v = r * omega  # r和omega为预设的目标半径和角速度
   v_r = k_r * (r - d)
   twist.linear.x = v * math.sin(alpha) + v_r * math.cos(alpha)
   twist.angular.z = omega + k_omega * (alpha - math.pi/2)
   
   cmd_vel_pub.publish(twist)
   

3. 参数调试：
   • 实际运行时需调整k_r和k_ω，避免机器人震荡或响应过慢
   • 可用rqt_plot监控速度指令、相对距离及角度的变化，辅助参数优化

内容的提问来源于stack exchange，提问作者A_Pumpkin