基于 Arduino 的 BLDC 差速驱动机器人（Speed and Steering Control） 是移动机器人领域中最经典、最基础的架构。该系统通过独立控制左右两个 无刷直流电机（BLDC） 的转速，利用速度差来实现机器人的前进、转向和原地旋转。

1、主要特点
差速转向运动学模型 (Differential Kinematics)
这是该系统的核心理论基础，通过简单的数学模型实现了复杂的平面运动。
运动原理：机器人的运动状态完全由左右轮的速度差决定：
直线行驶：机器人沿直线前进或后退。
曲线转向：机器人绕瞬时曲率中心转弯。速度差越大，转弯半径越小。
原地旋转 (Zero-turning)：机器人绕其中心点进行零半径旋转。
阿克曼简化模型：相较于汽车的阿克曼转向结构，差速转向省去了复杂的转向连杆机构，机械结构极其简单，仅需两个独立的驱动轮和辅助支撑轮。
高效动力与闭环控制
采用 BLDC 电机 替代传统的有刷电机，显著提升了机器人的性能边界。
高能效比：BLDC 电机效率通常高达 85%-90%，配合电子调速器（ESC），在频繁启停的差速控制场景下能显著延长电池续航时间。
速度闭环控制：为了实现精准的差速转向，必须构建速度闭环。通过霍尔传感器或外置编码器实时采集轮速，利用 PID 控制算法 动态调整 PWM 占空比，确保左右轮的实际转速严格跟随设定值，克服地面摩擦力不均带来的偏差。
Arduino 平台的灵活性与扩展性
Arduino（特别是高性能型号如 ESP32、Teensy）为差速控制提供了丰富的软硬件资源。
多路 PWM 输出：Arduino 平台支持多路硬件 PWM 输出，可同时生成两路甚至更多路的精确 PWM 信号（通常为 50Hz，脉宽 1000μs-2000μs）来控制两个 ESC。
传感器融合：易于集成 IMU（惯性测量单元） 和 编码器。IMU 提供航向角信息，可用于修正机器人的直线行驶精度（例如，若检测到向右偏航，则适当增加右轮速度进行纠正）。

2、应用场景
教育与竞赛机器人平台
这是最广泛的应用领域。例如全国大学生智能汽车竞赛中的“双轮直立组”或“专科电磁组”，参赛学生利用差速驱动原理，结合陀螺仪进行姿态控制，实现车模的直立行走和快速转向。其结构简单、控制逻辑清晰，非常适合教学和算法验证。
室内服务与巡检机器人
在写字楼、商场、仓库等结构化环境中，差速驱动机器人凭借其原地旋转能力，可以在狭窄的通道内灵活穿梭。常用于送餐机器人、酒店引导机器人或机房巡检机器人，执行自主导航、定点停靠和避障任务。
轻型 AGV (Automated Guided Vehicle)
在自动化仓储物流中，差速驱动 AGV 用于搬运轻型货物。虽然其抗侧滑能力不如全向轮或舵轮结构，但在平整地面上，其成本低廉、维护简单的优点使其在特定场景下具有极高的性价比。
科研与算法验证原型
在机器人学研究中，差速驱动底盘常被用作验证 SLAM（同步定位与地图构建）、路径规划（如 A*、Dijkstra）、视觉伺服等算法的底层硬件平台。开发者无需关心复杂的机械结构，可专注于上层算法的开发。

3、 注意事项
机械结构与安装精度
轮子同轴度：左右驱动轮必须严格保持在同一轴线上，且轮距（Track Width）必须精确测量并用于运动学计算。任何安装偏差都会导致里程计（Odometry）计算错误，造成直线跑偏。
轮胎摩擦系数：建议使用高摩擦系数的橡胶轮胎，防止在光滑地面（如瓷砖、木地板）上发生打滑。若地面环境复杂，可适当增加机器人自重以提高抓地力。
电源管理与抗干扰
独立供电：BLDC 电机启动电流大，且换向时会产生电压波动。必须使用独立的稳压模块（如 LM2596）为 Arduino 逻辑电路供电，严禁直接共用未经稳压的电机电源，防止电机启停导致主控复位。
去耦电容：在 ESC 的电源输入端应就近并联 1000μF - 2200μF 的电解电容，以吸收电压尖峰，保护电调和电池。
控制算法与系统标定
PID 参数整定：差速控制对两个电机的同步性要求较高。建议先单独对左右电机进行 PID 参数整定，确保两者的动态响应特性一致。若 PID 参数差异过大，会导致系统响应迟缓或振荡。
里程计误差补偿：由于轮子直径误差和地面打滑，单纯的编码器里程计会产生累积误差。建议结合 IMU（MPU6050 等） 进行数据融合（如使用互补滤波或卡尔曼滤波），利用 IMU 的航向角信息修正里程计的航向偏差。
BLDC 电调的配置
信号类型匹配：确保使用的 ESC 接受标准的 RC PWM 信号（50Hz）。部分高端 FOC 驱动器可能使用 CAN 或 UART 协议，与 Arduino 的 analogWrite() 或 Servo.writeMicroseconds() 函数不兼容。
行程校准：在首次使用前，必须对 ESC 进行油门行程校准（Throttle Calibration），以确保 Arduino 发送的脉宽范围（如 1000-2000μs）能被 ESC 正确识别为最小和最大油门。

1、基于PWM信号的差速驱动基础控制（2电机独立控制）

// 定义左右电机的PWM引脚和方向引脚
#define LEFT_MOTOR_PWM 12
#define LEFT_MOTOR_DIR 14
#define RIGHT_MOTOR_PWM 13
#define RIGHT_MOTOR_DIR 15

// PWM参数
#define PWM_FREQ 20000  // 高频PWM减少噪声
#define PWM_RES 8       // 8位分辨率（0-255）

