From b793d2d0021e5dac068ae2a1c310ea7c03e79244 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E7=90=89=E7=92=83=E6=9C=88=E5=85=89?= <15630071+llyg777@user.noreply.gitee.com> Date: Fri, 5 Sep 2025 14:59:40 +0800 Subject: [PATCH] =?UTF-8?q?=E8=BF=90=E5=8A=A8=E5=AD=A6=E4=BB=BF=E7=9C=9F?= =?UTF-8?q?=E6=9B=B2=E7=BA=BF=E8=BF=90=E5=8A=A8=E7=AD=89=E6=9B=B4=E6=96=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../car_controller_pkg/car_controller.py | 104 +++++++--- .../car_controller_pkg/car_controller_line.py | 186 ++++++++++++++++++ 2 files changed, 262 insertions(+), 28 deletions(-) create mode 100644 src/car_controller_pkg/car_controller_pkg/car_controller_line.py diff --git a/src/car_controller_pkg/car_controller_pkg/car_controller.py b/src/car_controller_pkg/car_controller_pkg/car_controller.py index d60fbc7..058076c 100644 --- a/src/car_controller_pkg/car_controller_pkg/car_controller.py +++ b/src/car_controller_pkg/car_controller_pkg/car_controller.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 """ -双对角舵轮机器人曲线运动控制（精简验证版） -仅实现：给定 v_x 和 omega_z，控制两轮转向角和速度 +双对角舵轮机器人曲线运动控制 +实现：给定 v_x/ v_x,v_y 和 omega_z，控制两轮转向角和速度 """ import rclpy from rclpy.node import Node @@ -12,6 +12,7 @@ from rclpy.action import ActionClient import math import time + class CurveMotionValidator(Node): def __init__(self): super().__init__('curve_motion_validator') @@ -63,7 +64,7 @@ class CurveMotionValidator(Node): return False self.get_logger().info( - f"✅ 转向设置: RF={math.degrees(angle_rf):+.1f}°, LB={math.degrees(angle_lb):+.1f}°" + f"转向设置: RF={math.degrees(angle_rf):+.1f}°, LB={math.degrees(angle_lb):+.1f}°" ) return True @@ -72,7 +73,7 @@ class CurveMotionValidator(Node): msg = Float64MultiArray() msg.data = [speed_rf, speed_lb] self.drive_pub.publish(msg) - self.get_logger().info(f"⚙️ 驱动速度: RF={speed_rf:.2f}, LB={speed_lb:.2f}") + self.get_logger().info(f"驱动速度: RF={speed_rf:.2f}, LB={speed_lb:.2f}") def follow_curve(self, v_x, omega_z): """ @@ -80,35 +81,78 @@ class CurveMotionValidator(Node): :param v_x: 车体 x 方向线速度 (m/s) :param omega_z: 车体角速度 (rad/s)，正数左转，负数右转 """ - self.get_logger().info(f"🌀 开始曲线运动: v_x={v_x:.2f}, ω_z={omega_z:.2f}") + self.get_logger().info(f"开始曲线运动: v_x={v_x:.2f}, ω_z={omega_z:.2f}") # === 1. 计算转向角度 === - # 右前轮 (RF): (L/2, W/2) + # 右前轮 (RF): (L/2, W/2) = (0.33, 0.145) + x_rf, y_rf = self.HALF_BASE, self.HALF_TRACK + theta_rf = math.atan2(omega_z * x_rf, v_x - omega_z * y_rf) + + # 左后轮 (LB): (-L/2, -W/2) = (-0.33, -0.145) + x_lb, y_lb = -self.HALF_BASE, -self.HALF_TRACK + theta_lb = math.atan2(omega_z * x_lb, v_x - omega_z * y_lb) + + # 设置转向 + success = self.set_steer_angles(theta_rf, theta_lb, duration=0.3) + if not success: + return + time.sleep(0.5) # 等待转向到位 + + # === 2. 计算驱动速度 === + # 公式: v = v_x * cosθ + ω_z * (x*sinθ - y*cosθ) + def compute_wheel_speed(v_x, omega_z, x, y, steer_angle): + return v_x * math.cos(steer_angle) + omega_z * (x * math.sin(steer_angle) - y * math.cos(steer_angle)) + + v_rf = compute_wheel_speed(v_x, omega_z, x_rf, y_rf, theta_rf) + v_lb = compute_wheel_speed(v_x, omega_z, x_lb, y_lb, theta_lb) + + # === 3. 转为电机角速度 === + omega_rf = v_rf / self.WHEEL_RADIUS + omega_lb = v_lb / self.WHEEL_RADIUS + + # 发送驱动速度 + self.set_drive_speeds(omega_rf, omega_lb) + + def follow_curve_all(self, v_x, v_y, omega_z): + """ + 核心函数：给定车体速度和角速度，控制全向运动 + :param v_x: 车体 x 方向线速度 (m/s) + :param v_y: 车体 y 方向线速度 (m/s) + :param omega_z: 车体角速度 (rad/s) + """ + self.get_logger().info(f"开始全向运动: v_x={v_x:.2f}, v_y={v_y:.2f}, ω_z={omega_z:.2f}") + + # === 1. 计算转向角度（支持 v_y）=== + x_rf, y_rf = self.HALF_BASE, self.HALF_TRACK theta_rf = math.atan2( - omega_z * self.HALF_BASE, - v_x - omega_z * self.HALF_TRACK + omega_z * x_rf + v_y, + v_x - omega_z * y_rf ) - # 左后轮 (LB): (-L/2, -W/2) + + x_lb, y_lb = -self.HALF_BASE, -self.HALF_TRACK theta_lb = math.atan2( - omega_z * (-self.HALF_BASE), - v_x - omega_z * (-self.HALF_TRACK) + omega_z * x_lb + v_y, + v_x - omega_z * y_lb ) # 设置转向 - self.set_steer_angles(theta_rf, theta_lb, duration=0.3) + success = self.set_steer_angles(theta_rf, theta_lb, duration=0.3) + if not success: + return time.sleep(0.5) # 等待转向到位 - # === 2. 计算轮子线速度大小 === - v_rf = math.sqrt( - (v_x - omega_z * self.HALF_TRACK)**2 + - (omega_z * self.HALF_BASE)**2 - ) - v_lb = math.sqrt( - (v_x + omega_z * self.HALF_TRACK)**2 + - (omega_z * self.HALF_BASE)**2 - ) + # === 2. 计算驱动速度（支持 v_y）=== + def compute_wheel_speed(v_x, v_y, omega_z, x, y, steer_angle): + return ( + v_x * math.cos(steer_angle) + + v_y * math.sin(steer_angle) + + omega_z * (x * math.sin(steer_angle) - y * math.cos(steer_angle)) + ) - # === 3. 转为驱动电机角速度 === + v_rf = compute_wheel_speed(v_x, v_y, omega_z, x_rf, y_rf, theta_rf) + v_lb = compute_wheel_speed(v_x, v_y, omega_z, x_lb, y_lb, theta_lb) + + # === 3. 转为电机角速度 === omega_rf = v_rf / self.WHEEL_RADIUS omega_lb = v_lb / self.WHEEL_RADIUS @@ -124,16 +168,20 @@ def main(): try: # === 验证曲线运动 === # 示例1: 左弧线前进 - controller.follow_curve(v_x=1.5, omega_z=0.8) - time.sleep(10) + # controller.follow_curve(v_x=1.5, omega_z=0.8) + # time.sleep(10) # 示例2: 右弧线前进 - #controller.follow_curve(v_x=1.5, omega_z=-0.6) - #time.sleep(10) + # controller.follow_curve(v_x=1.5, omega_z=-0.6) + # time.sleep(10) # 示例3: 直行（omega_z=0） - #controller.follow_curve(v_x=2.0, omega_z=0.0) - #time.sleep(5) + # controller.follow_curve(v_x=2.0, omega_z=0.0) + # time.sleep(5) + + # 示例4: 斜向转弯（全向运动） + controller.follow_curve_all(v_x=1.0, v_y=0.5, omega_z=0.5) + time.sleep(10) # 最终停止 controller.set_drive_speeds(0.0, 0.0) diff --git a/src/car_controller_pkg/car_controller_pkg/car_controller_line.py b/src/car_controller_pkg/car_controller_pkg/car_controller_line.py new file mode 100644 index 0000000..7dc81cb --- /dev/null +++ b/src/car_controller_pkg/car_controller_pkg/car_controller_line.py @@ -0,0 +1,186 @@ +#!/usr/bin/env python3 +""" +双对角舵轮机器人运动学控制脚本 +基于精确几何关系控制转向角，确保无滑动运动 +""" +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float64MultiArray +from control_msgs.action import FollowJointTrajectory +from trajectory_msgs.msg import JointTrajectoryPoint +from rclpy.action import ActionClient +import math +import time + +class DualSwerveController(Node): + def __init__(self): + super().__init__('dual_swerve_controller') + + # === 机器人参数（根据你的URDF调整）=== + self.WHEEL_BASE = 0.66 # 轴距 L (前后距离) + self.WHEEL_TRACK = 0.29 # 轮距 W (左右距离) + self.HALF_BASE = self.WHEEL_BASE / 2.0 # 0.33 + self.HALF_TRACK = self.WHEEL_TRACK / 2.0 # 0.145 + + # === 计算对角线夹角 === + # 对角线与X轴夹角 alpha + self.alpha = math.atan2(self.HALF_TRACK, self.HALF_BASE) # ≈ 23.7° + self.get_logger().info(f"对角线夹角 alpha = {math.degrees(self.alpha):.1f}°") + + # === 发布器和客户端 === + # 驱动速度发布 + self.drive_pub = self.create_publisher( + Float64MultiArray, + '/drive_group_controller/commands', + 10 + ) + + # 转向控制器 Action 客户端 + self.steer_client = ActionClient( + self, + FollowJointTrajectory, + '/steer_group_controller/follow_joint_trajectory' + ) + + # 等待控制器就绪 + self.get_logger().info("等待转向控制器就绪...") + self.steer_client.wait_for_server() + self.get_logger().info("转向控制器已连接！") + + def set_steer_angles(self, angle_rf, angle_lb, duration=1.0): + """ + 设置两个舵轮的转向角度（弧度） + angle_rf: 右前轮角度 + angle_lb: 左后轮角度 + """ + goal = FollowJointTrajectory.Goal() + goal.trajectory.joint_names = ['Right_wheel_f_joint', 'Left_wheel_back_joint'] + + point = JointTrajectoryPoint() + point.positions = [angle_rf, angle_lb] + point.time_from_start.sec = int(duration) + point.time_from_start.nanosec = int((duration - int(duration)) * 1e9) + goal.trajectory.points = [point] + + future = self.steer_client.send_goal_async(goal) + rclpy.spin_until_future_complete(self, future) + goal_handle = future.result() + + if not goal_handle.accepted: + self.get_logger().error("转向目标被拒绝！") + return False + + self.get_logger().info(f"转向角度已设置: RF={math.degrees(angle_rf):.1f}°, LB={math.degrees(angle_lb):.1f}°") + return True + + def set_drive_speeds(self, speed_rf, speed_lb): + """ + 设置驱动轮速度（rad/s） + """ + msg = Float64MultiArray() + msg.data = [speed_rf, speed_lb] + self.drive_pub.publish(msg) + self.get_logger().info(f"驱动速度: RF={speed_rf:.2f}, LB={speed_lb:.2f}") + + def go_straight(self, speed=2.0): + """ + 直行：两舵轮平行于X轴 + """ + self.get_logger().info("执行：直行") + # 两轮都指向 X 正方向（0弧度） + self.set_steer_angles(0.0, 0.0, duration=1.0) + time.sleep(1.2) # 等待转向到位 + self.set_drive_speeds(speed, speed) + + def rotate_in_place(self, speed=2.0, direction='left'): + """ + 原地旋转：两舵轮垂直于对角线 + direction: 'left' 或 'right' + """ + self.get_logger().info(f"执行：原地{direction}旋转") + + # 方案1：工程实用法（错误） + # if direction == 'left': + # self.set_steer_angles(math.pi/2, -math.pi/2, duration=1.0) + # self.set_drive_speeds(speed, -speed) + # else: + # self.set_steer_angles(-math.pi/2, math.pi/2, duration=1.0) + # self.set_drive_speeds(speed, -speed) + + # 方案2：精确几何法（已启用） + steer_angle = math.pi/2 - self.alpha # ≈ 66.3° + if direction == 'left': + self.set_steer_angles(steer_angle, steer_angle, duration=1.0) + else: + self.set_steer_angles(-steer_angle, -steer_angle, duration=1.0) + self.set_drive_speeds(speed, -speed) + + def turn_arc(self, speed=2.0, turn_angle=math.pi/2): + """ + 弧线转弯：两舵轮同向偏转 + turn_angle: 弧度，正数为左转，负数为右转 + """ + direction = "左" if turn_angle > 0 else "右" + self.get_logger().info(f" 执行：{direction}转弯斜行") + + self.set_steer_angles(turn_angle, turn_angle, duration=1.0) + time.sleep(1.2) + self.set_drive_speeds(speed, speed) + + def stop(self): + """停止所有运动""" + self.get_logger().info("停止") + self.set_drive_speeds(0.0, 0.0) + + def print_geometry_info(self): + """打印几何参数供调试""" + self.get_logger().info(" 机器人几何参数:") + self.get_logger().info(f" 轴距 (L): {self.WHEEL_BASE:.3f} m") + self.get_logger().info(f" 轮距 (W): {self.WHEEL_TRACK:.3f} m") + self.get_logger().info(f" 对角线夹角 α: {math.degrees(self.alpha):.1f}°") + self.get_logger().info(f" 垂直角度 (90°-α): {math.degrees(math.pi/2 - self.alpha):.1f}°") + + +def main(): + rclpy.init() + controller = DualSwerveController() + + # 打印几何信息 + controller.print_geometry_info() + time.sleep(1) + + try: + # === 测试序列 === + # 1. 直行 10秒 + controller.go_straight(speed=2.0) + time.sleep(10) + + # 2. 停止 + controller.stop() + time.sleep(5) + + # 3. 原地左转 5秒 + controller.rotate_in_place(speed=2.0, direction='left') + time.sleep(20) + + # 4. 停止 + controller.stop() + time.sleep(5) + + # 5. 右转弯 5秒 + controller.turn_arc(speed=2.0, turn_angle=-0.5) + time.sleep(15) + + # 6. 最终停止 + controller.stop() + + except Exception as e: + controller.get_logger().error(f"运行出错: {e}") + finally: + controller.stop() + controller.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main()