这是一个经典的两轮差速驱动（Differential Drive）机器人控制方案，它不仅需要精确控制左右两个驱动轮的速度以实现前进、后退、转向等基本运动，还需要利用位置信息（通常是里程计Odometry）来协同控制，实现更高级的导航功能，如精确移动到指定坐标、沿预定路径行驶、原地旋转指定角度等。
一、 主要特点 (Key Features)
双轮差速驱动原理 (Two-Wheel Differential Drive Principle):
核心: 机器人通过独立控制左右两个驱动轮的速度来实现运动。当两轮速度相同时，机器人直线运动；当两轮速度不同时，机器人围绕瞬时旋转中心（ICC）转弯；当两轮速度大小相等、方向相反时，机器人原地旋转。
功能: 这种结构简单、机动性强（可实现任意方向的平面运动），是移动机器人中最常用的驱动方式之一。
双闭环速度控制 (Dual Closed-Loop Speed Control):
核心: 为每个BLDC驱动轮配置独立的速度闭环控制器（通常采用PI或PID算法）。
工作原理: Arduino主控设定左右轮的目标速度（Velocity Setpoints）。通过安装在两轮上的编码器（或霍尔传感器）实时获取两轮的实际转速（Speed Feedback）。控制器将目标速度与实际速度进行比较，计算出误差，并通过PI/PID算法生成相应的PWM信号或电流指令，驱动对应的BLDC电机（通过ESC或专用驱动器）精确跟踪目标速度。
功能: 确保左右轮能够快速、准确地响应速度指令，即使在负载变化或地面摩擦力不均的情况下也能保持设定的速度。
里程计位置估计与协同控制 (Odometry-Based Position Estimation & Coordinated Control):
核心: 基于两个驱动轮的编码器反馈信息，实时计算机器人的位姿（位置 X, Y 和航向角 θ）。
工作原理:
里程计计算: 根据左右轮的转速（或累计转动角度），结合轮子直径和轮距（两轮间距），利用运动学模型（如阿克曼运动学模型的简化版）推算出机器人在自身坐标系下的位移和旋转角度增量。
位姿积分: 将这些增量累加到上一时刻的位姿估计值上，得到机器人在世界坐标系下的实时位姿估计。
协同控制: Arduino主控根据当前估计的位姿和期望的目标位姿（目标坐标X, Y, θ），通过路径规划或轨迹跟踪算法（如纯追踪Pure Pursuit、PID路径跟随等）计算出下一步需要达到的目标线速度（Linear Velocity）和角速度（Angular Velocity）。
速度分解: 将计算出的目标线速度和角速度分解为左右轮的目标速度（V_left_setpoint, V_right_setpoint）。
闭环执行: 这些分解后的速度指令再次进入各自的双闭环速度控制系统执行。
功能: 实现从“期望位置/轨迹”到“执行速度指令”的闭环控制，使机器人能够自主导航到指定地点或沿预定路径行驶。
高机动性与灵活性 (High Maneuverability & Flexibility):
功能: 结合精确的速度控制和基于位置的路径规划，机器人可以实现精确的直线行驶、曲线跟随、定点转向、原地旋转等多种复杂运动模式。
传感器融合潜力 (Potential for Sensor Fusion):
功能: 虽然基础是里程计，但此架构易于集成其他传感器（如IMU提供姿态和角速度校正、激光雷达/摄像头进行SLAM或地标定位）来校正里程计累积误差，进一步提高定位精度。
二、 应用场景 (Application Scenarios)
自主移动机器人 (AMR - Autonomous Mobile Robots):
用途: 在仓库、工厂、医院、办公室等环境中执行物料搬运、巡检、清洁、引导等任务。
价值: 实现无人化、智能化的物流和服务业。
服务机器人 (Service Robots):
用途: 如餐厅送餐机器人、酒店引导机器人，需要在复杂环境中导航到指定地点。
价值: 提供便捷的服务体验。
科研与教育平台 (Research & Education Platforms):
用途: 作为移动机器人导航、路径规划、SLAM（同步定位与地图构建）算法的实验和教学平台。
价值: 促进机器人技术的研究和人才培养。
竞赛机器人 (Competition Robots):
用途: 在各类机器人竞赛（如RoboMaster、RoboCup、全国大学生机器人大赛）中，作为基础的移动平台。
价值: 是实现复杂比赛策略的前提。
安防巡逻机器人 (Security Patrol Robots):
用途: 按照预设路线在特定区域进行巡逻监控。
价值: 提高安防效率和覆盖范围。
三、 需要注意的事项 (Considerations & Challenges)
Arduino计算能力与实时性 (Arduino Computational Power & Real-Time Performance):
问题: 实时运行双路速度闭环PI/PID、里程计积分计算、路径规划/跟踪算法、速度指令分解等，对Arduino主控的计算能力（特别是浮点运算）和实时性要求较高。经典Arduino（如Uno）可能难以胜任，尤其是在高控制频率（如100Hz以上）和复杂导航算法下。
对策: 选用性能更强的Arduino型号（如Mega 2560, Due, ESP32）或专用的机器人控制器。优化代码，使用中断处理编码器信号，确保控制周期稳定。
编码器分辨率与精度 (Encoder Resolution & Accuracy):
问题: 里程计的精度直接依赖于编码器的分辨率。分辨率太低会导致位姿估计误差大，影响导航精度。编码器的安装误差、机械间隙也会引入误差。
对策: 选用高分辨率编码器，确保精确的机械安装，进行编码器标定（如轮子直径、轮距）。
里程计累积误差 (Odometry Cumulative Error):
问题: 里程计是基于积分的，任何微小的测量误差或模型误差都会随时间累积，导致位姿估计严重偏离真实值（漂移）。
对策: 定期使用外部定位系统（如GPS、UWB、视觉地标）进行校正；使用IMU数据进行航向校正；或结合SLAM技术构建地图并进行位姿校正。
运动学模型准确性 (Kinematic Model Accuracy):
问题: 里程计计算基于理想的运动学模型（如假设车轮无滑移、机器人平面运动）。实际情况（如地面不平、轮子打滑、机器人倾斜）会使模型失效。
对策: 在模型中考虑滑移补偿；使用更复杂的模型；或依赖外部传感器进行校正。
速度指令分解与运动学约束 (Velocity Command Decomposition & Kinematic Constraints):
问题: 将期望的线速度和角速度转换为左右轮速度时，需要考虑物理极限（如最大转速、加速度限制），否则会导致指令无法执行或运动失真。
对策: 在算法中加入速度和加速度限制，并进行适当的指令平滑处理。
BLDC驱动器特性与同步 (BLDC Driver Characteristics & Synchronization):
问题: 两个BLDC驱动器（ESC或驱动器）的特性（如响应时间、控制精度）可能存在微小差异，影响差速控制的精确性。编码器反馈的同步性也很重要。
对策: 尽量选用特性一致的驱动器和电机；确保编码器读取的同步性；必要时对两轮进行单独的参数标定。
系统标定 (System Calibration):
问题: 包括编码器标定、轮子参数（直径）标定、轮距标定、速度控制器参数（PI/PID）标定等，是实现精确控制的关键步骤。
对策: 设计系统的标定程序，进行充分的实验验证。