void setup() {
  // 配置PWM通道
  ledcSetup(0, PWM_FREQ, PWM_RES);  // 左电机
  ledcSetup(1, PWM_FREQ, PWM_RES);  // 右电机
  ledcAttachPin(LEFT_MOTOR_PWM, 0);
  ledcAttachPin(RIGHT_MOTOR_PWM, 1);

  // 设置方向引脚为输出
  pinMode(LEFT_MOTOR_DIR, OUTPUT);
  pinMode(RIGHT_MOTOR_DIR, OUTPUT);

  // 初始化电机停止
  setMotorSpeed(0, 0);
}

void loop() {
  // 示例1：前进（速度=100）
  setMotorSpeed(100, 100);
  delay(1000);

  // 示例2：右转（左电机速度>右电机）
  setMotorSpeed(150, 50);
  delay(500);

  // 示例3：原地旋转（左电机正转，右电机反转）
  digitalWrite(LEFT_MOTOR_DIR, HIGH);
  digitalWrite(RIGHT_MOTOR_DIR, LOW);
  ledcWrite(0, 150);
  ledcWrite(1, 150);
  delay(1000);

  // 停止
  setMotorSpeed(0, 0);
  delay(1000);
}

// 设置电机速度（-255到255，负值表示反转）
void setMotorSpeed(int leftSpeed, int rightSpeed) {
  // 左电机方向与速度
  digitalWrite(LEFT_MOTOR_DIR, leftSpeed >= 0 ? HIGH : LOW);
  ledcWrite(0, abs(leftSpeed));

  // 右电机方向与速度
  digitalWrite(RIGHT_MOTOR_DIR, rightSpeed >= 0 ? HIGH : LOW);
  ledcWrite(1, abs(rightSpeed));
}

2、基于PS2遥控器的差速驱动控制（解析遥控器输入）

#include <PS2X_lib.h>  // PS2遥控器库

PS2X ps2x;
int error = 0;

// 电机引脚
#define LEFT_MOTOR_PWM 12
#define LEFT_MOTOR_DIR 14
#define RIGHT_MOTOR_PWM 13
#define RIGHT_MOTOR_DIR 15

void setup() {
  Serial.begin(115200);
  
  // 初始化PS2遥控器
  error = ps2x.config_gamepad(18, 19, 23, 21);  // CLK, CMD, ATT, DAT
  if (error != 0) Serial.println("PS2遥控器初始化失败");

  // 初始化PWM和方向引脚
  ledcSetup(0, 20000, 8);
  ledcSetup(1, 20000, 8);
  ledcAttachPin(LEFT_MOTOR_PWM, 0);
  ledcAttachPin(RIGHT_MOTOR_PWM, 1);
  pinMode(LEFT_MOTOR_DIR, OUTPUT);
  pinMode(RIGHT_MOTOR_DIR, OUTPUT);
}

void loop() {
  ps2x.read_gamepad();  // 读取遥控器数据

  // 获取摇杆值（-128 ~ 127）
  int ly = ps2x.Analog(PSS_LY) - 128;  // 左摇杆Y轴（前后）
  int rx = ps2x.Analog(PSS_RX) - 128;  // 右摇杆X轴（转向）

  // 差速控制：速度=Y轴，转向=X轴
  int leftSpeed = ly - rx;  // 左电机速度 = 前进/后退 + 转向
  int rightSpeed = ly + rx; // 右电机速度 = 前进/后退 - 转向

  // 限制速度范围（-255到255）
  leftSpeed = constrain(leftSpeed, -255, 255);
  rightSpeed = constrain(rightSpeed, -255, 255);

  // 设置电机
  digitalWrite(LEFT_MOTOR_DIR, leftSpeed >= 0 ? HIGH : LOW);
  digitalWrite(RIGHT_MOTOR_DIR, rightSpeed >= 0 ? HIGH : LOW);
  ledcWrite(0, abs(leftSpeed));
  ledcWrite(1, abs(rightSpeed));

  delay(50);  // 控制更新频率
}

3、基于ROS的差速驱动控制（串口通信）

#include <ros.h>
#include <geometry_msgs/Twist.h>

ros::NodeHandle nh;

// 电机引脚
#define LEFT_MOTOR_PWM 12
#define LEFT_MOTOR_DIR 14
#define RIGHT_MOTOR_PWM 13
#define RIGHT_MOTOR_DIR 15

// 电机速度变量
float leftSpeed = 0, rightSpeed = 0;

void messageCb(const geometry_msgs::Twist& msg) {
  // 解析ROS Twist消息（线速度和角速度）
  float linear = msg.linear.x;  // 前后速度（m/s）
  float angular = msg.angular.z; // 旋转速度（rad/s）

  // 差速模型：将线速度和角速度转换为轮速
  float wheelSeparation = 0.3;  // 轮距（需根据实际调整）
  float wheelRadius = 0.05;     // 轮半径（需根据实际调整）

  // 计算左右轮速度（单位：转/秒）
  float leftRPM = (linear - angular * wheelSeparation / 2.0) / wheelRadius;
  float rightRPM = (linear + angular * wheelSeparation / 2.0) / wheelRadius;

  // 转换为PWM值（简化：假设1 RPM = 1 PWM单位）
  leftSpeed = constrain(leftRPM * 50, -255, 255);  // 比例系数需调参
  rightSpeed = constrain(rightRPM * 50, -255, 255);
}

ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", messageCb);

void setup() {
  nh.initNode();
  nh.subscribe(sub);

  // 初始化PWM和方向引脚
  ledcSetup(0, 20000, 8);
  ledcSetup(1, 20000, 8);
  ledcAttachPin(LEFT_MOTOR_PWM, 0);
  ledcAttachPin(RIGHT_MOTOR_PWM, 1);
  pinMode(LEFT_MOTOR_DIR, OUTPUT);
  pinMode(RIGHT_MOTOR_DIR, OUTPUT);
}

