麦克纳姆轮的运动控制，听起来好像很高深，但其实拆开来理解，就是几个关键模块的组合。从接收一个底盘速度指令，到最终四个轮子以精确的转速转动，中间经历了运动学解算、PID调节、平滑处理，再到更高阶的轨迹跟随。把这套逻辑理清了，能避免不少实际开发中的坑。

先从基础速度控制说起。

1. 基础速度控制

class MecanumBaseController:
    def __init__(self):
        # 运动学参数
        self.kinematics = MecanumKinematics(
            wheel_radius=0.05,
            lx=0.15,  # 假设底盘长30cm
            ly=0.15   # 假设底盘宽30cm
        )
        # 四个电机的控制器
        self.motor_ctrl = [
            MotorController(1, kp=1.5, ki=0.2, kd=0.1),
            MotorController(2, kp=1.5, ki=0.2, kd=0.1),
            MotorController(3, kp=1.5, ki=0.2, kd=0.1),
            MotorController(4, kp=1.5, ki=0.2, kd=0.1)
        ]
        # 状态变量
        self.current_speeds = [0, 0, 0, 0]  # 四个轮子当前转速
        self.target_speeds = [0, 0, 0, 0]   # 目标转速

    def set_chassis_velocity(self, vx, vy, omega):
        """设置底盘速度"""
        # 1. 运动学转换
        w1, w2, w3, w4 = self.kinematics.chassis_to_wheel(vx, vy, omega)
        # 2. 转换为RPM
        self.target_speeds = [
            w1 * 60 / (2 * math.pi),  # rad/s 转 RPM
            w2 * 60 / (2 * math.pi),
            w3 * 60 / (2 * math.pi),
            w4 * 60 / (2 * math.pi)
        ]

    def update(self, dt):
        """控制循环更新"""
        pwm_values = []
        # 对每个电机进行PID控制
        for i in range(4):
            pwm = self.motor_ctrl[i].update(
                self.target_speeds[i],
                self.current_speeds[i],
                dt
            )
            pwm_values.append(pwm)
        return pwm_values

    def update_encoder_feedback(self, encoder_counts):
        """更新编码器反馈"""
        # 将编码器计数转换为转速
        # 假设编码器每转产生N个脉冲
        encoder_cpr = 1000  # 编码器每转脉冲数
        for i in range(4):
            # 简化计算：rpm = (counts_per_second * 60) / cpr
            self.current_speeds[i] = encoder_counts[i] * 60 / encoder_cpr

从这个类可以看到，核心逻辑其实很清晰：拿到一个底盘的目标速度（vx, vy, omega），先通过运动学模型算出每个轮子需要的转速（rad/s），再转成电机更熟悉的单位（RPM），最后丢给PID去执行。每个轮子独立控制，互不干扰——这正是麦克纳姆轮方案的精髓。值得注意的是，PID参数（kp=1.5, ki=0.2, kd=0.1）只是案例值，实际调参时一定要根据电机响应特性和底盘负载做针对性调整，直接照搬很容易出问题。

2. 运动控制

class SmoothMecanumController:
    def __init__(self):
        # 速度限制
        self.max_linear_accel = 2.0   # 最大线加速度 m/s²
        self.max_angular_accel = 5.0  # 最大角加速度 rad/s²
        # 当前速度
        self.current_vx = 0
        self.current_vy = 0
        self.current_omega = 0
        # 目标速度
        self.target_vx = 0
        self.target_vy = 0
        self.target_omega = 0

    def set_smooth_velocity(self, target_vx, target_vy, target_omega, dt):
        """平滑设置速度，避免急加速"""
        # 计算加速度
        accel_vx = (target_vx - self.current_vx) / dt
        accel_vy = (target_vy - self.current_vy) / dt
        accel_omega = (target_omega - self.current_omega) / dt
        # 限制加速度
        accel_vx = max(-self.max_linear_accel, min(self.max_linear_accel, accel_vx))
        accel_vy = max(-self.max_linear_accel, min(self.max_linear_accel, accel_vy))
        accel_omega = max(-self.max_angular_accel, min(self.max_angular_accel, accel_omega))
        # 更新速度
        self.current_vx += accel_vx * dt
        self.current_vy += accel_vy * dt
        self.current_omega += accel_omega * dt
        return self.current_vx, self.current_vy, self.current_omega

如果直接把目标速度丢给底层，底盘会“哐”一下窜出去，姿态相当难看。这个平滑控制器的存在，就是为了做速度的软着陆。它的逻辑并不复杂：每次更新时，算一下当前速度与目标速度的差距，然后强制限制加速度，把变化幅度控制在一个合理范围内。说白了，就是一个“防急加速”的缓冲层。实际应用中，max_linear_accel设到1.5~2.0 m/s²就是个不错的起点，太大会失去平滑效果，太小则显得底盘反应迟钝。

3. 轨迹跟踪

class TrajectoryTracker:
    def __init__(self):
        # 位姿
        self.x = 0       # 当前位置x
        self.y = 0       # 当前位置y
        self.theta = 0   # 当前角度
        # 控制参数
        self.k_linear = 2.0  # 线速度增益
        self.k_angular = 3.0 # 角速度增益

    def follow_trajectory(self, target_x, target_y, target_theta=None):
        """跟踪轨迹到目标点"""
        # 计算位置误差
        dx = target_x - self.x
        dy = target_y - self.y
        # 计算距离和角度误差
        distance = math.sqrt(dx**2 + dy**2)
        if distance < 0.01:
            # 到达目标点
            return 0, 0, 0

        # 计算目标方向
        target_heading = math.atan2(dy, dx)
        # 角度误差（归一化到[-π, π]）
        heading_error = target_heading - self.theta
        while heading_error > math.pi:
            heading_error -= 2 * math.pi
        while heading_error < -math.pi:
            heading_error += 2 * math.pi

        # 如果指定了目标角度，还要考虑最终角度误差
        final_angle_error = 0
        if target_theta is not None:
            final_angle_error = target_theta - self.theta
            while final_angle_error > math.pi:
                final_angle_error -= 2 * math.pi
            while final_angle_error < -math.pi:
                final_angle_error += 2 * math.pi

        # 控制律
        # 线速度：距离越近速度越慢
        linear_vel = min(0.5, distance * self.k_linear)
        # 角速度：转向目标方向
        angular_vel = heading_error * self.k_angular

        # 接近目标时，如果需要，调整最终角度
        if distance < 0.1 and target_theta is not None:
            angular_vel = final_angle_error * self.k_angular
            linear_vel = 0

        # 将线速度分解为vx, vy
        vx = linear_vel * math.cos(self.theta)
        vy = linear_vel * math.sin(self.theta)
        return vx, vy, angular_vel

到了轨迹跟踪这层，控制逻辑就更直观了——本质上就是个“追点”的过程。计算当前位置与目标点的距离和朝向偏差，然后算出一个合适的速度指令，让车边转边走。关键的点在于：当很接近目标点时，线速度要降下来，把控制权交给最終的角度调整。这个“先走位、再调姿”的思路，在工程实现中非常实用。算法里的k_linear=2.0和k_angular=3.0是典型增益取值，但如果底盘较重或地面摩擦系数低，线速度增益建议再保守一点，比如调到1.2左右，防止到位时过冲。