1、基于编码器的直线/圆弧轨迹跟踪（入门级）
功能：通过双BLDC电机的差速驱动，结合增量式编码器的位置反馈，实现机器人沿预设轨迹（直线、圆弧）的精准移动。
硬件：Arduino UNO、2个BLDC电机（带霍尔+增量式编码器）、L298N驱动器、OLED显示屏、红外避障传感器（HC-SR04）。
代码逻辑：
运动学模型：根据左右轮的目标速度计算机器人的线速度v和角速度ω；
闭环控制：用PID分别控制左右轮的速度（内层环），结合编码器反馈调整PWM占空比；
轨迹生成：发送Gcode指令（如G0 X100 Y0直线、G1 X100 Y100 I50圆弧），解析后生成左右轮速度曲线；
显示与报警：实时显示机器人位置、速度，遇障碍物时停止并报警。

#include <PID_v2.h>
#include <Adafruit_SSD1306.h>
#include <NewPing.h>

// 硬件引脚定义
#define LEFT_ENC_A D2       // 左轮编码器A相
#define LEFT_ENC_B D3       // 左轮编码器B相
#define RIGHT_ENC_A D4      // 右轮编码器A相
#define RIGHT_ENC_B D5      // 右轮编码器B相
#define LEFT_MOTOR_EN A1    // 左电机使能
#define RIGHT_MOTOR_EN A2   // 右电机使能
#define SONAR_TRIG D6       // 超声波触发
#define SONAR_ECHO D7       // 超声波接收