void loop() {
  // 设置电机方向与速度
  digitalWrite(LEFT_MOTOR_DIR, leftSpeed >= 0 ? HIGH : LOW);
  digitalWrite(RIGHT_MOTOR_DIR, rightSpeed >= 0 ? HIGH : LOW);
  ledcWrite(0, abs(leftSpeed));
  ledcWrite(1, abs(rightSpeed));

  nh.getHardware()->loop();
  delay(10);
}

要点解读
差速驱动原理
速度控制：左右电机同向旋转实现前进/后退。
转向控制：左右电机差速旋转实现转弯（如左电机快、右电机慢则右转）。
原地旋转：左右电机反向旋转（如案例1示例3）。
电机驱动方式
使用PWM+方向引脚控制BLDC电机（通过ESC或驱动模块）。
ESP32的ledcWrite()支持高频PWM（如20kHz），减少电机噪声。
方向引脚（DIR）决定电机正反转（HIGH/LOW）。
输入信号处理
遥控器控制（案例2）：解析摇杆数据，直接映射到电机速度。
ROS控制（案例3）：通过geometry_msgs/Twist接收线速度（linear.x）和角速度（angular.z），并转换为差速指令。
需根据实际机器人参数（如轮距、轮径）调整转换公式。
安全与限制
使用constrain()限制PWM范围，防止电机过载。
初始化时停止电机（案例1的setMotorSpeed(0, 0)）。
可增加急停功能（如检测遥控器特定按键或ROS紧急消息）。
性能优化
避免在loop()中使用delay()，改用非阻塞计时（如millis()）。
ROS通信中优化串口频率（如50Hz），避免数据堆积。
可引入PID控制（如案例3的ROS版本）提高速度跟踪精度。

4、基础差速控制机器人
场景：入门级两轮差速底盘，实现基本移动和转向。
核心逻辑：左右轮独立速度控制 + 简单差速转向。

#include <SimpleFOC.h>
#include <ESP32Servo.h>  // ESP32 PWM控制

// 左右BLDC电机
BLDCMotor motorLeft(7);   // 左电机，7对极
BLDCMotor motorRight(7);  // 右电机，7对极

// 编码器
Encoder encLeft(2, 3, 1024);   // 左编码器
Encoder encRight(18, 19, 1024); // 右编码器
void doA_left() { encLeft.handleA(); }
void doB_left() { encLeft.handleB(); }
void doA_right() { encRight.handleA(); }
void doB_right() { encRight.handleB(); }

// 机器人物理参数
#define WHEEL_RADIUS 0.035     // 轮半径3.5cm
#define WHEEL_DISTANCE 0.20    // 轮距20cm
#define ROBOT_MASS 2.0         // 机器人质量2kg
#define MAX_LINEAR_SPEED 1.0   // 最大线速度1m/s
#define MAX_ANGULAR_SPEED 3.14 // 最大角速度π rad/s

// 控制输入
struct DiffControl {
  float linear = 0.0;   // 线速度 m/s
  float angular = 0.0;  // 角速度 rad/s
  unsigned long timestamp = 0;
};
DiffControl cmd;

// 机器人状态
struct RobotState {
  float leftSpeed = 0.0;    // 左轮速度 m/s
  float rightSpeed = 0.0;   // 右轮速度 m/s
  float linearSpeed = 0.0;  // 机器人线速度
  float angularSpeed = 0.0; // 机器人角速度
  float x = 0.0, y = 0.0;   // 位置
  float heading = 0.0;      // 航向
  float battery = 12.6;     // 电池电压
};
RobotState state;

// 速度控制PID
float speedKp = 2.0, speedKi = 5.0, speedKd = 0.0;
float leftErrorIntegral = 0, rightErrorIntegral = 0;
float lastLeftError = 0, lastRightError = 0;

// 转向控制PID
float turnKp = 3.0, turnKi = 0.5, turnKd = 0.1;
float headingErrorIntegral = 0;
float lastHeadingError = 0;

// 急停标志
bool emergencyStop = false;
unsigned long lastCmdTime = 0;
#define CMD_TIMEOUT 1000  // 1秒指令超时

void setup() {
  Serial.begin(115200);
  delay(1000);
  
  Serial.println("===== 差速驱动机器人控制系统 =====");
  Serial.println("ESP32 + 双BLDC电机 + 差速控制");
  
  // 1. 初始化编码器
  initEncoders();
  
  // 2. 初始化BLDC电机
  initDifferentialMotors();
  
  // 3. 校准系统
  calibrateSystem();
  
  // 4. 启动安全监控
  startSafetyMonitor();
  
  Serial.println("系统就绪");
  Serial.println("命令格式: L线速度,A角速度 (例: 0.5,0.2)");
  Serial.println("特殊命令: S-停止, E-急停, R-重置");
}

void loop() {
  static unsigned long lastControlTime = 0;
  unsigned long now = micros();
  float dt = (now - lastControlTime) / 1000000.0;
  
  if (dt >= 0.01) {  // 100Hz控制频率
    // 1. 读取控制指令
    readControlInput();
    
    // 2. 差速运动学计算
    WheelSpeeds wheelSpeeds = calculateWheelSpeeds(cmd);
    
    // 3. 应用加速度限制
    wheelSpeeds = limitAcceleration(wheelSpeeds, dt);
    
    // 4. 速度控制
    controlWheelSpeeds(wheelSpeeds, dt);
    
    // 5. 更新里程计
    updateOdometry(dt);
    
    // 6. 安全检查
    checkSafety();
    
    lastControlTime = now;
  }
  
  // 7. 执行FOC控制
  motorLeft.loopFOC();
  motorRight.loopFOC();
  
  // 8. 状态显示
  static unsigned long lastDisplay = 0;
  if (millis() - lastDisplay >= 200) {
    displayRobotStatus();
    lastDisplay = millis();
  }
  
  // 9. 处理串口命令
  if (Serial.available()) {
    handleSerialCommand();
  }
  
  // 10. 看门狗
  vTaskDelay(1);
}

