在基于 Arduino 平台的 BLDC（无刷直流电机）轮式机器人控制系统中，“轨迹跟踪与负载自适应系统”是一种结合了高精度运动学规划与底层动力动态补偿的高级架构。该系统旨在解决机器人在复杂工况下因负载变化、地面摩擦不均导致的轨迹偏离和运动抖动问题。以下从专业视角为您详细解析其主要特点、应用场景及注意事项：
一、 主要特点
带航向角补偿的高精度路径跟踪
传统的轨迹跟踪仅关注位置误差，而该系统引入了航向角补偿机制。算法不仅计算机器人当前位置与目标路径的横向误差，还综合考量当前车头朝向与路径切线的夹角。通过前瞻控制（Look-ahead Point）动态调整左右轮的转速差，机器人能够以更平滑的弧线收敛至目标轨迹，有效抑制“锯齿形”振荡，显著提升路径跟踪的精度与平滑度。
基于负载识别的自适应 PID 控制
系统能够实时监测电机的运行状态（如电流变化），精准识别驱动轮的负载情况。当负载发生变化（如爬坡、搬运重物）时，自适应算法会动态调整 PID 控制器的比例、积分和微分参数。这种动态增益调度确保了电机在不同工作条件下均能保持极高的速度稳定性和响应速度。
双电机差速同步与抗扰动策略
针对差速底盘，系统引入了交叉耦合控制或主从同步策略。当一侧轮子因负载加重或地面阻力增大而减速时，算法会实时感知同步误差，并自动调整另一侧轮子的速度或扭矩输出，强制保持两轮线速度一致。这从根本上克服了因偏载或打滑导致的“蛇形”跑偏现象。
多传感器融合的状态估计
精准的轨迹跟踪高度依赖可靠的位姿反馈。系统通常采用 IMU（惯性测量单元）与轮式里程计（编码器）进行紧耦合的传感器融合（如扩展卡尔曼滤波）。这种融合机制能够有效补偿轮胎打滑带来的里程计累积误差，为上层轨迹跟踪算法提供无偏、高精度的速度和航向状态估计。
二、 应用场景
该架构主要适用于对运动精度、负载适应性和环境鲁棒性有较高要求的移动机器人场景：
高精度 AGV/AMR 物流搬运：在仓储环境中，机器人需要沿磁条或二维码精确行驶，且经常面临空载与满载的交替。自适应系统能确保在重载启动或制动时依然保持轨迹精准，不发生跑偏。
重载与偏载移动平台：当机器人搭载大型机械臂或不对称重物时，重心偏移会导致两侧轮子受力不均。负载自适应与差速同步控制能有效抵消偏载力矩，保证直线行驶的稳定性。
野外与非结构化地形巡检：在沙地、草地等摩擦系数多变的环境中，自适应算法能根据地形阻力隐含地调整控制增益，配合扭矩分配防止车轮打滑，确保机器人平稳通过复杂地形。
精密自动化作业设备：如智能割草机、高精度 3D 扫描平台等。航向角补偿机制能确保设备无遗漏地覆盖矩形区域，或在微米级步进中保持绝对精确的位置停留。
三、 需要注意的事项
算力瓶颈与 MCU 选型
自适应 PID、航向角补偿及传感器融合涉及大量的浮点运算。标准的 8 位 Arduino（如 Uno）主频过低，难以满足高频控制需求。必须选用带有硬件 FPU（浮点运算单元）和 DSP 指令的 32 位高性能 MCU，如 ESP32-S3、Teensy 4.x 或 STM32F4/H7 系列，以确保控制周期的确定性。
传感器精度与数据滤波
自适应算法对噪声极为敏感。低分辨率的霍尔传感器在低速下会产生量化噪声，导致 PID 高频震荡。建议优先选用高分辨率增量式或磁编码器。同时，必须在软件层面对传感器数据进行严格的数字滤波（如低通滤波、卡尔曼滤波），防止噪声被算法放大。
机械硬件标定与参数整定
算法无法完全弥补硬件的物理缺陷。上线前必须进行严格的机械标定，包括测量并补偿左右轮直径差异、胎压差异带来的固定速度偏差。此外，自适应 PID 的初始参数和航向补偿增益（K1, K2）需在实际系统中反复测试整定，避免参数过大引发系统振荡。
执行器饱和与系统安全性
当负载突变或遇到极大阻力时，自适应控制器可能会计算出超大的扭矩指令，导致电机电流饱和甚至失稳。必须在软件中设置严格的输出限幅（基于电机最大连续电流），并引入抗积分饱和（Anti-windup）机制。同时，需设计故障回退模式，当自适应模块失效时，系统能平滑降级为传统固定参数 PID，确保机器人不会失控。

