创建一个不仅能保持平衡，还能对外部指令（如遥控速度）做出快速、平稳响应，并能自适应不同运行状态的智能系统。

一、 主要特点
这个系统超越了基本的PID平衡控制，具备更智能、更鲁棒的行为。
分级控制结构
系统通常采用串级PID控制：
内环（速度环）： 响应最快。接收期望转速与实际转速（由电机编码器测得）的偏差，通过PID计算后输出给电机。此环路由BLDC驱动器在底层实现。
外环（角度环）： 核心平衡环。接收期望角度（通常为0°，即竖直）与实测角度（由IMU滤波得到）的偏差，通过PID计算后，其输出作为内环的设定值。即：为了平衡，需要让轮子加速。
动态调参
问题： 固定参数的PID控制器在平衡车静止时表现良好，但当车高速运动或大力加速时，可能会变得过于“敏感”或“迟钝”，导致振荡或失稳。
解决方案： 根据机器人的状态（如车速、倾斜角速度）动态调整角度环或速度环的PID参数。
示例： 当车速较高时，适当减小角度环的P增益，使控制器“反应慢一点”，避免因过度纠正而引起前后振荡（俗称“跷跷板”现象）。
速度前馈控制
问题： 当收到前进指令时，传统的反馈控制要等到车身开始前倾（即已出现角度误差）后，才发力纠正，响应有延迟，导致启动“抬头”现象。
解决方案： 速度前馈绕过反馈回路。当接收到速度指令时，直接向电机施加一个与指令加速度成比例的扭矩。这个扭矩正好用来克服车体和负载的惯性，从而实现“无缝”的启动和制动，极大改善操控手感。
复杂的传感器融合
平衡需要精确的姿态角。这需要通过卡尔曼滤波或互补滤波，融合IMU中陀螺仪（短期精确）和加速度计（长期基准）的数据，以得到稳定、无漂移的俯仰角。

二、 应用场景
这种高水平的平衡控制技术是各类自平衡机器人和载具的基础。
高性能自平衡机器人：
两轮自平衡机器人： 如经典的Segway模型，是此技术的直接应用。
独轮车/悬浮滑板： 控制原理完全相同，但对动态稳定性的要求更高。
仿生机器人：
双足机器人： 其站立和行走时的平衡控制，可以看作是在前后和左右两个方向上的倒立摆问题，控制逻辑相通。
高级个人交通工具：
任何需要动态保持平衡的电动载具，其核心控制器都采用了这些先进技术来保证行驶的平稳和安全。
教学与研究平台：
作为验证现代控制理论（自适应控制、前馈控制、状态空间控制等）的理想实验平台。

三、 需要注意的事项（实现难点与挑战）
实现这样一个系统是嵌入式控制领域的顶级挑战之一，难点遍布每个环节。
状态估计的精确性与实时性（最核心的挑战）
问题： 所有控制都基于准确的俯仰角和角速度。IMU数据噪声大，且加速度计在机体加速时会受运动干扰。
解决方案与挑战：
必须使用传感器融合。互补滤波是Arduino上性价比最高的选择，但卡尔曼滤波效果更优。
滤波算法的实时性： 融合算法必须在严格定时中断内完成，任何延迟都会导致控制失效。
动态调参策略的设计
问题： 如何设计调参规则？调参函数应该是线性的还是非线性的？调整哪些参数？
挑战：
这没有标准答案，严重依赖于机器人的机械参数（质量、重心高度等）。
调参过程异常复杂，需要大量的实验和数据记录，甚至需要引入更高级的自适应控制算法（如模型参考自适应）。
速度前馈量的确定
问题： 前馈扭矩应该给多大？这需要知道机器人的总质量和惯性。
挑战：
前馈量太小，效果不明显；太大，会引起超调甚至振荡。
通常需要通过实验来整定这个前馈增益系数，使其与系统的惯性相匹配。
执行器（BLDC）的响应性能
问题： 平衡控制要求电机扭矩响应极快。如果BLDC的电流环/速度环响应慢，整个系统将无法稳定。
解决方案：
必须使用带高性能FOC驱动的BLDC电机，确保扭矩响应快速、平滑。
底层电机的PID参数也需要精心整定。
安全与稳定性监控
问题： 复杂的算法一旦在某种极端情况下失效，机器人会瞬间摔倒，可能造成损坏。
解决方案：
必须编写看门狗程序或安全监控逻辑。例如，当检测到倾角超过安全范围时，立即切断电机动力，保护齿轮和机身。