void initEncoders() {
  Serial.println("初始化编码器...");
  
  // 左编码器
  encLeft.init();
  encLeft.enableInterrupts(doA_left, doB_left);
  
  // 右编码器
  encRight.init();
  encRight.enableInterrupts(doA_right, doB_right);
  
  Serial.println("编码器初始化完成");
}

void initDifferentialMotors() {
  Serial.println("初始化差速电机...");
  
  // 链接传感器
  motorLeft.linkSensor(&encLeft);
  motorRight.linkSensor(&encRight);
  
  // 配置电机参数
  BLDCMotor* motors[2] = {&motorLeft, &motorRight};
  for (int i = 0; i < 2; i++) {
    motors[i]->voltage_power_supply = 12.0;
    motors[i]->phase_resistance = 0.5;
    motors[i]->pole_pairs = 7;
    motors[i]->KV_rating = 1000;
    
    // 配置速度控制器
    motors[i]->PID_velocity.P = 0.5;
    motors[i]->PID_velocity.I = 10.0;
    motors[i]->PID_velocity.D = 0.0;
    motors[i]->PID_velocity.output_ramp = 1000.0;
    motors[i]->LPF_velocity.Tf = 0.01;
    
    // 配置电流控制器
    motors[i]->PID_current_q.P = 3.0;
    motors[i]->PID_current_q.I = 300.0;
    motors[i]->PID_current_q.D = 0.0;
    motors[i]->PID_current_q.output_ramp = 10000.0;
    
    // 初始化FOC
    motors[i]->init();
    motors[i]->initFOC();
    
    Serial.print("电机");
    Serial.print(i == 0 ? "左" : "右");
    Serial.println(" 初始化完成");
  }
  
  Serial.println("差速电机初始化完成");
}

WheelSpeeds calculateWheelSpeeds(DiffControl cmd) {
  // 差速运动学逆解
  // 公式: V_left = V_linear - ω * d/2
  //       V_right = V_linear + ω * d/2
  
  WheelSpeeds speeds;
  
  // 限制控制输入
  cmd.linear = constrain(cmd.linear, -MAX_LINEAR_SPEED, MAX_LINEAR_SPEED);
  cmd.angular = constrain(cmd.angular, -MAX_ANGULAR_SPEED, MAX_ANGULAR_SPEED);
  
  // 计算轮速
  speeds.left = cmd.linear - cmd.angular * WHEEL_DISTANCE / 2.0;
  speeds.right = cmd.linear + cmd.angular * WHEEL_DISTANCE / 2.0;
  
  // 转换为角速度 (rad/s)
  speeds.left /= WHEEL_RADIUS;
  speeds.right /= WHEEL_RADIUS;
  
  // 限制轮速
  float maxWheelSpeed = MAX_LINEAR_SPEED / WHEEL_RADIUS;
  speeds.left = constrain(speeds.left, -maxWheelSpeed, maxWheelSpeed);
  speeds.right = constrain(speeds.right, -maxWheelSpeed, maxWheelSpeed);
  
  return speeds;
}

WheelSpeeds limitAcceleration(WheelSpeeds target, float dt) {
  // 加速度限制
  
  static WheelSpeeds current = {0, 0};
  
  // 最大加速度
  float maxAccel = 2.0 / WHEEL_RADIUS;  // 转换为轮加速度
  
  // 左轮加速度限制
  float leftAccel = (target.left - current.left) / dt;
  if (abs(leftAccel) > maxAccel) {
    float sign = (leftAccel > 0) ? 1 : -1;
    target.left = current.left + sign * maxAccel * dt;
  }
  
  // 右轮加速度限制
  float rightAccel = (target.right - current.right) / dt;
  if (abs(rightAccel) > maxAccel) {
    float sign = (rightAccel > 0) ? 1 : -1;
    target.right = current.right + sign * maxAccel * dt;
  }
  
  current = target;
  return target;
}

void controlWheelSpeeds(WheelSpeeds target, float dt) {
  // 轮速控制
  
  if (emergencyStop) {
    // 急停状态
    motorLeft.move(0);
    motorRight.move(0);
    leftErrorIntegral = 0;
    rightErrorIntegral = 0;
    return;
  }
  
  // 获取实际轮速
  float actualLeft = motorLeft.shaft_velocity;
  float actualRight = motorRight.shaft_velocity;
  
  // 左轮PID控制
  float leftError = target.left - actualLeft;
  leftErrorIntegral += leftError * dt;
  leftErrorIntegral = constrain(leftErrorIntegral, -10, 10);
  float leftErrorDeriv = (leftError - lastLeftError) / dt;
  lastLeftError = leftError;
  
  float leftControl = speedKp * leftError + 
                     speedKi * leftErrorIntegral + 
                     speedKd * leftErrorDeriv;
  
  // 右轮PID控制
  float rightError = target.right - actualRight;
  rightErrorIntegral += rightError * dt;
  rightErrorIntegral = constrain(rightErrorIntegral, -10, 10);
  float rightErrorDeriv = (rightError - lastRightError) / dt;
  lastRightError = rightError;
  
  float rightControl = speedKp * rightError + 
                      speedKi * rightErrorIntegral + 
                      speedKd * rightErrorDeriv;
  
  // 应用控制
  motorLeft.move(leftControl);
  motorRight.move(rightControl);
  
  // 更新状态
  state.leftSpeed = actualLeft * WHEEL_RADIUS;
  state.rightSpeed = actualRight * WHEEL_RADIUS;
  state.linearSpeed = (state.leftSpeed + state.rightSpeed) / 2.0;
  state.angularSpeed = (state.rightSpeed - state.leftSpeed) / WHEEL_DISTANCE;
}