1、基于编码器反馈的自适应PID闭环速度控制
适用场景：双轮差速机器人在平地上的轨迹跟踪，通过编码器实时监测转速，PID控制器根据负载变化自动调节PWM输出，维持精确的轨迹跟踪。

#include <Encoder.h>
#include <PID_v1.h>

// ==================== 编码器与电机定义 ====================
// 左轮编码器（使用中断引脚）
Encoder leftEnc(2, 3);   // A相/B相
Encoder rightEnc(4, 5);

// BLDC驱动引脚（假设使用SimpleFOC或ESC）
const int PWM_L = 9, PWM_R = 10;
const int DIR_L = 8, DIR_R = 7;

// ==================== PID参数 ====================
double setpointL = 0, setpointR = 0;  // 目标速度(RPM)
double inputL = 0, inputR = 0;        // 当前速度(RPM)
double outputL = 0, outputR = 0;      // 控制输出

// PID控制器（初始参数）
double Kp = 0.8, Ki = 0.2, Kd = 0.05;
PID pidL(&inputL, &outputL, &setpointL, Kp, Ki, Kd, DIRECT);
PID pidR(&inputR, &outputR, &setpointR, Kp, Ki, Kd, DIRECT);

// ==================== 负载识别参数 ====================
const int CURRENT_SENSOR_PIN = A0;     // 电流传感器（ACS712）
float baseCurrent = 0.5;               // 空载基准电流(A)
float currentThreshold = 1.0;          // 负载触发阈值

// ==================== 运动学参数 ====================
const float WHEEL_RADIUS = 0.05;       // 轮半径(m)
const float WHEELBASE = 0.30;          // 轮距(m)
const float ENCODER_PPR = 20.0;        // 编码器脉冲/转

void setup() {
  Serial.begin(115200);
  
  // 电机引脚初始化
  pinMode(PWM_L, OUTPUT); pinMode(PWM_R, OUTPUT);
  pinMode(DIR_L, OUTPUT); pinMode(DIR_R, OUTPUT);
  
  // PID初始化
  pidL.SetMode(AUTOMATIC);
  pidR.SetMode(AUTOMATIC);
  pidL.SetOutputLimits(-255, 255);
  pidR.SetOutputLimits(-255, 255);
  
  // 校准空载电流
  baseCurrent = readCurrent();
  Serial.println("空载电流基准: " + String(baseCurrent));
  
  delay(1000);
}

void loop() {
  static unsigned long lastTime = 0;
  if (millis() - lastTime >= 50) {  // 20Hz控制周期
    lastTime = millis();
    
    // 1. 读取编码器计算转速
    long countL = leftEnc.readAndReset();
    long countR = rightEnc.readAndReset();
    inputL = (countL * 60.0) / (ENCODER_PPR * 0.05);  // RPM
    inputR = (countR * 60.0) / (ENCODER_PPR * 0.05);
    
    // 2. 【核心】负载检测（基于电流传感器）
    float current = readCurrent();
    float loadFactor = constrain((current - baseCurrent) / currentThreshold, 0, 2.0);
    
    // 3. 【核心】自适应PID参数调整
    // 负载越大，P参数和I参数增大以提升响应
    double adaptiveKp = Kp * (1 + loadFactor * 0.5);
    double adaptiveKi = Ki * (1 + loadFactor * 0.3);
    double adaptiveKd = Kd * (1 + loadFactor * 0.2);
    
    pidL.SetTunings(adaptiveKp, adaptiveKi, adaptiveKd);
    pidR.SetTunings(adaptiveKp, adaptiveKi, adaptiveKd);
    
    // 4. 计算期望速度（轨迹跟踪）
    static float t = 0;
    t += 0.05;
    // 示例：跟踪圆形轨迹
    float v_linear = 0.3;                      // 线速度(m/s)
    float v_angular = 0.2 * sin(t * 0.2);      // 角速度(rad/s)
    
    // 差速逆解
    float vL = (v_linear - v_angular * WHEELBASE/2) / WHEEL_RADIUS;
    float vR = (v_linear + v_angular * WHEELBASE/2) / WHEEL_RADIUS;
    setpointL = vL * 60.0 / (2 * PI);  // 目标RPM
    setpointR = vR * 60.0 / (2 * PI);
    
    // 5. PID计算
    pidL.Compute();
    pidR.Compute();
    
    // 6. 驱动电机
    setMotor(PWM_L, DIR_L, outputL);
    setMotor(PWM_R, DIR_R, outputR);
    
    // 调试输出
    Serial.print("SetL:"); Serial.print(setpointL);
    Serial.print(" ActL:"); Serial.print(inputL);
    Serial.print(" Load:"); Serial.println(loadFactor);
  }
}