1、基础速度前馈平衡车控制

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

// 编码器引脚定义
#define ENC_A_LEFT 2
#define ENC_B_LEFT 3
#define ENC_A_RIGHT 4
#define ENC_B_RIGHT 5

// 电机控制引脚
#define MOTOR_LEFT_PWM 9
#define MOTOR_LEFT_DIR1 7
#define MOTOR_LEFT_DIR2 8
#define MOTOR_RIGHT_PWM 10
#define MOTOR_RIGHT_DIR1 11
#define MOTOR_RIGHT_DIR2 12

// 全局变量
Encoder leftEncoder(ENC_A_LEFT, ENC_B_LEFT);
Encoder rightEncoder(ENC_A_RIGHT, ENC_B_RIGHT);

double currentLeftSpeed = 0;
double currentRightSpeed = 0;
double targetSpeed = 0;
double speedError = 0;
double motorPower = 0;

// PID参数 - 可动态调整
double Kp = 0.8;
double Ki = 0.05;
double Kd = 0.1;
double feedforwardGain = 0.6;

// 速度前馈函数
double calculateFeedforward(double targetVel) {
    return feedforwardGain * targetVel;
}

void setup() {
    pinMode(MOTOR_LEFT_PWM, OUTPUT);
    pinMode(MOTOR_LEFT_DIR1, OUTPUT);
    pinMode(MOTOR_LEFT_DIR2, OUTPUT);
    pinMode(MOTOR_RIGHT_PWM, OUTPUT);
    pinMode(MOTOR_RIGHT_DIR1, OUTPUT);
    pinMode(MOTOR_RIGHT_DIR2, OUTPUT);
    
    Serial.begin(9600);
    
    // 设置初始方向
    setMotorDirection(MOTOR_LEFT_DIR1, MOTOR_LEFT_DIR2, true);
    setMotorDirection(MOTOR_RIGHT_DIR1, MOTOR_RIGHT_DIR2, true);
}

void loop() {
    static unsigned long lastTime = 0;
    unsigned long currentTime = millis();
    float dt = (currentTime - lastTime) / 1000.0;
    
    if (dt > 0.01) { // 10ms控制周期
        // 读取编码器值并计算速度
        long leftCount = leftEncoder.read();
        long rightCount = rightEncoder.read();
        
        currentLeftSpeed = leftCount / (dt * CPR); // CPR为每转脉冲数
        currentRightSpeed = rightCount / (dt * CPR);
        
        // 平均速度
        double avgSpeed = (currentLeftSpeed + currentRightSpeed) / 2.0;
        
        // 计算误差
        speedError = targetSpeed - avgSpeed;
        
        // PID控制 + 速度前馈
        double proportional = Kp * speedError;
        double integralTerm = integrateError(speedError, dt);
        double derivative = Kd * (speedError - lastError) / dt;
        double feedforward = calculateFeedforward(targetSpeed);
        
        motorPower = proportional + integralTerm + derivative + feedforward;
        
        // 限制功率范围
        motorPower = constrain(motorPower, -255, 255);
        
        // 应用控制
        applyMotorControl(motorPower);
        
        // 更新上一次误差
        lastError = speedError;
        lastTime = currentTime;
        
        // 重置编码器
        leftEncoder.write(0);
        rightEncoder.write(0);
        
        // 串口监控
        monitorSystem();
    }
}

void setMotorDirection(int dir1Pin, int dir2Pin, bool forward) {
    digitalWrite(dir1Pin, forward ? HIGH : LOW);
    digitalWrite(dir2Pin, forward ? LOW : HIGH);
}