void updateOdometry(float dt) {
  // 更新里程计
  
  // 获取轮子位移
  static float lastLeftPos = 0, lastRightPos = 0;
  float leftPos = motorLeft.shaft_angle;
  float rightPos = motorRight.shaft_angle;
  
  float deltaLeft = (leftPos - lastLeftPos) * WHEEL_RADIUS;
  float deltaRight = (rightPos - lastRightPos) * WHEEL_RADIUS;
  
  // 计算机器人位移
  float deltaLinear = (deltaLeft + deltaRight) / 2.0;
  float deltaTheta = (deltaRight - deltaLeft) / WHEEL_DISTANCE;
  
  // 更新位置
  state.x += deltaLinear * cos(state.heading + deltaTheta / 2.0);
  state.y += deltaLinear * sin(state.heading + deltaTheta / 2.0);
  state.heading += deltaTheta;
  
  // 归一化航向到[-π, π]
  while (state.heading > PI) state.heading -= 2 * PI;
  while (state.heading < -PI) state.heading += 2 * PI;
  
  lastLeftPos = leftPos;
  lastRightPos = rightPos;
}

void readControlInput() {
  // 模拟读取遥控输入
  // 这里可以替换为实际接收机
  
  static DiffControl lastCmd = {0, 0, 0};
  
  // 模拟控制：缓慢增加速度
  static unsigned long lastAutoCmd = 0;
  if (millis() - lastAutoCmd > 5000) {  // 每5秒自动命令
    cmd.linear = random(-10, 11) / 10.0;  // -1.0 到 1.0
    cmd.angular = random(-10, 11) / 10.0; // -1.0 到 1.0
    cmd.timestamp = millis();
    lastAutoCmd = millis();
  }
  
  // 指令超时检查
  if (millis() - cmd.timestamp > CMD_TIMEOUT) {
    cmd.linear = 0;
    cmd.angular = 0;
  }
}

5、精确转向与航向控制
场景：需要精确航向控制的机器人，如自动导航车。
核心逻辑：IMU航向反馈 + PID转向控制。

#include <SimpleFOC.h>
#include <MPU6050_tockn.h>
#include <Wire.h>

// IMU传感器
MPU6050 mpu6050(Wire);
#define IMU_SDA 21
#define IMU_SCL 22

// 航向控制参数
float targetHeading = 0.0;      // 目标航向
float currentHeading = 0.0;     // 当前航向
float headingError = 0.0;       // 航向误差
float headingErrorIntegral = 0.0;
float lastHeadingError = 0.0;
float headingKp = 2.0, headingKi = 0.1, headingKd = 0.5;

// 互补滤波
float compFilterHeading = 0.0;
float alpha = 0.98;  // 互补滤波系数
unsigned long lastIMUTime = 0;

// 转向模式
enum TurnMode {
  TURN_RATE,      // 角速度控制
  TURN_ANGLE,     // 角度控制
  TURN_POINT      // 定点转向
};
TurnMode turnMode = TURN_RATE;

// 转向控制器
class HeadingController {
private:
  float kp, ki, kd;
  float errorIntegral = 0;
  float lastError = 0;
  float outputLimit = 2.0;  // 最大输出角速度
  
public:
  HeadingController(float p, float i, float d) : kp(p), ki(i), kd(d) {}
  
  float calculate(float target, float current, float dt) {
    // 计算航向误差
    float error = normalizeAngle(target - current);
    
    // PID计算
    errorIntegral += error * dt;
    errorIntegral = constrain(errorIntegral, -outputLimit/ki, outputLimit/ki);
    
    float errorDeriv = (error - lastError) / dt;
    lastError = error;
    
    float output = kp * error + ki * errorIntegral + kd * errorDeriv;
    output = constrain(output, -outputLimit, outputLimit);
    
    return output;
  }
  
  void reset() {
    errorIntegral = 0;
    lastError = 0;
  }
  
private:
  float normalizeAngle(float angle) {
    while (angle > PI) angle -= 2 * PI;
    while (angle < -PI) angle += 2 * PI;
    return angle;
  }
};

HeadingController headingCtrl(headingKp, headingKi, headingKd);

// 直线跟踪
class LineTracking {
private:
  float targetLine = 0.0;      // 目标直线
  float lineError = 0.0;       // 直线误差
  float lineKp = 1.0, lineKd = 0.1;
  float lastLineError = 0.0;
  
public:
  float calculate(float currentX, float currentY, float targetX, 
                  float targetY, float heading, float dt) {
    
    // 计算直线误差
    // 直线方程: (y - y1) = m(x - x1)
    float dx = targetX - currentX;
    float dy = targetY - currentY;
    
    if (abs(dx) < 0.001) {
      // 垂直线
      lineError = currentX - targetX;
    } else {
      float m = dy / dx;  // 斜率
      float c = targetY - m * targetX;  // 截距
      
      // 点到直线距离
      lineError = (m * currentX - currentY + c) / sqrt(m * m + 1);
    }
    
    // PD控制
    float errorDeriv = (lineError - lastLineError) / dt;
    lastLineError = lineError;
    
    float correction = lineKp * lineError + lineKd * errorDeriv;
    
    return correction;
  }
  
  void setTarget(float x1, float y1, float x2, float y2) {
    targetLine = atan2(y2 - y1, x2 - x1);
  }
};

LineTracking lineTracker;

void setupIMU() {
  Serial.println("初始化IMU...");
  
  Wire.begin(IMU_SDA, IMU_SCL);
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true);
  
  Serial.println("IMU校准完成");
}

void updateIMUHeading(float dt) {
  // 更新IMU航向
  
  mpu6050.update();
  
  // 读取陀螺仪
  float gyroZ = mpu6050.getGyroZ() * PI / 180.0;  // 转换为rad/s
  
  // 读取加速度计
  float accX = mpu6050.getAccX();
  float accY = mpu6050.getAccY();
  float accZ = mpu6050.getAccZ();
  
  // 加速度计航向
  float accHeading = atan2(accY, accX);
  
  // 互补滤波
  compFilterHeading = alpha * (compFilterHeading + gyroZ * dt) + 
                     (1 - alpha) * accHeading;
  
  // 更新当前航向
  currentHeading = compFilterHeading;
  
  // 限制到[-π, π]
  while (currentHeading > PI) currentHeading -= 2 * PI;
  while (currentHeading < -PI) currentHeading += 2 * PI;
}