float readCurrent() {
  int raw = analogRead(CURRENT_SENSOR_PIN);
  return (raw * 5.0 / 1023.0) / 0.185;  // ACS712 5A量程
}

void setMotor(int pwmPin, int dirPin, float output) {
  int pwm = constrain(abs(output), 0, 255);
  digitalWrite(dirPin, output >= 0 ? HIGH : LOW);
  analogWrite(pwmPin, pwm);
}

代码要点：

负载检测：通过ACS712电流传感器读取电机电流，反映负载变化

自适应PID：根据负载系数动态调整Kp/Ki/Kd，高负载时增大增益提升响应

差速逆解：将期望线速度和角速度分解为左右轮目标转速

2、基于速度波动的负载识别与自适应控制
适用场景：不具备电流传感器的场合，通过编码器速度波动间接判断负载变化，更适合轻载/重载切换频繁的物流场景。

#include <Encoder.h>
#include <PID_v1.h>

// ==================== 编码器与电机 ====================
Encoder leftEnc(2, 3);
Encoder rightEnc(4, 5);
const int PWM_L = 9, PWM_R = 10;

// ==================== PID参数 ====================
double setpointL = 0, setpointR = 0;
double inputL = 0, inputR = 0;
double outputL = 0, outputR = 0;
PID pidL(&inputL, &outputL, &setpointL, 0.8, 0.2, 0.05, DIRECT);
PID pidR(&inputR, &outputR, &setpointR, 0.8, 0.2, 0.05, DIRECT);

// ==================== 负载识别参数 ====================
float speedHistoryL[10] = {0};  // 速度历史
float speedHistoryR[10] = {0};
int historyIndex = 0;
float baseSpeed = 0;            // 空载期望速度下的稳态转速
float loadFactor = 0;           // 负载系数

void setup() {
  Serial.begin(115200);
  pidL.SetMode(AUTOMATIC);
  pidR.SetMode(AUTOMATIC);
  pidL.SetOutputLimits(-255, 255);
  pidR.SetOutputLimits(-255, 255);
  
  // 建立空载基准
  Serial.println("校准空载基准...");
  calibrateBaseSpeed();
  Serial.println("基准速度: " + String(baseSpeed));
}

void loop() {
  static unsigned long lastTime = 0;
  if (millis() - lastTime >= 50) {
    lastTime = millis();
    
    // 1. 读取编码器
    long countL = leftEnc.readAndReset();
    long countR = rightEnc.readAndReset();
    inputL = (countL * 60.0) / (20.0 * 0.05);
    inputR = (countR * 60.0) / (20.0 * 0.05);
    
    // 2. 【核心】基于速度波动的负载识别
    // 更新速度历史
    speedHistoryL[historyIndex] = inputL;
    speedHistoryR[historyIndex] = inputR;
    historyIndex = (historyIndex + 1) % 10;
    
    // 计算平均速度偏差（实际速度与期望速度的比值）
    static float targetSpeedL = 100;  // 假设目标速度100 RPM
    float avgSpeedL = 0;
    for (int i = 0; i < 10; i++) avgSpeedL += speedHistoryL[i];
    avgSpeedL /= 10.0;
    
    // 负载越大，实际速度与期望速度的偏差越大
    float speedRatio = avgSpeedL / targetSpeedL;
    loadFactor = constrain((1 - speedRatio) * 2.0, 0, 2.0);
    
    // 3. 自适应PID
    double adaptiveKp = 0.8 * (1 + loadFactor * 0.6);
    double adaptiveKi = 0.2 * (1 + loadFactor * 0.4);
    pidL.SetTunings(adaptiveKp, adaptiveKi, 0.05);
    pidR.SetTunings(adaptiveKp, adaptiveKi, 0.05);
    
    // 4. 轨迹跟踪
    setpointL = targetSpeedL;
    setpointR = targetSpeedL;
    
    pidL.Compute();
    pidR.Compute();
    
    analogWrite(PWM_L, constrain(abs(outputL), 0, 255));
    analogWrite(PWM_R, constrain(abs(outputR), 0, 255));
    
    Serial.print("Load:"); Serial.print(loadFactor);
    Serial.print(" Kp:"); Serial.println(adaptiveKp);
  }
}