void applyMotorControl(double power) {
    bool direction = power >= 0;
    double absPower = abs(power);
    
    setMotorDirection(MOTOR_LEFT_DIR1, MOTOR_LEFT_DIR2, direction);
    setMotorDirection(MOTOR_RIGHT_DIR1, MOTOR_RIGHT_DIR2, direction);
    
    analogWrite(MOTOR_LEFT_PWM, absPower);
    analogWrite(MOTOR_RIGHT_PWM, absPower);
}

double integrateError(double error, float dt) {
    static double integral = 0;
    integral += error * dt;
    return Ki * integral;
}

void monitorSystem() {
    Serial.print("Target: "); Serial.print(targetSpeed);
    Serial.print(" | Current: "); Serial.print((currentLeftSpeed + currentRightSpeed)/2.0);
    Serial.print(" | Error: "); Serial.print(speedError);
    Serial.print(" | Power: "); Serial.println(motorPower);
}

2、自适应参数调整平衡车

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

// 编码器和电机引脚定义...

// 自适应参数类
class AdaptiveController {
private:
    double baseKp, baseKi, baseKd, baseFeedforward;
    double adaptiveKp, adaptiveKi, adaptiveKd, adaptiveFeedforward;
    double maxSpeed, minSpeed;
    
public:
    AdaptiveController(double kp, double ki, double kd, double ff, double maxSpd, double minSpd) {
        baseKp = kp;
        baseKi = ki;
        baseKd = kd;
        baseFeedforward = ff;
        maxSpeed = maxSpd;
        minSpeed = minSpd;
        
        // 初始化自适应参数
        adaptiveKp = kp;
        adaptiveKi = ki;
        adaptiveKd = kd;
        adaptiveFeedforward = ff;
    }
    
    void updateParameters(double currentSpeed, double targetSpeed) {
        // 根据速度差调整参数
        double speedDiff = abs(targetSpeed - currentSpeed);
        double normalizedDiff = constrain(speedDiff / maxSpeed, 0, 1);
        
        // 动态调整PID参数
        adaptiveKp = baseKp * (1 + normalizedDiff * 0.5);
        adaptiveKi = baseKi * (1 + normalizedDiff * 0.3);
        adaptiveKd = baseKd * (1 + normalizedDiff * 0.2);
        
        // 动态调整前馈增益
        adaptiveFeedforward = baseFeedforward * (1 + normalizedDiff * 0.4);
    }
    
    double getKp() { return adaptiveKp; }
    double getKi() { return adaptiveKi; }
    double getKd() { return adaptiveKd; }
    double getFeedforward() { return adaptiveFeedforward; }
};

// 主程序中使用自适应控制器
AdaptiveController adaptCtrl(0.8, 0.05, 0.1, 0.6, 5.0, 0.1);

void loop() {
    // 读取编码器和计算速度...
    
    // 更新自适应参数
    double avgSpeed = (currentLeftSpeed + currentRightSpeed) / 2.0;
    adaptCtrl.updateParameters(avgSpeed, targetSpeed);
    
    // 使用更新后的参数进行控制
    double currentKp = adaptCtrl.getKp();
    double currentKi = adaptCtrl.getKi();
    double currentKd = adaptCtrl.getKd();
    double currentFF = adaptCtrl.getFeedforward();
    
    // PID计算...
    motorPower = currentKp * speedError + 
                 currentKi * integralTerm + 
                 currentKd * derivative + 
                 currentFF * targetSpeed;
    
    // 其余控制逻辑...
}

3、完整动态调参平衡车系统

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

// IMU传感器用于姿态检测
Adafruit_MPU6050 mpu;

// 系统状态枚举
enum ControlMode { POSITION_CONTROL, SPEED_CONTROL, BALANCE_CONTROL };
enum SystemState { IDLE, RUNNING, EMERGENCY_STOP };

// 全局变量
ControlMode currentMode = BALANCE_CONTROL;
SystemState systemState = IDLE;
double balanceAngle = 0; // 目标平衡角度
double currentAngle = 0; // 当前角度
double angularVelocity = 0; // 角速度