// 机器人参数（需校准）
#define WHEEL_RADIUS 0.1     // 车轮半径（m）
#define ROBOT_WIDTH 0.3      // 机器人宽度（m）
int leftEncoderCount = 0;   // 左轮编码器计数
int rightEncoderCount = 0;  // 右轮编码器计数

// PID控制器（左右轮速度环）
PID leftPID(&leftSpeedError, &leftPwm, &leftSpeedRef, 1.5, 0.1, 0.5);
PID rightPID(&rightSpeedError, &rightPwm, &rightSpeedRef, 1.5, 0.1, 0.5);

// 全局变量
float currentX = 0, currentY = 0; // 当前位置（m）
float currentTheta = 0;           // 当前角度（rad）
float targetV = 0, targetOmega = 0; // 目标线速度、角速度
float leftSpeedRef = 0, rightSpeedRef = 0; // 左右轮目标速度（m/s）
float leftSpeedError = 0, rightSpeedError = 0; // 左右轮速度误差
float leftPwm = 0, rightPwm = 0; // 左右轮PWM输出

void setup() {
  Serial.begin(9600);
  pinMode(LEFT_ENC_A, INPUT);
  pinMode(LEFT_ENC_B, INPUT);
  pinMode(RIGHT_ENC_A, INPUT);
  pinMode(RIGHT_ENC_B, INPUT);
  attachInterrupt(digitalPinToInterrupt(LEFT_ENC_A), leftEncoderISR, CHANGE);
  attachInterrupt(digitalPinToInterrupt(RIGHT_ENC_A), rightEncoderISR, CHANGE);

  // 初始化PID
  leftPID.SetSampleTime(10);
  rightPID.SetSampleTime(10);

  // 初始化OLED
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
}

void loop() {
  // 1. 读取Gcode指令（示例：G0 X100 Y0 F50）
  if (Serial.available()) {
    String cmd = Serial.readStringUntil('\n');
    parseGcode(cmd);
  }

  // 2. 更新机器人位姿（运动学模型）
  updateOdometry();

  // 3. 速度环控制（每10ms执行一次）
  if (millis() - lastControlTime >= 10) {
    // 计算左右轮目标速度（由v和ω推导）
    leftSpeedRef = (targetV - targetOmega * ROBOT_WIDTH / 2) / WHEEL_RADIUS;
    rightSpeedRef = (targetV + targetOmega * ROBOT_WIDTH / 2) / WHEEL_RADIUS;

    // 左右轮PID控制
    leftPID.Compute();
    rightPID.Compute();

    // 输出PWM（限制在0-255）
    analogWrite(LEFT_MOTOR_EN, constrain(leftPwm, 0, 255));
    analogWrite(RIGHT_MOTOR_EN, constrain(rightPwm, 0, 255));

    lastControlTime = millis();
  }

  // 4. 避障检测
  float distance = sonar.ping_cm();
  if (distance < 20) { // 小于20cm停止
    stopMotors();
    displayAlert("Obstacle!");
  }

  // 5. 显示状态
  displayStatus();

  delay(10);
}

// 编码器中断服务函数（计数）
void leftEncoderISR() {
  if (digitalRead(LEFT_ENC_A)) {
    leftEncoderCount += digitalRead(LEFT_ENC_B) ? 1 : -1;
  } else {
    leftEncoderCount += digitalRead(LEFT_ENC_B) ? -1 : 1;
  }
}

void rightEncoderISR() {
  if (digitalRead(RIGHT_ENC_A)) {
    rightEncoderCount += digitalRead(RIGHT_ENC_B) ? 1 : -1;
  } else {
    rightEncoderCount += digitalRead(RIGHT_ENC_B) ? -1 : 1;
  }
}

// 更新里程计（计算当前位置）
void updateOdometry() {
  static float lastLeftSpeed = 0, lastRightSpeed = 0;
  float deltaTime = 0.1; // 假设100ms采样周期

  // 计算左右轮位移
  float deltaLeft = lastLeftSpeed * deltaTime;
  float deltaRight = lastRightSpeed * deltaTime;

  // 计算机器人位移和角度变化
  float deltaS = (deltaLeft + deltaRight) / 2;
  float deltaTheta = (deltaRight - deltaLeft) / ROBOT_WIDTH;

  // 更新位姿
  currentTheta += deltaTheta;
  currentX += deltaS * cos(currentTheta);
  currentY += deltaS * sin(currentTheta);

  // 保存上一次速度
  lastLeftSpeed = leftSpeedRef;
  lastRightSpeed = rightSpeedRef;
}