void calibrateBaseSpeed() {
  // 设定目标速度并记录稳态转速
  setpointL = 100; setpointR = 100;
  delay(2000);
  baseSpeed = inputL;
}

代码要点：

无电流传感器的负载识别：通过监测实际速度与期望速度的偏差来推断负载

速度历史滑动平均：滤波消除瞬时干扰

自适应策略：负载增大时，PID增益自动提高以维持速度精度

3、速度区间约束 + 自适应重物质量跟踪控制
适用场景：重载AGV在搬运不同质量货物时的轨迹跟踪，需将运动速度约束在安全区间内，同时自适应不同重物质量，确保安全与效率。

#include <Encoder.h>
#include <PID_v1.h>

// ==================== 硬件定义 ====================
Encoder leftEnc(2, 3);
Encoder rightEnc(4, 5);
const int PWM_L = 9, PWM_R = 10;

// ==================== PID参数 ====================
double setpointL = 0, setpointR = 0;
double inputL = 0, inputR = 0;
double outputL = 0, outputR = 0;
PID pidL(&inputL, &outputL, &setpointL, 0.8, 0.2, 0.05, DIRECT);
PID pidR(&inputR, &outputR, &setpointR, 0.8, 0.2, 0.05, DIRECT);

// ==================== 速度区间约束 ====================
const float V_MIN = 0.1;      // 最小线速度(m/s)
const float V_MAX = 0.8;      // 最大线速度(m/s)
const float W_MAX = 1.0;      // 最大角速度(rad/s)

// ==================== 负载估计参数 ====================
float estimatedMass = 0;        // 估计负载质量(kg)
float baseMass = 0;             // 空载质量
const float ENCODER_PPR = 20.0;
const float WHEEL_RADIUS = 0.05;
const float WHEELBASE = 0.30;

void setup() {
  Serial.begin(115200);
  pidL.SetMode(AUTOMATIC);
  pidR.SetMode(AUTOMATIC);
  pidL.SetOutputLimits(-255, 255);
  pidR.SetOutputLimits(-255, 255);
  
  // 读取空载质量（系统初始质量）
  baseMass = estimateMass();
  Serial.println("空载质量: " + String(baseMass) + " kg");
}

void loop() {
  static unsigned long lastTime = 0;
  if (millis() - lastTime >= 50) {
    lastTime = millis();
    
    // 1. 读取编码器
    long countL = leftEnc.readAndReset();
    long countR = rightEnc.readAndReset();
    inputL = (countL * 60.0) / (ENCODER_PPR * 0.05);
    inputR = (countR * 60.0) / (ENCODER_PPR * 0.05);
    
    // 2. 【核心】估算负载质量
    float currentMass = estimateMass();
    float massChange = currentMass - baseMass;
    estimatedMass = 0.9 * estimatedMass + 0.1 * massChange;  // 低通滤波
    
    // 3. 【核心】速度区间约束
    // 根据负载质量限制最大速度（重载时减速）
    float maxSpeed = V_MAX * (1 - constrain(estimatedMass / 50.0, 0, 0.5));
    maxSpeed = max(maxSpeed, V_MIN);  // 不低于最小速度
    
    // 4. 自适应PID
    // 负载越大，增益越大，同时增加积分项权重
    double massFactor = 1 + constrain(estimatedMass / 20.0, 0, 2.0);
    pidL.SetTunings(0.8 * massFactor, 0.2 * massFactor, 0.05);
    pidR.SetTunings(0.8 * massFactor, 0.2 * massFactor, 0.05);
    
    // 5. 轨迹跟踪（带速度约束）
    static float t = 0;
    t += 0.05;
    float v_linear = 0.3 + 0.2 * sin(t * 0.1);
    float v_angular = 0.3 * cos(t * 0.1);
    
    // 应用速度区间约束
    v_linear = constrain(v_linear, V_MIN, maxSpeed);
    v_angular = constrain(v_angular, -W_MAX, W_MAX);
    
    // 差速逆解
    float vL = (v_linear - v_angular * WHEELBASE/2) / WHEEL_RADIUS;
    float vR = (v_linear + v_angular * WHEELBASE/2) / WHEEL_RADIUS;
    setpointL = vL * 60.0 / (2 * PI);
    setpointR = vR * 60.0 / (2 * PI);
    
    pidL.Compute();
    pidR.Compute();
    
    analogWrite(PWM_L, constrain(abs(outputL), 0, 255));
    analogWrite(PWM_R, constrain(abs(outputR), 0, 255));
    
    Serial.print("Mass:"); Serial.print(estimatedMass);
    Serial.print(" Vmax:"); Serial.print(maxSpeed);
    Serial.print(" v:"); Serial.println(v_linear);
  }
}