// 动态参数表
struct TuningParams {
    double Kp_balance;
    double Ki_balance;
    double Kd_balance;
    double Kp_speed;
    double Feedforward;
    double MaxPower;
};

// 存储多个参数集
TuningParams paramsSet[3];
int activeParamSet = 0;

void setup() {
    // 初始化IMU
    if (!mpu.begin()) {
        Serial.println("Failed to start MPU6050!");
        while (1) delay(10);
    }
    mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
    mpu.setGyroscopeRange(MPU6050_RANGE_500_DPS);
    
    // 初始化参数集
    paramsSet[0] = {1.2, 0.02, 0.3, 0.9, 0.7, 200}; // 低速模式
    paramsSet[1] = {1.0, 0.01, 0.2, 0.7, 0.6, 220}; // 中速模式
    paramsSet[2] = {0.8, 0.005, 0.1, 0.5, 0.5, 250}; // 高速模式
    
    Serial.begin(115200);
    Serial.println("Balance Car with Dynamic Tuning");
}

void loop() {
    // 读取IMU数据
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);
    
    // 计算角度和角速度
    currentAngle = atan2(a.acceleration.x, a.acceleration.z) * 180 / PI;
    angularVelocity = g.gyroscope.y;
    
    // 根据速度动态选择参数集
    double currentSpeed = (currentLeftSpeed + currentRightSpeed) / 2.0;
    selectParameterSet(currentSpeed);
    
    // 执行相应控制模式
    switch(currentMode) {
        case BALANCE_CONTROL:
            executeBalanceControl();
            break;
        case SPEED_CONTROL:
            executeSpeedControl();
            break;
        case POSITION_CONTROL:
            executePositionControl();
            break;
    }
    
    // 监控系统状态
    monitorSafety();
    
    delay(10); // 控制周期约10ms
}

void executeBalanceControl() {
    // 平衡控制算法 - 使用PD控制加速度前馈
    double angleError = balanceAngle - currentAngle;
    
    // PD控制部分
    double pdControl = paramsSet[activeParamSet].Kp_balance * angleError - 
                      paramsSet[activeParamSet].Kd_balance * angularVelocity;
    
    // 速度前馈部分
    double speedFeedforward = paramsSet[activeParamSet].Feedforward * targetSpeed;
    
    // 组合控制输出
    motorPower = pdControl + speedFeedforward;
    
    // 应用安全限制
    motorPower = constrain(motorPower, -paramsSet[activeParamSet].MaxPower, 
                          paramsSet[activeParamSet].MaxPower);
    
    // 应用电机控制
    applyMotorControl(motorPower);
}

void executeSpeedControl() {
    // 速度PID控制加前馈
    double speedError = targetSpeed - (currentLeftSpeed + currentRightSpeed) / 2.0;
    
    double proportional = paramsSet[activeParamSet].Kp_speed * speedError;
    double feedforward = paramsSet[activeParamSet].Feedforward * targetSpeed;
    
    motorPower = proportional + feedforward;
    motorPower = constrain(motorPower, -255, 255);
    
    applyMotorControl(motorPower);
}

void selectParameterSet(double speed) {
    // 根据当前速度选择合适的参数集
    if (abs(speed) < 1.0) {
        activeParamSet = 0; // 低速参数
    } else if (abs(speed) < 3.0) {
        activeParamSet = 1; // 中速参数
    } else {
        activeParamSet = 2; // 高速参数
    }
}

void monitorSafety() {
    // 紧急情况处理
    if (abs(currentAngle) > 45 || isnan(currentAngle)) {
        systemState = EMERGENCY_STOP;
        emergencyStop();
    } else if (systemState == EMERGENCY_STOP && abs(currentAngle) < 10) {
        systemState = RUNNING;
    }
    
    // 过热保护
    if (motorTemperature() > 80) {
        reducePower(0.5); // 降低50%功率
    }
}

void emergencyStop() {
    // 立即停止所有电机
    analogWrite(MOTOR_LEFT_PWM, 0);
    analogWrite(MOTOR_RIGHT_PWM, 0);
    Serial.println("EMERGENCY STOP ACTIVATED!");
}