// 解析Gcode指令（简化版）
void parseGcode(String cmd) {
  if (cmd.startsWith("G0")) { // 直线运动
    int x = cmd.indexOf('X');
    int y = cmd.indexOf('Y');
    float targetX = cmd.substring(x+1, y).toFloat();
    float targetY = cmd.substring(y+1).toFloat();
    // 计算目标v和ω（直线运动ω=0）
    targetV = sqrt(pow(targetX - currentX, 2) + pow(targetY - currentY, 2)) / 10; // 10秒到达
    targetOmega = 0;
  } else if (cmd.startsWith("G1")) { // 圆弧运动
    // 省略圆弧插补算法（需用Bresenham或逆运动学）
  }
}

2、基于卡尔曼滤波的SLAM自主导航（进阶级）
功能：结合激光雷达（RPLiDAR A1）和里程计的数据，用卡尔曼滤波融合定位信息，实现机器人的自主建图（SLAM）和路径规划（如A算法）。
硬件：Arduino Mega、RPLiDAR A1、2个BLDC电机（带编码器）、TB6560驱动器、蜂鸣器、SD卡模块。
代码逻辑：
数据融合：用卡尔曼滤波融合激光雷达的绝对定位数据和里程计的相对定位数据，得到更精准的机器人位姿；
SLAM建图：将激光雷达扫描的点云数据转换为栅格地图（Occupancy Grid Map）；
路径规划：用A算法计算从起点到终点的最短路径；
导航控制：将路径分解为左右轮的速度序列，通过差速驱动实现轨迹跟踪。

#include <KalmanFilter.h>
#include <RPLidar.h>
#include <SD.h>
#include <AStar.h>

// 硬件定义
#define LIDAR_SERIAL Serial1 // Lidar通信串口
#define MOTOR_LEFT_EN A1
#define MOTOR_RIGHT_EN A2
RPLidar lidar; // Lidar对象
AStar planner; // A*路径规划对象
KalmanFilter kf; // 卡尔曼滤波对象（状态：x,y,theta）

// 全局变量
float lidarData[360] = {0}; // Lidar点云数据
float odomX = 0, odomY = 0, odomTheta = 0; // 里程计位姿
float filteredX = 0, filteredY = 0, filteredTheta = 0; // 卡尔曼滤波后的位姿
Path navigationPath; // 导航路径

void setup() {
  Serial.begin(115200);
  lidar.begin(LIDAR_SERIAL, 115200); // 初始化Lidar
  SD.begin(10); // 初始化SD卡（存储地图）

  // 初始化卡尔曼滤波（状态维度3，观测维度360）
  kf.init(3, 360);
  kf.processNoiseCov << 0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01;
  kf.measurementNoiseCov << 0.1, 0, ...; // 观测噪声协方差（需校准）

  // 加载地图（从SD卡）
  loadMapFromSD();
}

void loop() {
  // 1. 获取Lidar数据
  for (int i = 0; i < 360; i++) {
    lidarData[i] = lidar.getDistance(i); // 获取i角度的距离（mm）
  }

  // 2. 更新里程计（同案例1）
  updateOdometry();

  // 3. 卡尔曼滤波融合数据
  kf.predict(); // 预测下一状态（基于里程计）
  kf.update(lidarData); // 更新观测值（基于Lidar）
  filteredX = kf.statePost[0];
  filteredY = kf.statePost[1];
  filteredTheta = kf.statePost[2];

  // 4. 路径规划（A*算法）
  if (navigationPath.empty()) {
    calculatePath(); // 计算从当前位置到目标位置的路径
  }

  // 5. 导航控制（跟随路径）
  followPath(navigationPath);

  // 6. 存储地图（到SD卡）
  saveMapToSD();

  delay(50);
}

// 卡尔曼滤波更新函数（简化版）
void kfUpdate() {
  // 状态转移矩阵（里程计模型）
  float F[3][3] = {{1, 0, -odomY*cos(odomTheta)},
                   {0, 1, odomX*sin(odomTheta)},
                   {0, 0, 1}};
  kf.setStateTransitionMatrix(F);

  // 观测矩阵（Lidar模型：每个点对应一个观测维度）
  float H[360][3] = {0};
  for (int i = 0; i < 360; i++) {
    H[i][0] = cos(filteredTheta + i*PI/180);
    H[i][1] = sin(filteredTheta + i*PI/180);
    H[i][2] = 0;
  }
  kf.setMeasurementMatrix(H);

  // 更新状态
  kf.predict();
  kf.update(lidarData);
}