float estimateMass() {
  // 通过速度和加速阶段的电流/扭矩关系估算质量
  // 简化：使用当前速度与期望速度的偏差估算
  static float steadySpeed = 0;
  if (abs(setpointL - setpointR) < 5) {
    steadySpeed = (inputL + inputR) / 2;
  }
  // 负载越大，稳态速度越低（同PWM下）
  float speedDrop = 1.0 - steadySpeed / abs(setpointL);
  return baseMass + speedDrop * 30.0;  // 经验系数
}

代码要点：

速度区间约束：根据负载质量动态调整最大速度，重载时自动限速保证安全

质量在线估计：通过编码器速度偏差估算负载质量

安全导向：空载时可达全速，重载时自动降低速度上限

要点解读
速度闭环是轨迹跟踪的基础保障：单纯的PWM开环控制无法抵抗负载变化导致的转速波动。引入编码器反馈的闭环PID控制，使实际转速精确跟踪目标值，这是所有轨迹跟踪算法的前提。

负载识别的本质是“感知-决策-执行”的闭环：通过电流传感器或编码器速度偏差感知负载变化→在线调整PID参数→执行器输出补偿力矩→再次感知形成闭环。这种自适应机制使机器人在重载时自动增大输出扭矩，维持速度精度。

差速逆解是轨迹跟踪的数学桥梁：将期望的线速度v和角速度ω分解为左右轮目标转速，公式为v_L = (v - ω·B/2)/R，v_R = (v + ω·B/2)/R。高精度逆解要求精确的轮径R和轮距B参数。

速度区间约束是安全与效率的平衡器：重载时若仍以空载速度运行，可能因制动距离变长导致失控。通过速度区间约束，在重载时自动限速，轻载时恢复全速，同时保障安全和效率。

自适应策略需考虑系统稳定性：PID参数的实时调整可能引入不稳定性。工程实践中应采用参数平滑变化（如低通滤波）和参数限幅，避免参数突变导致系统振荡。

4、纯追踪模型 + 负载前馈补偿
适用场景：室内AGV、仓储搬运机器人。纯追踪模型跟踪预定义路径点，负载变化时通过前馈补偿保持跟踪精度。

/* ===== Arduino BLDC 轮式机器人 — 纯追踪模型 + 负载前馈补偿 =====
 * 硬件：2×BLDC差速底盘 + 编码器 + 电流传感器(检测负载)
 * 核心：纯追踪几何跟踪 → 负载检测 → 前馈力矩补偿
 */
#include <SimpleFOC.h>
#include <Math.h>
#include <vector>

BLDCMotor motorL(5), motorR(6);
BLDCDriver3PWM drvL, drvR;
Encoder encL(2,3,2048), encR(4,5,2048);

// 路径点
struct Waypoint { float x, y; };
std::vector<Waypoint> path;
int pathIndex = 0;

// 机器人状态
float robotX = 0, robotY = 0, robotYaw = 0;
float robotVx = 0, robotVy = 0;