float controlHeading(float dt) {
  // 航向控制
  
  float angularCmd = 0;
  
  switch(turnMode) {
    case TURN_RATE:
      // 角速度直接控制
      angularCmd = cmd.angular;
      break;
      
    case TURN_ANGLE:
      // 角度控制
      angularCmd = headingCtrl.calculate(targetHeading, currentHeading, dt);
      break;
      
    case TURN_POINT:
      // 定点转向
      angularCmd = turnToPoint(dt);
      break;
  }
  
  return angularCmd;
}

float turnToPoint(float dt) {
  // 转向到目标点
  
  float targetX = 1.0;  // 目标点X
  float targetY = 1.0;  // 目标点Y
  
  // 计算目标角度
  float dx = targetX - state.x;
  float dy = targetY - state.y;
  float targetAngle = atan2(dy, dx);
  
  // 航向控制
  float angularCmd = headingCtrl.calculate(targetAngle, currentHeading, dt);
  
  return angularCmd;
}

void followLine(float dt) {
  // 直线跟踪
  
  float targetX1 = 0, targetY1 = 0;  // 直线起点
  float targetX2 = 2, targetY2 = 2;  // 直线终点
  
  // 设置直线
  lineTracker.setTarget(targetX1, targetY1, targetX2, targetY2);
  
  // 计算直线跟踪修正
  float lineCorrection = lineTracker.calculate(state.x, state.y, 
                                               targetX1, targetY1, 
                                               currentHeading, dt);
  
  // 计算目标航向
  float targetAngle = atan2(targetY2 - targetY1, targetX2 - targetX1);
  
  // 合成角速度命令
  float headingCorrection = headingCtrl.calculate(targetAngle, currentHeading, dt);
  float angularCmd = headingCorrection + lineCorrection;
  
  // 更新命令
  cmd.angular = angularCmd;
  cmd.linear = 0.3;  // 固定线速度
}

6、自适应差速与防滑控制
场景：复杂地形或高速运行，需要自适应控制和防滑。
核心逻辑：负载自适应 + 滑移检测 + 扭矩分配。

#include <SimpleFOC.h>
#include <Filters.h>
#include <PID_AutoTune_v0.h>

// 电流传感器
#define SHUNT_RESISTANCE 0.001  // 1mΩ
#define OPAMP_GAIN 20
InlineCurrentSense currentSenseLeft(SHUNT_RESISTANCE, OPAMP_GAIN, A0, A1, A2);
InlineCurrentSense currentSenseRight(SHUNT_RESISTANCE, OPAMP_GAIN, A3, A4, A5);

// 自适应PID
class AdaptivePID {
private:
  float kp, ki, kd;
  float errorIntegral = 0;
  float lastError = 0;
  float adaptiveGain = 1.0;
  float learningRate = 0.01;
  float errorHistory[10] = {0};
  int historyIndex = 0;
  float outputLimit = 5.0;
  
public:
  AdaptivePID(float p, float i, float d) : kp(p), ki(i), kd(d) {}
  
  float calculate(float error, float dt, float processVariable) {
    // 自适应增益调整
    adaptGain(error, processVariable);
    
    // 更新误差历史
    errorHistory[historyIndex] = error;
    historyIndex = (historyIndex + 1) % 10;
    
    // PID计算
    errorIntegral += error * dt;
    errorIntegral = constrain(errorIntegral, -outputLimit/ki, outputLimit/ki);
    
    float errorDeriv = (error - lastError) / dt;
    lastError = error;
    
    float output = kp * error + ki * errorIntegral + kd * errorDeriv;
    output *= adaptiveGain;
    output = constrain(output, -outputLimit, outputLimit);
    
    return output;
  }
  
  void autoTune(float setpoint, float processVariable, float dt) {
    // 简单的自整定算法
    
    static float lastOutput = 0;
    static unsigned long lastOscillationTime = 0;
    static int oscillationCount = 0;
    
    float output = calculate(setpoint - processVariable, dt, processVariable);
    
    // 检测振荡
    if ((lastOutput > 0 && output < 0) || (lastOutput < 0 && output > 0)) {
      oscillationCount++;
      unsigned long now = millis();
      
      if (oscillationCount > 5) {
        // 检测到持续振荡，调整参数
        float oscillationPeriod = (now - lastOscillationTime) / 1000.0;
        adjustZieglerNichols(oscillationPeriod);
        oscillationCount = 0;
      }
      
      lastOscillationTime = now;
    }
    
    lastOutput = output;
  }
  
  void reset() {
    errorIntegral = 0;
    lastError = 0;
    adaptiveGain = 1.0;
  }
  
private:
  void adaptGain(float error, float processVariable) {
    // 基于误差和过程变量调整增益
    
    // 计算误差统计
    float errorMean = 0;
    for (int i = 0; i < 10; i++) {
      errorMean += errorHistory[i];
    }
    errorMean /= 10.0;
    
    float errorStd = 0;
    for (int i = 0; i < 10; i++) {
      float diff = errorHistory[i] - errorMean;
      errorStd += diff * diff;
    }
    errorStd = sqrt(errorStd / 10.0);
    
    // 自适应调整
    if (errorStd < 0.1) {
      // 误差稳定，减小增益
      adaptiveGain = max(adaptiveGain - learningRate, 0.5);
    } else if (errorStd > 1.0) {
      // 误差波动大，增加增益
      adaptiveGain = min(adaptiveGain + learningRate, 2.0);
    }
    
    // 基于速度调整
    float speed = abs(processVariable);
    if (speed < 0.1) {
      // 低速增加增益
      adaptiveGain *= 1.2;
    } else if (speed > 1.0) {
      // 高速减小增益
      adaptiveGain *= 0.8;
    }
  }
  