3、基于CAN总线的多机器人协同搬运（高级级）
功能：通过CAN总线实现多台差速驱动机器人的协同作业（如仓库中的货物搬运），主机器人作为“领队”，从机器人跟随领队的位姿，保持固定队形（如纵队、横队）。
硬件：Arduino Mega（主）、Arduino Uno（从×3）、MCP2515 CAN模块、2个BLDC电机（带编码器）、RFID读卡器（识别货物）。
代码逻辑：
主机器人：接收任务指令（如“搬运货架A”），规划路径，向从机器人发送同步帧（包含自身位姿、队列目标位置）；
从机器人：接收同步帧，计算相对于领队的位姿偏移，调整左右轮速度以保持队形；
货物识别：通过RFID读卡器识别货物标签，确认货物位置；
协同搬运：到达货架后，所有机器人同步停止，完成货物抓取。

// 主机器人代码（Arduino Mega）
#include <MCP2515.h>
#include <PID_v2.h>

MCP2515 canBus(CAN_CS, CAN_INT); // CAN总线对象
PID mainLeftPID(&mainLeftError, &mainLeftPwm, &mainLeftSpeed, 2, 0.1, 0.3); // 主机器人左轮PID
PID mainRightPID(&mainRightError, &mainRightPwm, &mainRightSpeed, 2, 0.1, 0.3); // 主机器人右轮PID

void setup() {
  canBus.reset();
  canBus.setBitrate(CAN_500KBPS);
  canBus.setNormalMode();
}

void loop() {
  // 1. 主机器人导航（同案例2）
  navigateToTarget();

  // 2. 发送同步帧（包含主机器人位姿、队列参数）
  CanMsg syncFrame;
  syncFrame.id = 0x200; // 同步帧ID
  syncFrame.data[0] = (uint8_t)(currentX >> 8); // 主机器人x坐标高字节
  syncFrame.data[1] = (uint8_t)(currentX & 0xFF); // 主机器人x坐标低字节
  syncFrame.data[2] = (uint8_t)(currentY >> 8); // 主机器人y坐标高字节
  syncFrame.data[3] = (uint8_t)(currentY & 0xFF); // 主机器人y坐标低字节
  syncFrame.data[4] = (uint8_t)(currentTheta >> 8); // 主机器人角度高字节
  syncFrame.data[5] = (uint8_t)(currentTheta & 0xFF); // 主机器人角度低字节
  syncFrame.data[6] = formationType; // 队形类型（0=纵队，1=横队）
  syncFrame.data[7] = robotID; // 主机器人ID
  canBus.sendMessage(syncFrame);

  // 3. 接收从机器人反馈（检查同步状态）
  if (canBus.available()) {
    CanMsg feedback = canBus.readMessage();
    checkFormationSync(feedback);
  }

  delay(10);
}

// 从机器人代码（Arduino Uno）
#include <MCP2515.h>
#include <PID_v2.h>

MCP2515 canBus(CAN_CS, CAN_INT);
PID slaveLeftPID(&slaveLeftError, &slaveLeftPwm, &slaveLeftSpeed, 2, 0.1, 0.3); // 从机器人左轮PID
PID slaveRightPID(&slaveRightError, &slaveRightPwm, &slaveRightSpeed, 2, 0.1, 0.3); // 从机器人右轮PID

void setup() {
  canBus.init();
  canBus.setListenOnlyMode(); // 监听CAN总线
}