// 纯追踪参数
const float LOOKAHEAD_DIST = 0.5;  // 预瞄距离(m)
const float BASE_SPEED = 0.3;      // 基础速度(m/s)

// 负载检测(通过电流)
float loadCurrent = 0;
float loadRatio = 1.0;  // 负载比例(1.0=空载)

// 前馈补偿
float feedForwardTorque = 0;
const float FF_GAIN = 0.002;  // 前馈增益

void setup() {
  Serial.begin(115200);
  
  // 初始化路径(正方形)
  path.push_back({0, 0});
  path.push_back({2, 0});
  path.push_back({2, 2});
  path.push_back({0, 2});
  
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
}

void loop() {
  float dt = 0.02;
  
  // 1. 更新位姿
  robotVx = (motorL.shaft_velocity + motorR.shaft_velocity) / 2.0 * cos(robotYaw);
  robotVy = (motorL.shaft_velocity + motorR.shaft_velocity) / 2.0 * sin(robotYaw);
  robotX += robotVx * dt;
  robotY += robotVy * dt;
  
  // 2. 检测负载(通过电机电流)
  loadCurrent = (abs(motorL.current) + abs(motorR.current)) / 2.0;
  loadRatio = 1.0 + loadCurrent * 0.1;  // 电流越大负载越重
  
  // 3. 纯追踪找目标点
  if(pathIndex >= path.size()) {
    motorL.move(0); motorR.move(0);
    return;
  }
  
  Waypoint target = path[pathIndex];
  float dx = target.x - robotX;
  float dy = target.y - robotY;
  float dist = sqrt(dx*dx + dy*dy);
  
  // 到达路径点
  if(dist < 0.1) {
    pathIndex++;
    if(pathIndex >= path.size()) return;
    target = path[pathIndex];
    dx = target.x - robotX;
    dy = target.y - robotY;
    dist = sqrt(dx*dx + dy*dy);
  }
  
  // 4. 预瞄点计算(纯追踪)
  float alpha = atan2(dy, dx) - robotYaw;
  float lookahead = min(LOOKAHEAD_DIST, dist);
  
  // 5. 负载前馈补偿
  feedForwardTorque = FF_GAIN * (loadRatio - 1.0) * BASE_SPEED;
  
  // 6. 控制输出
  float vLin = BASE_SPEED * 1000;
  float vAng = 2.0 * sin(alpha) / lookahead;
  
  // 加入前馈补偿
  vLin += feedForwardTorque * 500;
  
  motorL.move(vLin - vAng * 80);
  motorR.move(vLin + vAng * 80);
  
  motorL.loopFOC();
  motorR.loopFOC();
  delay(20);
}

关键设计点：
纯追踪模型根据预瞄距离计算转向角
电机电流检测负载变化
前馈补偿抵消负载引起的速度损失
路径点到达检测

5、Stanley控制器 + 自适应增益
适用场景：户外巡检机器人、校园快递车。Stanley控制器跟踪路径，根据负载和速度自适应调整控制增益。

/* ===== Arduino BLDC 轮式机器人 — Stanley控制器 + 自适应增益 =====
 * 硬件：2×BLDC差速底盘 + GPS/IMU + 编码器
 * 核心：Stanley横向控制 → 根据负载和速度自适应调整增益
 */
#include <SimpleFOC.h>
#include <Math.h>

BLDCMotor motorL(5), motorR(6);

// 路径(多项式拟合)
float pathCoeffs[4];  // 三次多项式系数
float pathLength = 10;

// 机器人状态
float robotX = 0, robotY = 0, robotYaw = 0;
float robotSpeed = 0;

// Stanley参数
float K_stanley = 0.5;      // 横向误差增益(自适应)
float K_soft = 2.0;         // 软约束增益
const float MAX_STEER = 1.5; // 最大转向角

// 负载检测
float motorCurrent = 0;
float adaptiveFactor = 1.0;