  void adjustZieglerNichols(float oscillationPeriod) {
    // Ziegler-Nichols方法调整PID
    
    float Ku = 2.0;  // 临界增益
    float Tu = oscillationPeriod;  // 振荡周期
    
    // Z-N参数
    kp = 0.6 * Ku;
    ki = 1.2 * Ku / Tu;
    kd = 0.075 * Ku * Tu;
  }
};

AdaptivePID leftSpeedPID(2.0, 5.0, 0.1);
AdaptivePID rightSpeedPID(2.0, 5.0, 0.1);

// 滑移检测
class SlipDetection {
private:
  float slipThreshold = 0.3;  // 30%滑移率
  float wheelAccelLimit = 10.0;  // 轮加速度限制
  float lastSpeeds[2] = {0, 0};
  unsigned long lastTime = 0;
  int slipCount[2] = {0, 0};
  bool slipDetected[2] = {false, false};
  
public:
  void update(float leftSpeed, float rightSpeed, float targetLeft, float targetRight) {
    unsigned long now = millis();
    float dt = (now - lastTime) / 1000.0;
    
    if (dt > 0.01) {
      // 计算加速度
      float leftAccel = (leftSpeed - lastSpeeds[0]) / dt;
      float rightAccel = (rightSpeed - lastSpeeds[1]) / dt;
      
      // 检测异常加速度
      if (abs(leftAccel) > wheelAccelLimit) {
        slipCount[0]++;
      } else {
        slipCount[0] = max(slipCount[0] - 1, 0);
      }
      
      if (abs(rightAccel) > wheelAccelLimit) {
        slipCount[1]++;
      } else {
        slipCount[1] = max(slipCount[1] - 1, 0);
      }
      
      // 计算滑移率
      float leftSlip = 0, rightSlip = 0;
      if (abs(targetLeft) > 0.1) {
        leftSlip = abs((targetLeft - leftSpeed) / targetLeft);
      }
      if (abs(targetRight) > 0.1) {
        rightSlip = abs((targetRight - rightSpeed) / targetRight);
      }
      
      // 滑移检测
      slipDetected[0] = (leftSlip > slipThreshold) || (slipCount[0] > 5);
      slipDetected[1] = (rightSlip > slipThreshold) || (slipCount[1] > 5);
      
      lastSpeeds[0] = leftSpeed;
      lastSpeeds[1] = rightSpeed;
      lastTime = now;
    }
  }
  
  bool isSlipping(int wheel) {
    return slipDetected[wheel];
  }
  
  void reset() {
    slipCount[0] = slipCount[1] = 0;
    slipDetected[0] = slipDetected[1] = false;
  }
};

SlipDetection slipDetector;

// 扭矩分配
class TorqueDistributor {
private:
  float leftTorque = 0, rightTorque = 0;
  float maxTorque = 5.0;  // 最大扭矩5Nm
  float torqueBalance = 0.5;  // 扭矩平衡系数
  
public:
  void distribute(float totalTorque, float steering, float slipLeft, float slipRight) {
    // 基础扭矩分配
    leftTorque = totalTorque * torqueBalance;
    rightTorque = totalTorque * (1 - torqueBalance);
    
    // 转向补偿
    leftTorque -= steering * 0.5;
    rightTorque += steering * 0.5;
    
    // 滑移补偿
    if (slipLeft) {
      leftTorque *= 0.7;  // 左轮滑移，降低扭矩
      rightTorque *= 1.1;  // 右轮增加扭矩补偿
    }
    if (slipRight) {
      rightTorque *= 0.7;
      leftTorque *= 1.1;
    }
    
    // 扭矩限制
    leftTorque = constrain(leftTorque, -maxTorque, maxTorque);
    rightTorque = constrain(rightTorque, -maxTorque, maxTorque);
    
    // 更新平衡系数
    updateBalance();
  }
  
  float getLeftTorque() { return leftTorque; }
  float getRightTorque() { return rightTorque; }
  
private:
  void updateBalance() {
    // 自适应调整扭矩平衡
    
    static float leftLoad = 0, rightLoad = 0;
    
    // 读取电流估算负载
    DQCurrent_s leftCurrent = currentSenseLeft.getFOCCurrents();
    DQCurrent_s rightCurrent = currentSenseRight.getFOCCurrents();
    
    leftLoad = sqrt(leftCurrent.d * leftCurrent.d + leftCurrent.q * leftCurrent.q);
    rightLoad = sqrt(rightCurrent.d * rightCurrent.d + rightCurrent.q * rightCurrent.q);
    
    // 调整平衡系数
    float totalLoad = leftLoad + rightLoad;
    if (totalLoad > 0.1) {
      torqueBalance = rightLoad / totalLoad;  // 负载重的轮子分配更多扭矩
    }
    
    torqueBalance = constrain(torqueBalance, 0.3, 0.7);
  }
};

TorqueDistributor torqueDistributor;

// 地形适应
class TerrainAdaptation {
private:
  float terrainRoughness = 0;
  float terrainSlope = 0;
  float lastHeights[4] = {0, 0, 0, 0};
  int sampleCount = 0;
  
public:
  void update(float leftSpeed, float rightSpeed, float leftCurrent, float rightCurrent) {
    // 估算地形粗糙度
    
    // 计算速度波动
    static float lastLeftSpeed = 0, lastRightSpeed = 0;
    float leftAccel = (leftSpeed - lastLeftSpeed) / 0.01;
    float rightAccel = (rightSpeed - lastRightSpeed) / 0.01;
    
    // 粗糙度指标
    float roughness = (abs(leftAccel) + abs(rightAccel)) / 2.0;
    
    // 低通滤波
    terrainRoughness = terrainRoughness * 0.9 + roughness * 0.1;
    
    // 坡度估算
    float currentDiff = leftCurrent - rightCurrent;
    terrainSlope = currentDiff * 0.1;  // 简化估算
    
    lastLeftSpeed = leftSpeed;
    lastRightSpeed = rightSpeed;
  }
  