要点解读

1. 速度前馈的核心价值
   原理: 速度前馈基于目标速度直接计算所需控制量，无需等待误差产生
   优势: 显著改善系统响应速度，减少滞后现象，特别是在快速变速场景
   实施关键: 准确建模系统动力学特性，合理选择前馈增益系数
2. 动态参数调整策略
   多层次调参: 根据运行状态(速度、加速度、负载)自动切换最佳参数组
   平滑过渡: 参数切换时应有渐变过程，避免突变引起系统不稳定
   实时监测: 持续评估系统性能指标，为参数调整提供依据
3. 平衡控制算法设计
   双环架构: 外环角度控制+内环速度控制的经典结构
   互补优势: PD控制保证稳定性，前馈控制提升响应性
   抗干扰能力: 引入角速度反馈增强系统对扰动的抑制能力
4. 安全机制重要性
   多重保护: 包括倾角限制、温度监控、电流检测等
   故障降级: 异常情况下自动进入安全模式而非完全停机
   状态指示: 清晰的系统状态反馈便于操作人员及时干预
5. 系统集成考量
   传感器融合: 编码器+IMU的数据融合提高测量精度
   实时性能: 确保控制周期稳定，避免因延迟导致性能下降
   调试工具: 完善的串口监控和参数调整接口方便现场优化

1、基础平衡控制（单传感器融合）

#include <Encoder.h>
#include <PID_v1.h>
#include <MPU6050.h> // 陀螺仪/加速度计库

Encoder leftWheel(2, 3), rightWheel(4, 5); // 左右轮编码器
MPU6050 mpu;
double targetAngle = 0; // 目标平衡角度（垂直时为0）
double currentAngle, wheelSpeed, motorOutput;

// PID控制器（姿态环）
double Kp = 3.0, Ki = 0.2, Kd = 0.5;
double inputAngle, outputPID;
PID pid(&inputAngle, &outputPID, &targetAngle, Kp, Ki, Kd, DIRECT);

void setup() {
  Serial.begin(115200);
  mpu.initialize();
  pid.SetMode(AUTOMATIC);
  pid.SetOutputLimits(-255, 255); // 电机PWM范围
}

void loop() {
  // 读取陀螺仪数据（角度和角速度）
  mpu.getRotationY(&currentAngle);
  inputAngle = currentAngle;
  
  // 计算车轮速度（左右轮平均）
  long leftPos = leftWheel.read();
  long rightPos = rightWheel.read();
  wheelSpeed = (leftPos + rightPos) * 0.5; // 简化处理
  
  // PID计算姿态控制
  pid.Compute();
  
  // 速度前馈补偿（加速时增加输出）
  motorOutput = outputPID + wheelSpeed * 0.1; // 经验系数
  
  // 控制电机（假设使用L298N驱动）
  analogWrite(9, constrain(motorOutput + 150, 0, 255)); // 偏移量防止反转
  analogWrite(10, constrain(motorOutput + 150, 0, 255));
  
  delay(10); // 10ms控制周期
}

应用场景：适用于初学者的平衡车实验平台，通过单传感器（MPU6050）实现基本平衡功能。

5、动态PID参数自整定（双环控制）

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

Encoder wheelEncoder(2, 3);
double targetSpeed = 0; // 目标速度
double currentSpeed, currentAngle;
double Kp_angle = 4.0, Ki_angle = 0.3, Kd_angle = 0.8;
double Kp_speed = 1.2, Ki_speed = 0.1, Kd_speed = 0.2;

PID anglePID(&currentAngle, &targetSpeed, &targetAngle, Kp_angle, Ki_angle, Kd_angle, DIRECT);
PID speedPID(&currentSpeed, &targetSpeed, &targetSpeed, Kp_speed, Ki_speed, Kd_speed, DIRECT);

void setup() {
  Serial.begin(115200);
  anglePID.SetMode(AUTOMATIC);
  speedPID.SetMode(AUTOMATIC);
  anglePID.SetOutputLimits(-255, 255);
  speedPID.SetOutputLimits(-100, 100); // 速度环输出范围
}