void stanleyControl() {
  // 1. 找到路径上最近点
  float nearestX, nearestY, nearestYaw;
  float crossTrackError = findNearestPoint(robotX, robotY, &nearestX, &nearestY, &nearestYaw);
  
  // 2. 航向误差
  float headingError = nearestYaw - robotYaw;
  while(headingError > PI) headingError -= 2*PI;
  while(headingError < -PI) headingError += 2*PI;
  
  // 3. 自适应增益
  // 负载越大、速度越快 → 增益越大
  adaptiveFactor = 1.0 + motorCurrent * 0.2 + robotSpeed * 0.3;
  float K_adaptive = K_stanley * adaptiveFactor;
  
  // 4. Stanley控制律
  float steerAngle = headingError + atan2(K_adaptive * crossTrackError, robotSpeed + K_soft);
  steerAngle = constrain(steerAngle, -MAX_STEER, MAX_STEER);
  
  // 5. 速度控制(带负载补偿)
  float targetSpeed = BASE_SPEED * (1.0 - abs(crossTrackError) * 0.3);
  targetSpeed = max(targetSpeed, 0.1);
  
  // 6. 执行
  float vLin = targetSpeed * 1000;
  float vAng = steerAngle * 1.5;
  
  motorL.move(vLin - vAng * 80);
  motorR.move(vLin + vAng * 80);
}

float findNearestPoint(float x, float y, float* nx, float* ny, float* nyaw) {
  // 简化：沿路径采样找最近点
  float minDist = 999;
  float bestT = 0;
  
  for(float t=0; t<=pathLength; t+=0.1) {
    float px = evaluatePoly(t, pathCoeffs[0], pathCoeffs[1], pathCoeffs[2], pathCoeffs[3]);
    float py = evaluatePoly(t, pathCoeffs[0], pathCoeffs[1], pathCoeffs[2], pathCoeffs[3]);
    float d = sqrt(pow(x-px,2) + pow(y-py,2));
    if(d < minDist) {
      minDist = d;
      bestT = t;
    }
  }
  
  *nx = evaluatePoly(bestT, pathCoeffs[0], pathCoeffs[1], pathCoeffs[2], pathCoeffs[3]);
  *ny = evaluatePoly(bestT, pathCoeffs[0], pathCoeffs[1], pathCoeffs[2], pathCoeffs[3]);
  *nyaw = atan2(evaluateDerivative(bestT, pathCoeffs[1], pathCoeffs[2], pathCoeffs[3]), 1);
  
  return minDist;
}

void setup() {
  Serial.begin(115200);
  
  // 路径系数(示例直线)
  pathCoeffs[0] = 0; pathCoeffs[1] = 0.5;
  pathCoeffs[2] = 0; pathCoeffs[3] = 0;
  
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
}

void loop() {
  // 1. 更新状态
  robotSpeed = (motorL.shaft_velocity + motorR.shaft_velocity) / 2.0;
  motorCurrent = (abs(motorL.current) + abs(motorR.current)) / 2.0;
  
  // 2. Stanley控制
  stanleyControl();
  
  motorL.loopFOC();
  motorR.loopFOC();
  delay(20);
}

关键设计点：
Stanley控制器结合横向误差和航向误差
自适应增益根据负载和速度调整
路径用多项式表示，便于求导
横向误差大时自动减速

6、模型预测控制(MPC) + 负载辨识
适用场景：高速移动机器人、重载AGV。MPC预测未来轨迹，结合在线负载辨识优化控制量。

/* ===== Arduino BLDC 轮式机器人 — MPC + 负载辨识 =====
 * 硬件：2×BLDC差速底盘 + 编码器 + 电流传感器
 * 核心：MPC预测未来N步轨迹 → 在线负载辨识 → 优化控制
 */
#include <SimpleFOC.h>
#include <Math.h>

BLDCMotor motorL(5), motorR(6);

// MPC参数
const int N = 10;           // 预测步数
const float DT_MPC = 0.1;   // 预测步长时间(s)
const float MAX_V = 0.5;    // 最大速度
const float MAX_W = 1.5;    // 最大角速度

// 机器人模型参数(在线辨识)
float massEstimate = 10.0;  // 质量估计(kg)
float frictionEstimate = 0.5; // 摩擦系数估计

// 路径
float refPath[N][2];  // 未来N步参考路径

// 负载辨识
void identifyLoad() {
  // 基于加速度和电流的关系辨识负载
  static float lastSpeed = 0;
  float acceleration = (motorL.shaft_velocity - lastSpeed) / 0.02;
  lastSpeed = motorL.shaft_velocity;
  
  float torque = (abs(motorL.current) + abs(motorR.current)) / 2.0 * 0.1;
  
  if(abs(acceleration) > 0.01) {
    // F = ma + f*v
    float estimatedMass = (torque - frictionEstimate * motorL.shaft_velocity) / acceleration;
    massEstimate = 0.9 * massEstimate + 0.1 * estimatedMass;
    massEstimate = constrain(massEstimate, 5, 50);
  }
}