void loop() {
  if (canBus.available()) {
    CanMsg frame = canBus.readMessage();
    if (frame.id == 0x200 && frame.data[7] == masterID) { // 接收主机器人同步帧
      // 解析同步数据
      float masterX = (frame.data[0] << 8) | frame.data[1];
      float masterY = (frame.data[2] << 8) | frame.data[3];
      float masterTheta = (frame.data[4] << 8) | frame.data[5];
      uint8_t formation = frame.data[6];

      // 根据队形计算从机器人的目标位姿（如纵队：保持前后间距1m）
      float offsetX = 0, offsetY = 0, offsetTheta = 0;
      if (formation == 0) { // 纵队
        offsetX = myPositionInFormation * 1.0; // 第n台从机器人的x偏移
      } else if (formation == 1) { // 横队
        offsetY = myPositionInFormation * 0.5; // 第n台从机器人的y偏移
      }

      // 计算从机器人的目标位姿
      float targetX = masterX + offsetX * cos(masterTheta) - offsetY * sin(masterTheta);
      float targetY = masterY + offsetX * sin(masterTheta) + offsetY * cos(masterTheta);
      float targetTheta = masterTheta;

      // 计算左右轮目标速度（差速驱动模型）
      float v = sqrt(pow(targetX - currentX, 2) + pow(targetY - currentY, 2)) / 10; // 10秒到达
      float omega = (targetTheta - currentTheta) / 5; // 5秒转至目标角度
      slaveLeftSpeed = (v - omega * ROBOT_WIDTH / 2) / WHEEL_RADIUS;
      slaveRightSpeed = (v + omega * ROBOT_WIDTH / 2) / WHEEL_RADIUS;

      // 速度环PID控制
      slaveLeftPID.Compute();
      slaveRightPID.Compute();

      // 输出PWM
      analogWrite(LEFT_MOTOR_EN, constrain(slaveLeftPwm, 0, 255));
      analogWrite(RIGHT_MOTOR_EN, constrain(slaveRightPwm, 0, 255));
    }
  }
}

要点解读

1. 差速驱动的“运动学模型”：速度与位姿的核心关系
   差速驱动机器人的运动学模型是速度→位姿转换的基础，公式如下：
   线速度：v = (v_r + v_l) / 2（v_r右轮速度，v_l左轮速度）
   角速度：ω = (v_r - v_l) / L（L机器人宽度）
   位姿更新：dx = v * cos(θ) * Δt，dy = v * sin(θ) * Δt，dθ = ω * Δt
   注意：必须校准车轮半径和机器人宽度（用尺子测量），否则会导致位姿计算误差累积。
   案例对应：所有案例均基于此模型计算左右轮速度和机器人位姿。
2. 多传感器的“融合定位”：解决单一传感器的局限性
   里程计：通过编码器计算相对位移，优点是高频（100Hz），缺点是误差累积（长时间运行会漂移）；
   激光雷达：通过三角定位或ICP算法计算绝对位姿，优点是无累积误差，缺点是低频（10Hz）且受环境遮挡影响；
   卡尔曼滤波：将两者的优势结合，用里程计的高频数据做预测，用雷达的低频数据做修正，得到更精准的位姿。
   案例对应：案例2用卡尔曼滤波融合里程计和激光雷达数据，解决了里程计的漂移问题；案例3用CAN总线融合主机器人的位姿数据，解决了从机器人的定位问题。
3. 轨迹跟踪的“前馈+反馈”：提升动态响应性能
   单纯的PID反馈控制无法应对快速变化的轨迹（如圆弧转弯），需加入前馈控制补偿系统的惯性：
   前馈项：根据轨迹的加速度和角加速度，提前调整PWM输出（如转弯时增加外侧轮的PWM占空比）；
   反馈项：用PID纠正实际轨迹与目标轨迹的偏差。
   案例对应：案例1中的Gcode轨迹跟踪可通过加入前馈项提升圆弧运动的平滑性（需修改parseGcode函数，计算轨迹的加速度）。
4. 多机器人的“协同机制”：避免碰撞与保持队形
   多机器人协同的核心是“通信+决策”：
   通信协议：用CAN总线的高优先级帧传递同步指令（如紧急停止），确保实时性（延迟<1ms）；
   队形保持：从机器人根据主机器人的位姿计算自身的目标位姿（如纵队的前后偏移、横队的左右偏移），通过差速驱动调整；
   避障策略：从机器人需内置局部避障算法（如人工势场法），避免因主机器人突然转向而碰撞。
   案例对应：案例3通过CAN总线传递同步帧，从机器人根据主机器人的位姿调整自身速度，保持纵队队形。
5. 工业级的“可靠性设计”：抗干扰与容错
   电磁兼容：BLDC电机的相线需套屏蔽网，避免电机启停时的电磁干扰影响传感器（如编码器、Lidar）；
   电源设计：采用隔离电源（如DC-DC模块）给控制电路供电，避免动力电源的电压波动；
   软件容错：加入看门狗定时器（Watchdog Timer），当程序跑飞时自动重启；设置传感器故障检测（如编码器脉冲丢失时，切换到Lidar定位）。
   案例对应：案例3中的CAN总线具有CRC校验功能，可检测数据传输错误；案例2中的SD卡模块用于存储地图，防止数据丢失。

4、基础差速驱动（速度控制为主，位置辅助）

#include <SimpleFOC.h>