void loop() {
  // 动态调整PID参数（根据角度变化率）
  if (abs(currentAngle) > 10) { // 大角度时增强阻尼
    Kp_angle = 5.0; Kd_angle = 1.0;
  } else {
    Kp_angle = 4.0; Kd_angle = 0.8;
  }
  
  // 读取编码器和姿态数据
  currentSpeed = wheelEncoder.read() * 0.01; // 转换为m/s
  currentAngle = readGyroAngle(); // 需实现陀螺仪读取函数
  
  // 双环控制计算
  anglePID.Compute();
  speedPID.Compute();
  
  // 最终输出 = 姿态控制 + 速度控制
  int finalOutput = targetSpeed + speedPID.GetOutput();
  
  // 电机控制
  setMotorSpeed(finalOutput);
  
  delay(5); // 5ms控制周期
}

应用场景：适用于需要自适应调节的平衡车，如地形变化或负载变化时的自动参数调整。

6、轨迹跟踪与速度前馈（带路径规划）

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

Encoder leftEnc(2, 3), rightEnc(4, 5);
double targetPath[] = {0, 45, 90, 135, 180}; // 目标路径角度
int pathIndex = 0;
double feedforwardSpeed = 0; // 前馈速度项

// 位置环PID
double Kp_pos = 2.0, Ki_pos = 0.1, Kd_pos = 0.3;
PID posPID(&currentAngle, &targetAngle, &targetPath[pathIndex], Kp_pos, Ki_pos, Kd_pos, DIRECT);

void setup() {
  Serial.begin(115200);
  posPID.SetMode(AUTOMATIC);
  posPID.SetOutputLimits(-200, 200);
}

void loop() {
  // 路径跟踪逻辑
  if (abs(currentAngle - targetPath[pathIndex]) < 2) {
    pathIndex = (pathIndex + 1) % 5; // 循环路径
  }
  
  // 速度前馈计算（基于路径曲率）
  if (pathIndex % 2 == 0) {
    feedforwardSpeed = 50; // 直行时高速
  } else {
    feedforwardSpeed = 30; // 转弯时低速
  }
  
  // 读取编码器数据
  long leftCount = leftEnc.read();
  long rightCount = rightEnc.read();
  currentAngle = calculateAngle(leftCount, rightCount); // 需实现角度计算函数
  
  // PID计算 + 前馈补偿
  posPID.Compute();
  int motorOutput = posPID.GetOutput() + feedforwardSpeed;
  
  // 电机控制
  setMotorPower(motorOutput);
  
  delay(20); // 20ms控制周期
}

应用场景：适用于需要精确轨迹跟踪的平衡车，如仓库AGV或巡逻机器人，结合路径规划实现动态速度调整。

要点解读
多传感器融合与状态估计
平衡车需融合陀螺仪（角度/角速度）、加速度计（倾斜补偿）和编码器（速度/位置）数据，通过卡尔曼滤波或互补滤波提高姿态估计精度。
案例体现：案例一使用MPU6050实现单传感器融合，案例三通过编码器数据计算车身角度。
动态PID参数自整定
根据运行状态（如倾斜角度、速度）实时调整PID参数，例如大角度时增大Kp增强响应，小角度时减小Kp避免振荡。
案例体现：案例二通过条件判断动态调整Kp_angle和Kd_angle参数。
速度前馈控制的作用
在PID输出基础上叠加速度项，提前补偿系统惯性，减少相位滞后，提升动态响应性能。
案例体现：案例一和案例三均通过feedforwardSpeed或速度项实现前馈补偿。
控制周期与实时性优化
平衡车控制周期通常需≤20ms以保证实时性，过长的周期会导致系统延迟和不稳定。
案例体现：所有案例均通过delay()控制周期，案例三使用20ms周期适应轨迹跟踪需求。
安全与容错机制设计
需加入限位保护（如角度超过阈值时紧急停止）、过流检测、编码器故障检测等安全机制。
案例体现：案例一中通过constrain()限制电机输出范围，实际应用中需扩展硬件限位和异常处理逻辑。

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