// MPC优化(简化：暴力搜索)
void mpcOptimize(float* optV, float* optW) {
  float bestCost = 999;
  float bestV = 0, bestW = 0;
  
  // 速度空间采样
  for(float v=0; v<=MAX_V; v+=0.05) {
    for(float w=-MAX_W; w<=MAX_W; w+=0.1) {
      // 模拟未来轨迹
      float simX = robotX, simY = robotY, simYaw = robotYaw;
      float cost = 0;
      
      for(int k=0; k<N; k++) {
        // 运动学模型(带负载影响)
        simYaw += w * DT_MPC;
        simX += v * cos(simYaw) * DT_MPC;
        simY += v * sin(simYaw) * DT_MPC;
        
        // 代价：跟踪误差 + 控制量惩罚
        float dx = simX - refPath[k][0];
        float dy = simY - refPath[k][1];
        cost += dx*dx + dy*dy;
        cost += 0.01 * v*v + 0.01 * w*w;
        
        // 障碍物惩罚(简化)
        if(simX < 0 || simX > 5 || simY < 0 || simY > 5) cost += 100;
      }
      
      if(cost < bestCost) {
        bestCost = cost;
        bestV = v;
        bestW = w;
      }
    }
  }
  
  *optV = bestV;
  *optW = bestW;
}

void setup() {
  Serial.begin(115200);
  
  // 初始化参考路径(直线)
  for(int k=0; k<N; k++) {
    refPath[k][0] = k * 0.3;
    refPath[k][1] = 1.0;
  }
  
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
}

void loop() {
  // 1. 负载辨识
  identifyLoad();
  
  // 2. 更新参考路径
  updateReferencePath();
  
  // 3. MPC优化
  float optV, optW;
  mpcOptimize(&optV, &optW);
  
  // 4. 执行
  motorL.move(optV * 1000 - optW * 80);
  motorR.move(optV * 1000 + optW * 80);
  
  // 5. 更新位姿
  float dt = 0.02;
  robotX += optV * cos(robotYaw) * dt;
  robotY += optV * sin(robotYaw) * dt;
  robotYaw += optW * dt;
  
  motorL.loopFOC();
  motorR.loopFOC();
  delay(20);
}

关键设计点：
MPC预测未来N步轨迹并优化
在线负载辨识更新模型参数
暴力搜索速度空间找最优解
代价函数包含跟踪误差和控制量惩罚

要点解读
① 轨迹跟踪的核心是“横向控制”
横向控制决定了机器人能否精确跟踪路径。
② 负载自适应是重载AGV的刚需
负载变化对轨迹跟踪的影响：
加速/减速变慢
转向响应变慢
稳态误差增大
三种案例分别用了不同的自适应方法：
案例一：前馈补偿（直接抵消）
案例二：自适应增益（调整控制器参数）
案例三：负载辨识（更新模型参数）
③ 三种方案的适用场景
④ MPC的计算量是Arduino的瓶颈
案例三中的暴力搜索在Arduino上只能做到10Hz左右。优化方法：
减少采样密度（粗搜索+精细搜索）
使用梯度下降代替暴力搜索
将MPC放在上位机，Arduino只执行
⑤ 实际部署建议：分层控制
上层：路径规划(低频)
中层：轨迹跟踪(50Hz)
下层：电机控制(1kHz)
负载自适应应该在中层实现，不影响下层的实时性。

请注意：以上案例仅作为思路拓展的参考示例，不保证完全正确、适配所有场景或可直接编译运行。由于硬件平台、实际使用场景、Arduino 版本的差异，均可能影响代码的适配性与使用方法的选择。在实际编程开发时，请务必根据自身硬件配置、使用场景及具体功能需求进行针对性调整，并通过多次实测验证效果；同时需确保硬件接线正确，充分了解所用传感器、执行器等设备的技术规范与核心特性。对于涉及硬件操作的代码，使用前务必核对引脚定义、电平参数等关键信息的准确性与安全性，避免因参数错误导致硬件损坏或运行异常。