  float getAdaptationFactor() {
    // 返回地形适应因子
    float factor = 1.0;
    
    if (terrainRoughness > 2.0) {
      factor *= 0.7;  // 粗糙地形降低增益
    }
    if (abs(terrainSlope) > 0.2) {
      factor *= 0.8;  // 坡度降低增益
    }
    
    return factor;
  }
  
  float getRoughness() { return terrainRoughness; }
  float getSlope() { return terrainSlope; }
};

TerrainAdaptation terrainAdapter;

void adaptiveControl(float dt) {
  // 自适应控制
  
  // 1. 读取电流
  DQCurrent_s leftCurrent = currentSenseLeft.getFOCCurrents();
  DQCurrent_s rightCurrent = currentSenseRight.getFOCCurrents();
  
  float leftCurrentMag = sqrt(leftCurrent.d * leftCurrent.d + leftCurrent.q * leftCurrent.q);
  float rightCurrentMag = sqrt(rightCurrent.d * rightCurrent.d + rightCurrent.q * rightCurrent.q);
  
  // 2. 滑移检测
  slipDetector.update(state.leftSpeed, state.rightSpeed, 
                      cmd.linear, cmd.angular);
  
  bool leftSlipping = slipDetector.isSlipping(0);
  bool rightSlipping = slipDetector.isSlipping(1);
  
  // 3. 地形适应
  terrainAdapter.update(state.leftSpeed, state.rightSpeed, 
                       leftCurrentMag, rightCurrentMag);
  float terrainFactor = terrainAdapter.getAdaptationFactor();
  
  // 4. 扭矩分配
  float totalTorque = cmd.linear * 2.0;  // 简化扭矩估算
  float steeringTorque = cmd.angular * 0.5;
  
  torqueDistributor.distribute(totalTorque, steeringTorque, 
                               leftSlipping, rightSlipping);
  
  // 5. 自适应PID控制
  float leftTarget = torqueDistributor.getLeftTorque() * 0.2;  // 转换为速度参考
  float rightTarget = torqueDistributor.getRightTorque() * 0.2;
  
  float leftControl = leftSpeedPID.calculate(leftTarget - state.leftSpeed, 
                                            dt, state.leftSpeed);
  float rightControl = rightSpeedPID.calculate(rightTarget - state.rightSpeed, 
                                              dt, state.rightSpeed);
  
  // 应用地形适应
  leftControl *= terrainFactor;
  rightControl *= terrainFactor;
  
  // 6. 应用控制
  motorLeft.move(leftControl);
  motorRight.move(rightControl);
}

要点解读
差速运动学原理与实现
核心公式：V_left = V_linear - ω * d/2，V_right = V_linear + ω * d/2。案例4的calculateWheelSpeeds()实现了这个转换。
单位转换：控制输入是m/s和rad/s，需要转换为轮子角速度rad/s：ω_wheel = V_linear / R。
运动范围限制：必须同时限制MAX_LINEAR_SPEED和MAX_ANGULAR_SPEED，防止超调。
运动学正解：从轮速计算机器人状态：V_robot = (V_left + V_right)/2，ω_robot = (V_right - V_left)/d。
两种转向控制模式的区别
角速度控制（案例4的TURN_RATE）：直接控制机器人旋转速度。响应快，适合遥控操作。但无法精确控制最终朝向。
角度控制（案例5的TURN_ANGLE）：控制机器人转到特定角度。需要IMU反馈，精度高。案例5的HeadingController实现闭环控制。
航向归一化：角度控制必须处理-π到π的跳变。案例5的normalizeAngle()函数是关键。
IMU在差速控制中的关键作用
互补滤波：案例5的alpha=0.98融合陀螺仪（短期稳定）和加速度计（长期参考）。这是低成本IMU的标准方法。
航向漂移补偿：纯积分陀螺仪会漂移。需要周期性用加速度计或磁力计校正。
转向中心估算：IMU可检测实际转向中心是否与几何中心匹配，修正运动学模型。
颠簸检测：加速度计可检测地面不平，触发控制参数调整。
自适应与防滑控制策略
滑移检测：案例6通过比较目标速度与实际速度检测滑移。slipThreshold=0.3表示30%的速度差认为是滑移。
加速度检测：轮子加速度突然增大可能表示打滑。wheelAccelLimit=10.0限制加速度。
扭矩重分配：检测到单边滑移时，减少该轮扭矩，增加另一轮扭矩补偿。
地形适应：案例6的terrainAdapter根据电流和速度波动估计地形，调整控制增益。
自适应PID：案例6的AdaptivePID根据误差统计自动调整增益，适应不同负载。
工程实施的性能优化
控制频率分层：
电流环：20kHz（FOC内部）
速度环：100-200Hz
位置环：50-100Hz
航向环：50Hz
加速度限制：案例4的limitAcceleration()防止急启急停。最大加速度2.0m/s²是合理值。
积分抗饱和：所有PID都必须限制积分项。案例4的constrain(errorIntegral, -10, 10)。
安全机制：
指令超时：案例4 CMD_TIMEOUT=1000ms无指令自动停止。
电流保护：监控电机电流，超限降额。
温度保护：监控电机和控制器温度。
编码器选择：至少1000线，4倍频。6.5寸轮子，1000线对应0.2mm分辨率。

注意，以上案例只是为了拓展思路，仅供参考。它们可能有错误、不适用或者无法编译。您的硬件平台、使用场景和Arduino版本可能影响使用方法的选择。实际编程时，您要根据自己的硬件配置、使用场景和具体需求进行调整，并多次实际测试。您还要正确连接硬件，了解所用传感器和设备的规范和特性。涉及硬件操作的代码，您要在使用前确认引脚和电平等参数的正确性和安全性。