// 左右电机配置
BLDCMotor motorLeft(3), motorRight(9);
BLDCDriver3PWM driverLeft(5, 6, 7), driverRight(10, 11, 12);
Encoder encoderLeft(2, 4), encoderRight(3, 5); // 假设共用中断引脚需调整

// 控制变量
float targetSpeedLeft = 2.0, targetSpeedRight = 2.0; // rad/s
float targetPosLeft = 0, targetPosRight = 0;         // 目标位置（编码器脉冲）
bool positionMode = false;

void setup() {
  Serial.begin(115200);
  
  // 左电机初始化（速度控制）
  motorLeft.linkDriver(&driverLeft);
  motorLeft.linkSensor(&encoderLeft);
  motorLeft.controller = MotionControlType::velocity;
  motorLeft.PID_velocity.P = 0.5;
  motorLeft.init();
  
  // 右电机初始化（同左电机）
  motorRight.linkDriver(&driverRight);
  motorRight.linkSensor(&encoderRight);
  motorRight.controller = MotionControlType::velocity;
  motorRight.init();

  // 外部输入（模拟远程控制）
  pinMode(A0, INPUT); // 左电机速度
  pinMode(A1, INPUT); // 右电机速度
  pinMode(7, INPUT_PULLUP); // 位置模式切换按钮
}

void loop() {
  // 1. 模式切换（通过按钮）
  if (digitalRead(7) == LOW) {
    positionMode = !positionMode;
    delay(300); // 防抖
  }

  // 2. 控制逻辑
  if (positionMode) {
    // 位置控制（例如移动1000脉冲）
    targetPosLeft += 10; // 小步进移动
    targetPosRight += 10;
    
    motorLeft.controller = motorRight.controller = MotionControlType::position;
    motorLeft.target = targetPosLeft;
    motorRight.target = targetPosRight;
  } else {
    // 速度控制（通过电位器）
    targetSpeedLeft = map(analogRead(A0), 0, 1023, -5.0, 5.0); // -5~5 rad/s
    targetSpeedRight = map(analogRead(A1), 0, 1023, -5.0, 5.0);
    
    motorLeft.controller = motorRight.controller = MotionControlType::velocity;
    motorLeft.target = targetSpeedLeft;
    motorRight.target = targetSpeedRight;
  }

  // 3. 实时监控
  Serial.print("Left Speed:"); Serial.print(motorLeft.shaftVelocity());
  Serial.print(" Right Speed:"); Serial.println(motorRight.shaftVelocity());
}

5、差速转向+精确直线运动（速度+位置协同）

#include <SimpleFOC.h>

// 电机和传感器配置（同案例1）
BLDCMotor motorLeft(3), motorRight(9);
BLDCDriver3PWM driverLeft(5, 6, 7), driverRight(10, 11, 12);
Encoder encoderLeft(2, 4), encoderRight(3, 5);

// 控制变量
float targetLinearSpeed = 1.5; // m/s（需换算为电机转速）
float targetAngularSpeed = 0.5; // rad/s（转向）
float wheelRadius = 0.05;      // 车轮半径（m）
float wheelbase = 0.3;         // 轮距（m）

void setup() {
  Serial.begin(115200);
  
  // 电机初始化（默认速度控制）
  motorLeft.linkDriver(&driverLeft);
  motorLeft.linkSensor(&encoderLeft);
  motorLeft.controller = MotionControlType::velocity;
  motorLeft.init();
  
  motorRight.linkDriver(&driverRight);
  motorRight.linkSensor(&encoderRight);
  motorRight.controller = MotionControlType::velocity;
  motorRight.init();

  // 远程控制输入
  pinMode(A0, INPUT); // 直线速度
  pinMode(A1, INPUT); // 转向速度
}

void loop() {
  // 1. 读取输入
  float linearInput = analogRead(A0) / 1023.0; // 0~1
  float angularInput = (analogRead(A1) - 512) / 512.0; // -1~1

  // 2. 差速模型计算（阿克曼转向）
  float leftSpeed = (linearInput * targetLinearSpeed - angularInput * targetAngularSpeed * wheelbase / 2.0) / wheelRadius;
  float rightSpeed = (linearInput * targetLinearSpeed + angularInput * targetAngularSpeed * wheelbase / 2.0) / wheelRadius;

  // 3. 速度控制执行
  motorLeft.target = leftSpeed;
  motorRight.target = rightSpeed;

  // 4. 位置闭环辅助（例如防打滑）
  static float lastPosLeft = 0, lastPosRight = 0;
  float currentPosLeft = encoderLeft.getPulseCount();
  float currentPosRight = encoderRight.getPulseCount();
  
  if (abs(currentPosLeft - lastPosLeft) < 5) { // 检测打滑
    motorLeft.target *= 0.9; // 降速补偿
  }
  if (abs(currentPosRight - lastPosRight) < 5) {
    motorRight.target *= 0.9;
  }

  lastPosLeft = currentPosLeft;
  lastPosRight = currentPosRight;

  // 5. 监控输出
  Serial.print("Left RPM:"); Serial.print(motorLeft.shaftVelocity() * 60 / (2 * PI));
  Serial.print(" Right RPM:"); Serial.println(motorRight.shaftVelocity() * 60 / (2 * PI));
}

6、自主导航（位置控制为主，速度动态调整）

#include <SimpleFOC.h>

// 电机和传感器配置（同案例1）
BLDCMotor motorLeft(3), motorRight(9);
BLDCDriver3PWM driverLeft(5, 6, 7), driverRight(10, 11, 12);
Encoder encoderLeft(2, 4), encoderRight(3, 5);

// 导航变量
float targetX = 1000, targetY = 500; // 目标坐标（编码器脉冲）
float currentX = 0, currentY = 0;    // 当前坐标
float maxSpeed = 3.0;                // 最大速度（rad/s）

void setup() {
  Serial.begin(115200);
  
  // 电机初始化（默认位置控制）
  motorLeft.linkDriver(&driverLeft);
  motorLeft.linkSensor(&encoderLeft);
  motorLeft.controller = MotionControlType::position;
  motorLeft.PID_position.P = 20.0; // 高增益位置控制
  motorLeft.init();
  
  motorRight.linkDriver(&driverRight);
  motorRight.linkSensor(&encoderRight);
  motorRight.controller = MotionControlType::position;
  motorRight.init();

  // 模拟导航输入（实际应用中来自传感器或上位机）
  pinMode(A0, INPUT); // 目标X坐标
  pinMode(A1, INPUT); // 目标Y坐标
}

void loop() {
  // 1. 更新目标位置（模拟）
  targetX = map(analogRead(A0), 0, 1023, 0, 2000);
  targetY = map(analogRead(A1), 0, 1023, 0, 1000);

  // 2. 计算路径和速度
  float dx = targetX - currentX;
  float dy = targetY - currentY;
  float distance = sqrt(dx * dx + dy * dy);
  
  if (distance > 10) { // 距离阈值
    // 动态调整速度（接近目标时减速）
    float speed = constrain(distance * 0.01, 0.5, maxSpeed);
    
    // 差速控制（简化版，实际需用PID计算角度）
    float leftSpeed = speed * (1 + dx * 0.01); // 简单转向补偿
    float rightSpeed = speed * (1 - dx * 0.01);
    
    motorLeft.target = encoderLeft.getPulseCount() + leftSpeed * 10; // 脉冲增量
    motorRight.target = encoderRight.getPulseCount() + rightSpeed * 10;
  }

  // 3. 更新当前位置（需结合实际编码器分辨率）
  currentX = encoderLeft.getPulseCount();
  currentY = encoderRight.getPulseCount();

  // 4. 监控
  Serial.print("Current:"); Serial.print(currentX); Serial.print(","); Serial.print(currentY);
  Serial.print(" Target:"); Serial.print(targetX); Serial.print(","); Serial.println(targetY);
}

要点解读
差速模型与运动学解算
必须根据 车轮半径 和 轮距 计算差速（如案例2的leftSpeed/rightSpeed公式）。
直线运动时左右轮速度相同，转向时需满足 阿克曼几何。
控制模式切换策略
速度控制（动态响应）和位置控制（精确停靠）需 平滑过渡（如案例4的positionMode标志）。
避免硬切换导致的电机抖动。
传感器反馈与防滑处理
通过编码器脉冲变化检测打滑（如案例5的lastPosLeft/Right比较）。
必要时切换至 开环控制 或降低目标速度。
动态速度规划
接近目标位置时需 减速（如案例6的constrain(distance * 0.01, …)）。
可结合 S型曲线加减速算法 提高轨迹平滑性。
多轴协同与冗余设计
左右电机需 独立PID调参（案例1的PID_velocity.P可能不同）。
建议增加 通信超时保护（如案例3的distance > 10判断）。

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