时间：2026-03-16 06:09
人气：
作者：admin
基于 Arduino 平台实现 BLDC 机器人 IMU 角度读取 + 互补滤波 + PID 控制，构成了一个典型的姿态闭环控制系统。该架构是自平衡机器人（如两轮平衡车、倒立摆）或稳定云台的核心技术栈。它通过 互补滤波 融合 IMU 原始数据以获得精准姿态角，再利用 PID 控制器 计算出维持平衡所需的电机驱动力矩，驱动 BLDC 电机 执行动作。
1、主要特点
传感器融合：互补滤波（Complementary Filter）
这是系统的“感知中枢”，解决了单一传感器无法同时满足动态与静态精度需求的矛盾。
频域分割策略：互补滤波本质上是一个频域滤波器。它利用低通滤波（LPF）处理加速度计数据，提取低频的重力方向分量（长期稳定，用于修正漂移）；同时利用高通滤波（HPF）处理陀螺仪数据，提取高频的角速度变化分量（动态响应快，短期精度高）。
时域加权实现：在离散的嵌入式系统中，该过程通常简化为加权平均公式：Angle = α * (Angle + Gyro_Rate * dt) + (1-α) * Accel_Angle。其中 α 通常取值在 0.95~0.98 之间，决定了系统对陀螺仪的信任程度。
优势：相较于复杂的卡尔曼滤波，互补滤波计算量极小，仅需几次乘法和加法，非常适合 Arduino 这类资源受限平台，且参数调节直观。
核心算法：PID 控制器（Proportional-Integral-Derivative）
这是系统的“决策大脑”，负责将姿态误差转化为电机控制指令。
比例项（P）：提供与当前倾角误差成正比的恢复力。这是维持平衡的主力项，决定了系统的“刚度”。P 值过小会导致软瘫（无法站稳），过大则会引起高频振荡。
微分项（D）：提供与倾角变化率（即角速度）成正比的阻尼力。它能预测未来的倾角趋势并提前刹车，有效抑制系统的超调和振荡，使运动更加平滑。
积分项（I）：通常在此类平衡系统中设为 0 或极小值。因为平衡环是快速动态环，积分累积容易导致过冲。但在需要消除静差（如精确位置控制）时，需谨慎加入 I 项并配合抗饱和策略。
执行机构：BLDC 电机闭环驱动
这是系统的“肌肉”，负责将控制信号转化为物理动作。
快速响应特性：BLDC 电机相较于有刷电机，具有更高的功率密度和更快的动态响应速度，能够迅速跟随 PID 控制器输出的 PWM 指令，产生所需的恢复力矩。
驱动模式：通常工作在速度环或电流环（力矩环）模式。对于自平衡机器人，速度环更为常用，通过控制轮子的转速来抵消车身的倾角。
2、应用场景
两轮自平衡机器人（Segway 类）
这是最经典的倒立摆应用。系统通过 IMU 检测车身的俯仰角（Pitch），PID 控制器计算出左右轮子的目标转速，驱动 BLDC 电机转动以维持车身在垂直位置的动态平衡。
云台稳定系统（Gimbal）
用于相机或传感器的减震稳定。IMU 检测云台的姿态角，PID 控制器驱动 BLDC 电机反向旋转，以抵消载体（如无人机、手持杆）的晃动，从而保持画面或传感器的水平稳定。
倒立摆实验装置
在自动控制原理的教学实验中，该系统用于验证各种控制算法（如 PID、LQR）的有效性，直观展示开环不稳定系统如何通过闭环控制实现稳定。
双足/多足机器人基座平衡
对于腿部机器人，维持躯干的姿态稳定是行走的基础。该技术栈可用于控制机器人躯干的俯仰和横滚角，防止机器人在迈步时倾倒。
3、注意事项
硬件选型与抗干扰
IMU 选型与安装：推荐使用集成度高、稳定性好的 MPU6050 或 ICM-20689。IMU 模块必须刚性固定在机器人车身的重心附近，且尽量远离电机和大电流导线，防止振动和电磁干扰（EMI）引入噪声。
电源隔离：BLDC 电机启停时的大电流会拉低电源电压，导致 Arduino 复位或 IMU 数据异常。务必使用独立的稳压电路（如 LM2596 或隔离 DC-DC 模块）为传感器和逻辑电路供电，并在电源端并联大容量电容（如 1000μF）。
电机驱动：严禁使用普通航模 ESC（仅支持油门 PWM，无闭环能力）。必须使用支持闭环控制的驱动方案，如 SimpleFOC 库配合专用驱动板（如 B-G431B-ESC1），或带有编码器反馈的伺服驱动器。
算法实现细节
滤波器参数整定：互补滤波系数 α 需要根据采样频率 dt 调整。α 越大，系统响应越快但漂移越明显；α 越小，系统越稳但动态响应越迟钝。通常先通过公式 α = τ / (τ + dt) 进行估算（τ 为时间常数，通常取 0.1s）。
PID 参数整定：建议采用“由小到大”的试凑法。先将 I、D 设为 0，逐步增大 P 直到系统开始振荡，然后加入 D 项抑制振荡。对于平衡车，通常 D 项至关重要，用于提供阻尼。
采样频率与定时：控制回路的频率必须稳定且足够高（建议 ≥ 100Hz）。严禁在主循环中使用 delay() 函数阻塞程序。应使用硬件定时器中断（如 Timer1）来触发控制周期，确保 dt 的精确性。
机械结构设计
重心位置：对于自平衡机器人，重心越高，系统的不稳定性越强，对控制算法的响应速度要求也越高。建议在调试初期尽量降低重心（如将电池放低），待参数调好后再恢复。
传动刚性：电机与轮子之间的连接必须牢固，避免皮带过松或齿轮间隙过大。传动系统的弹性会导致相位滞后，容易引发 PID 控制的振荡。
1、两轮自平衡机器人（IMU角度读取 + PID控制）
#include <Wire.h>
#include <MPU6050.h>
#include <SimpleFOC.h>
MPU6050 mpu;
BLDCMotor motor(7);
Encoder encoder(2, 3); // 编码器引脚
// PID参数
float Kp = 40.0, Ki = 10.0, Kd = 0.5;
float targetAngle = 0.0; // 目标角度（垂直平衡点）
float previousError = 0, integral = 0;
// 互补滤波参数
float alpha = 0.98; // 加速度计权重
float dt = 0.01; // 采样时间(s)
float filteredAngle = 0;
void setup() {
Serial.begin(9600);
Wire.begin();
mpu.initialize();
mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
motor.init();
encoder.init();
motor.linkEncoder(&encoder);
}
void loop() {
// 1. 读取IMU数据
int16_t ax, ay, az, gx, gy, gz;
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
// 2. 互补滤波计算角度
float accelAngle = atan2(ay, az) * RAD_TO_DEG;
float gyroRate = gx / 131.0; // 转换为°/s
static float gyroAngle = 0;
gyroAngle += gyroRate * dt;
filteredAngle = alpha * (filteredAngle + gyroAngle * dt) + (1 - alpha) * accelAngle;
// 3. PID控制
float error = targetAngle - filteredAngle;
integral += error * dt;
float derivative = (error - previousError) / dt;
float output = Kp * error + Ki * integral + Kd * derivative;
previousError = error;
// 4. 电机控制（限制输出范围）
motor.move(constrain(output, -10, 10));
// 调试输出
Serial.print("Angle: "); Serial.print(filteredAngle);
Serial.print(" Output: "); Serial.println(output);
delay(dt * 1000); // 保持固定采样周期
}
2、四轴飞行器姿态控制（简化版）
#include <Wire.h>
#include <MPU6050.h>
#include <Servo.h>
MPU6050 mpu;
Servo motors[4]; // 4个电机
// PID参数（俯仰/滚转/偏航）
float Kp_pitch = 1.2, Ki_pitch = 0.05, Kd_pitch = 0.8;
float Kp_roll = 1.2, Ki_roll = 0.05, Kd_roll = 0.8;
float targetPitch = 0, targetRoll = 0;
// 互补滤波
float alpha = 0.95;
float pitchAngle = 0, rollAngle = 0;
void setup() {
Serial.begin(115200);
Wire.begin();
mpu.initialize();
// 初始化电机
for (int i = 0; i < 4; i++) {
motors[i].attach(5 + i); // 引脚5-8
motors[i].write(90); // 初始中立位置
}
}
void computeAngles() {
int16_t ax, ay, az, gx, gy, gz;
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
// 加速度计角度
float accelPitch = atan2(-ax, sqrt(ay * ay + az * az)) * RAD_TO_DEG;
float accelRoll = atan2(ay, az) * RAD_TO_DEG;
// 陀螺仪积分
static float gyroPitch = 0, gyroRoll = 0;
gyroPitch += gy / 131.0 * 0.01;
gyroRoll += gx / 131.0 * 0.01;
// 互补滤波
pitchAngle = alpha * (pitchAngle + gyroPitch) + (1 - alpha) * accelPitch;
rollAngle = alpha * (rollAngle + gyroRoll) + (1 - alpha) * accelRoll;
}
void loop() {
static unsigned long lastTime = 0;
unsigned long now = millis();
if (now - lastTime >= 10) { // 100Hz控制频率
lastTime = now;
// 1. 计算姿态角
computeAngles();
// 2. PID控制（简化版：仅俯仰和滚转）
static float iTerm_pitch = 0, iTerm_roll = 0;
float errorPitch = targetPitch - pitchAngle;
float errorRoll = targetRoll - rollAngle;
iTerm_pitch += errorPitch * 0.01;
iTerm_roll += errorRoll * 0.01;
float outputPitch = Kp_pitch * errorPitch + Ki_pitch * iTerm_pitch;
float outputRoll = Kp_roll * errorRoll + Ki_roll * iTerm_roll;
// 3. 电机混合（十字型布局）
int throttle = 1100; // 基础油门（PWM值）
int m1 = throttle + outputPitch + outputRoll;
int m2 = throttle - outputPitch + outputRoll;
int m3 = throttle - outputPitch - outputRoll;
int m4 = throttle + outputPitch - outputRoll;
// 4. 限制输出并写入电机
for (int i = 0; i < 4; i++) {
motors[i].write(constrain(map(i==0?m1:i==1?m2:i==2?m3:m4, 1000, 2000, 0, 180), 0, 180));
}
}
}
3、云台稳定系统（单轴）
#include <Wire.h>
#include <MPU6050.h>
#include <SimpleFOC.h>
MPU6050 mpu;
BLDCMotor motor(7);
Encoder encoder(2, 3);
// PID参数
float Kp = 2.0, Ki = 0.1, Kd = 0.05;
float targetAngle = 90.0; // 目标角度（度）
// 互补滤波
float alpha = 0.92;
float filteredAngle = 0;
void setup() {
Serial.begin(115200);
Wire.begin();
mpu.initialize();
motor.init();
encoder.init();
motor.linkEncoder(&encoder);
}
void loop() {
static unsigned long lastTime = 0;
unsigned long now = millis();
float dt = (now - lastTime) / 1000.0;
if (dt > 0.1) dt = 0.1; // 限制最大dt
// 1. 读取IMU
int16_t ax, ay, az, gx, gy, gz;
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
// 2. 互补滤波（假设绕X轴旋转）
float accelAngle = atan2(ay, az) * RAD_TO_DEG;
float gyroRate = gx / 131.0;
static float gyroAngle = 0;
gyroAngle += gyroRate * dt;
filteredAngle = alpha * (filteredAngle + gyroAngle) + (1 - alpha) * accelAngle;
// 3. PID控制
static float integral = 0;
float error = targetAngle - filteredAngle;
integral += error * dt;
float derivative = (error - (lastTime ? (targetAngle - (alpha * (filteredAngle + gyroAngle * dt) + (1 - alpha) * accelAngle)) : 0)) / dt;
float output = Kp * error + Ki * integral + Kd * derivative;
// 4. 电机控制
motor.move(output);
// 调试输出
if (now - lastTime >= 50) { // 20Hz打印
Serial.print("Target: "); Serial.print(targetAngle);
Serial.print(" Actual: "); Serial.print(filteredAngle);
Serial.print(" Output: "); Serial.println(output);
lastTime = now;
}
}
要点解读
IMU数据读取与处理
关键点：
加速度计数据需进行单位转换（如案例1的atan2(ay, az)计算角度）。
陀螺仪数据需根据量程设置转换（如/131.0对应±250°/s量程）。
优化建议：
添加校准程序消除零偏（如开机时采集500个样本取平均）。
使用DMP（数字运动处理器）硬件解算（如MPU6050的DMP模式）。
互补滤波实现
核心公式：
angle = α * (angle + gyro * dt) + (1-α) * accel_angle
参数选择：
α接近1时信任陀螺仪（动态响应快但易漂移）。
α接近0时信任加速度计（无漂移但噪声大）。
代码技巧：
使用静态变量保持状态（如案例1的gyroAngle）。
限制积分项增长（防止积分饱和）。
PID控制实现细节
常见问题：
微分项噪声过大 → 使用derivative = (error - previousError)/dt而非直接微分陀螺仪数据。
积分项漂移 → 添加积分限幅（如案例2的iTerm_pitch限制）。
调试技巧：
先调P参数，再调D，最后调I。
通过串口打印error和output观察响应曲线。
电机控制接口适配
不同电机类型处理：
有刷电机：直接PWM输出（如analogWrite）。
无刷电机：需FOC库（如案例1/3的SimpleFOC）。
舵机：使用Servo.h库（如案例2）。
安全限制：
必须使用constrain()限制输出范围。
紧急停止机制（如案例1的constrain(output, -10, 10)）。
实时性保障措施
固定采样周期：
使用millis()而非delay()实现定时控制（如所有案例）。
动态调整dt但限制最大值（如案例3的if(dt>0.1) dt=0.1）。
资源优化：
避免在循环中使用float除法（可预先计算倒数）。
对于低端MCU（如Arduino UNO），建议控制频率≤100Hz。
4、基础IMU平衡控制
场景：自平衡小车基础控制
核心逻辑：MPU6050 + 互补滤波 + 位置式PID
#include <SimpleFOC.h>
#include <MPU6050_tockn.h>
#include <Wire.h>
#include <PID_v1.h>
// IMU传感器
MPU6050 mpu6050(Wire);
#define IMU_SDA 21
#define IMU_SCL 22
// BLDC电机
BLDCMotor motor(7);
Encoder encoder(2, 3, 2048);
void doA() { encoder.handleA(); }
void doB() { encoder.handleB(); }
// 互补滤波参数
struct ComplementaryFilter {
float angle = 0.0; // 滤波后角度
float angleVelocity = 0.0; // 角速度
float alpha = 0.98; // 滤波系数
float dt = 0.01; // 采样时间
float accAngle = 0.0; // 加速度计角度
float gyroBias = 0.0; // 陀螺零偏
float lastGyroZ = 0.0; // 上次陀螺读数
};
ComplementaryFilter compFilter;
// PID控制器参数
struct PIDControl {
double input, output, setpoint;
double kp, ki, kd;
double integral = 0;
double lastError = 0;
double outputLimit = 12.0;
double integralLimit = 10.0;
bool antiWindup = true;
};
PIDControl balancePID = {0, 0, 0, 25.0, 0.5, 0.2};
// 校准数据
struct CalibrationData {
float accX_offset = 0, accY_offset = 0, accZ_offset = 0;
float gyroX_offset = 0, gyroY_offset = 0, gyroZ_offset = 0;
float temperature_offset = 0;
bool calibrated = false;
};
CalibrationData calib;
// 安全参数
struct SafetyParams {
float maxAngle = 0.5; // 最大允许角度 30°
float maxAngularVel = 3.0; // 最大角速度
float maxCurrent = 5.0; // 最大电流
int faultCount = 0;
int maxFaults = 10;
};
SafetyParams safety;
void setup() {
Serial.begin(115200);
Serial.println("===== IMU角度读取 + PID控制 + 互补滤波 =====");
Serial.println("基于MPU6050的自平衡控制系统");
// 1. 初始化I2C
Wire.begin(IMU_SDA, IMU_SCL);
delay(100);
// 2. 初始化IMU
if (!initIMU()) {
Serial.println("IMU初始化失败!");
while(1);
}
// 3. 校准IMU
calibrateIMU();
// 4. 初始化电机
initMotor();
// 5. 初始化滤波器
initFilter();
Serial.println("系统初始化完成");
Serial.println("等待3秒后开始平衡...");
delay(3000);
}
bool initIMU() {
Serial.println("初始化MPU6050...");
mpu6050.begin();
// 检查连接
uint8_t deviceID = mpu6050.readByte(MPU6050_ADDRESS, MPU6050_WHO_AM_I);
if (deviceID != 0x68) {
Serial.print("错误的设备ID: 0x");
Serial.println(deviceID, HEX);
return false;
}
// 配置IMU
mpu6050.writeByte(MPU6050_ADDRESS, MPU6050_PWR_MGMT_1, 0x00); // 唤醒
mpu6050.writeByte(MPU6050_ADDRESS, MPU6050_SMPLRT_DIV, 0x07); // 采样率1kHz
mpu6050.writeByte(MPU6050_ADDRESS, MPU6050_CONFIG, 0x00); // 禁用DLPF
mpu6050.writeByte(MPU6050_ADDRESS, MPU6050_GYRO_CONFIG, 0x18); // ±2000°/s
mpu6050.writeByte(MPU6050_ADDRESS, MPU6050_ACCEL_CONFIG, 0x10); // ±8g
Serial.println("MPU6050初始化成功");
return true;
}
void calibrateIMU() {
Serial.println("开始IMU校准...");
Serial.println("请保持设备静止10秒");
float accX_sum = 0, accY_sum = 0, accZ_sum = 0;
float gyroX_sum = 0, gyroY_sum = 0, gyroZ_sum = 0;
int samples = 1000;
for (int i = 0; i < samples; i++) {
// 读取原始数据
int16_t ax, ay, az, gx, gy, gz;
readRawIMU(ax, ay, az, gx, gy, gz);
accX_sum += ax;
accY_sum += ay;
accZ_sum += az;
gyroX_sum += gx;
gyroY_sum += gy;
gyroZ_sum += gz;
delay(10);
if (i % 100 == 0) Serial.print(".");
}
Serial.println();
// 计算偏移
calib.accX_offset = accX_sum / samples;
calib.accY_offset = accY_sum / samples;
calib.accZ_offset = accZ_sum / samples;
calib.gyroX_offset = gyroX_sum / samples;
calib.gyroY_offset = gyroY_sum / samples;
calib.gyroZ_offset = gyroZ_sum / samples;
// 计算重力加速度
float accZ_calib = (accZ_sum / samples - calib.accZ_offset);
if (abs(accZ_calib) > 100) { // 应该在1g附近
float scale = 16384.0 / accZ_calib; // 1g = 16384 LSB
calib.accX_offset *= scale;
calib.accY_offset *= scale;
}
Serial.println("校准完成");
Serial.print("加速度计偏移: ");
Serial.print(calib.accX_offset); Serial.print(", ");
Serial.print(calib.accY_offset); Serial.print(", ");
Serial.println(calib.accZ_offset);
Serial.print("陀螺仪偏移: ");
Serial.print(calib.gyroX_offset); Serial.print(", ");
Serial.print(calib.gyroY_offset); Serial.print(", ");
Serial.println(calib.gyroZ_offset);
calib.calibrated = true;
}
void readRawIMU(int16_t &ax, int16_t &ay, int16_t &az,
int16_t &gx, int16_t &gy, int16_t &gz) {
// 读取原始IMU数据
mpu6050.readBytes(MPU6050_ADDRESS, MPU6050_ACCEL_XOUT_H, 14);
ax = (mpu6050.buffer[0] << 8) | mpu6050.buffer[1];
ay = (mpu6050.buffer[2] << 8) | mpu6050.buffer[3];
az = (mpu6050.buffer[4] << 8) | mpu6050.buffer[5];
gx = (mpu6050.buffer[8] << 8) | mpu6050.buffer[9];
gy = (mpu6050.buffer[10] << 8) | mpu6050.buffer[11];
gz = (mpu6050.buffer[12] << 8) | mpu6050.buffer[13];
}
void loop() {
static unsigned long lastTime = 0;
unsigned long now = micros();
float dt = (now - lastTime) / 1000000.0;
if (dt >= 0.01) { // 100Hz控制
// 1. 读取并处理IMU数据
processIMUData(dt);
// 2. 互补滤波
complementaryFilter(dt);
// 3. PID控制计算
pidControl(dt);
// 4. 应用电机控制
applyMotorControl();
// 5. 安全检查
safetyCheck();
// 6. 执行FOC
motor.loopFOC();
lastTime = now;
}
// 状态显示
static unsigned long lastDisplay = 0;
if (millis() - lastDisplay >= 50) { // 20Hz显示
displayStatus();
lastDisplay = millis();
}
}
void processIMUData(float dt) {
// 读取并处理IMU数据
int16_t ax_raw, ay_raw, az_raw, gx_raw, gy_raw, gz_raw;
readRawIMU(ax_raw, ay_raw, az_raw, gx_raw, gy_raw, gz_raw);
// 应用校准
float ax = (ax_raw - calib.accX_offset) / 16384.0; // 转换为g
float ay = (ay_raw - calib.accY_offset) / 16384.0;
float az = (az_raw - calib.accZ_offset) / 16384.0;
float gx = (gx_raw - calib.gyroX_offset) / 131.0; // 转换为°/s
float gy = (gy_raw - calib.gyroY_offset) / 131.0;
float gz = (gz_raw - calib.gyroZ_offset) / 131.0;
// 转换为rad/s
gx *= PI / 180.0;
gy *= PI / 180.0;
gz *= PI / 180.0;
// 计算加速度计角度
float acc_mag = sqrt(ax*ax + ay*ay + az*az);
if (acc_mag > 0.5 && acc_mag < 1.5) { // 检查有效性
compFilter.accAngle = atan2(ay, az);
}
// 更新陀螺仪数据
compFilter.lastGyroZ = gz;
// 记录角速度
compFilter.angleVelocity = gz;
}
void complementaryFilter(float dt) {
// 互补滤波
if (!calib.calibrated) return;
// 陀螺仪积分
float gyroAngle = compFilter.angle + compFilter.lastGyroZ * dt;
// 互补滤波融合
compFilter.angle = compFilter.alpha * gyroAngle +
(1.0 - compFilter.alpha) * compFilter.accAngle;
// 更新采样时间
compFilter.dt = dt;
// 零漂补偿
static float driftIntegral = 0;
float driftError = compFilter.accAngle - compFilter.angle;
driftIntegral += driftError * dt;
driftIntegral = constrain(driftIntegral, -0.1, 0.1);
// 应用零漂补偿
compFilter.gyroBias = driftIntegral * 0.01;
compFilter.lastGyroZ -= compFilter.gyroBias;
}
void pidControl(float dt) {
// PID控制计算
// 设置点（垂直位置）
balancePID.setpoint = 0.0;
// 当前角度
balancePID.input = compFilter.angle;
// 计算误差
double error = balancePID.setpoint - balancePID.input;
// 比例项
double pTerm = balancePID.kp * error;
// 积分项
balancePID.integral += error * dt;
// 抗饱和
if (balancePID.antiWindup) {
if (balancePID.integral > balancePID.integralLimit) {
balancePID.integral = balancePID.integralLimit;
} else if (balancePID.integral < -balancePID.integralLimit) {
balancePID.integral = -balancePID.integralLimit;
}
}
double iTerm = balancePID.ki * balancePID.integral;
// 微分项
double dTerm = balancePID.kd * (error - balancePID.lastError) / dt;
balancePID.lastError = error;
// 计算输出
balancePID.output = pTerm + iTerm + dTerm;
// 输出限制
if (balancePID.output > balancePID.outputLimit) {
balancePID.output = balancePID.outputLimit;
} else if (balancePID.output < -balancePID.outputLimit) {
balancePID.output = -balancePID.outputLimit;
}
// 死区处理
if (abs(error) < 0.01 && abs(compFilter.angleVelocity) < 0.1) {
balancePID.output = 0;
}
}
void applyMotorControl() {
// 应用电机控制
if (safety.faultCount >= safety.maxFaults) {
// 故障状态，停止电机
motor.disable();
return;
}
// 启用电机
motor.enable();
// 应用PID输出
motor.move(balancePID.output);
// 记录控制量
static float lastOutput = 0;
float outputChange = abs(balancePID.output - lastOutput);
lastOutput = balancePID.output;
// 输出变化率限制
float maxChange = 10.0 * compFilter.dt;
if (outputChange > maxChange) {
balancePID.output = lastOutput + (balancePID.output > lastOutput ? maxChange : -maxChange);
}
}
void safetyCheck() {
// 安全检查
// 检查角度
if (abs(compFilter.angle) > safety.maxAngle) {
safety.faultCount++;
Serial.print("角度超限: ");
Serial.print(compFilter.angle * 180 / PI);
Serial.println("°");
if (safety.faultCount >= safety.maxFaults) {
emergencyStop();
}
}
// 检查角速度
if (abs(compFilter.angleVelocity) > safety.maxAngularVel) {
safety.faultCount++;
Serial.print("角速度超限: ");
Serial.print(compFilter.angleVelocity);
Serial.println("rad/s");
}
// 检查IMU数据有效性
static int imuErrorCount = 0;
int16_t ax, ay, az, gx, gy, gz;
readRawIMU(ax, ay, az, gx, gy, gz);
float accMag = sqrt(ax*ax + ay*ay + az*az) / 16384.0;
if (accMag < 0.8 || accMag > 1.2) { // 应该在1g附近
imuErrorCount++;
if (imuErrorCount > 5) {
safety.faultCount++;
Serial.print("加速度异常: ");
Serial.println(accMag);
}
} else {
imuErrorCount = 0;
}
// 自动恢复
static unsigned long lastFaultTime = 0;
if (safety.faultCount > 0 && safety.faultCount < safety.maxFaults) {
if (millis() - lastFaultTime > 1000) { // 1秒无新故障
safety.faultCount--;
lastFaultTime = millis();
}
}
}
void emergencyStop() {
// 紧急停止
Serial.println("紧急停止激活!");
motor.disable();
balancePID.integral = 0;
balancePID.lastError = 0;
safety.faultCount = 0;
// 等待恢复
delay(1000);
motor.enable();
}
void displayStatus() {
// 显示状态
Serial.print("角度: ");
Serial.print(compFilter.angle * 180 / PI, 1);
Serial.print("° 角速度: ");
Serial.print(compFilter.angleVelocity, 2);
Serial.print("rad/s 输出: ");
Serial.print(balancePID.output, 2);
Serial.print("V 故障: ");
Serial.println(safety.faultCount);
}
5、自适应互补滤波
场景：动态环境下的精确姿态估计
核心逻辑：自适应滤波系数 + 多传感器融合
#include <SimpleFOC.h>
#include <MPU6050_tockn.h>
#include <MadgwickAHRS.h>
// Madgwick滤波
Madgwick madgwick;
#define BETA_DEF 0.1f // 默认beta值
// 自适应滤波参数
struct AdaptiveFilter {
float alpha = 0.98f; // 基础滤波系数
float alpha_min = 0.90f; // 最小滤波系数
float alpha_max = 0.995f; // 最大滤波系数
float beta = BETA_DEF; // Madgwick beta
float innovation = 0.0f; // 新息
float innovation_threshold = 0.1f; // 新息阈值
// 状态
float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // 四元数
float roll = 0.0f, pitch = 0.0f, yaw = 0.0f; // 欧拉角
float angularVelocity[3] = {0, 0, 0}; // 角速度
};
AdaptiveFilter adaptiveFilter;
// 传感器方差估计
struct VarianceEstimator {
float acc_var[3] = {0.01f, 0.01f, 0.01f}; // 加速度方差
float gyro_var[3] = {0.001f, 0.001f, 0.001f}; // 陀螺方差
float mag_var[3] = {0.1f, 0.1f, 0.1f}; // 磁力计方差
// 在线估计
float acc_window[3][10] = {0};
float gyro_window[3][10] = {0};
int window_index = 0;
void update(float ax, float ay, float az, float gx, float gy, float gz) {
// 更新滑动窗口
acc_window[0][window_index] = ax;
acc_window[1][window_index] = ay;
acc_window[2][window_index] = az;
gyro_window[0][window_index] = gx;
gyro_window[1][window_index] = gy;
gyro_window[2][window_index] = gz;
window_index = (window_index + 1) % 10;
// 计算方差
for (int i = 0; i < 3; i++) {
float mean_acc = 0, mean_gyro = 0;
float var_acc = 0, var_gyro = 0;
for (int j = 0; j < 10; j++) {
mean_acc += acc_window[i][j];
mean_gyro += gyro_window[i][j];
}
mean_acc /= 10.0f;
mean_gyro /= 10.0f;
for (int j = 0; j < 10; j++) {
var_acc += (acc_window[i][j] - mean_acc) * (acc_window[i][j] - mean_acc);
var_gyro += (gyro_window[i][j] - mean_gyro) * (gyro_window[i][j] - mean_gyro);
}
acc_var[i] = var_acc / 9.0f;
gyro_var[i] = var_gyro / 9.0f;
}
}
};
VarianceEstimator varianceEst;
// 自适应PID控制器
class AdaptivePID {
private:
float kp, ki, kd;
float integral = 0;
float lastError = 0;
float lastOutput = 0;
// 自适应参数
float adaptive_kp = 1.0f;
float adaptive_ki = 1.0f;
float adaptive_kd = 1.0f;
float learning_rate = 0.01f;
// 性能指标
float error_history[10] = {0};
int error_index = 0;
float performance = 0;
public:
AdaptivePID(float p, float i, float d) : kp(p), ki(i), kd(d) {}
float calculate(float error, float dt, float angular_vel) {
// 更新误差历史
error_history[error_index] = error;
error_index = (error_index + 1) % 10;
// 自适应调整
adaptParameters(error, angular_vel, dt);
// 计算积分
integral += error * dt;
// 抗饱和
float integral_limit = 5.0f;
if (integral > integral_limit) integral = integral_limit;
if (integral < -integral_limit) integral = -integral_limit;
// 计算微分
float derivative = 0;
if (dt > 0) {
derivative = (error - lastError) / dt;
}
lastError = error;
// 计算输出
float output = (kp * adaptive_kp) * error +
(ki * adaptive_ki) * integral +
(kd * adaptive_kd) * derivative;
// 考虑角速度
output -= angular_vel * 0.5f;
// 输出限制
float output_limit = 12.0f;
if (output > output_limit) output = output_limit;
if (output < -output_limit) output = -output_limit;
// 输出变化率限制
float max_change = 10.0f * dt;
float change = output - lastOutput;
if (abs(change) > max_change) {
output = lastOutput + (change > 0 ? max_change : -max_change);
}
lastOutput = output;
return output;
}
void reset() {
integral = 0;
lastError = 0;
lastOutput = 0;
}
private:
void adaptParameters(float error, float angular_vel, float dt) {
// 计算误差统计
float mean_error = 0;
for (int i = 0; i < 10; i++) {
mean_error += error_history[i];
}
mean_error /= 10.0f;
float variance = 0;
for (int i = 0; i < 10; i++) {
float diff = error_history[i] - mean_error;
variance += diff * diff;
}
variance /= 10.0f;
// 基于误差统计调整
if (variance < 0.001f) { // 误差稳定
adaptive_kp *= 0.99f; // 减小增益
} else if (variance > 0.01f) { // 误差波动大
adaptive_kp *= 1.01f; // 增加增益
}
// 基于角速度调整微分
if (abs(angular_vel) > 2.0f) {
adaptive_kd = 1.2f; // 高速时增加微分
} else {
adaptive_kd = 1.0f;
}
// 限制范围
adaptive_kp = constrain(adaptive_kp, 0.5f, 2.0f);
adaptive_ki = constrain(adaptive_ki, 0.1f, 2.0f);
adaptive_kd = constrain(adaptive_kd, 0.1f, 2.0f);
}
};
AdaptivePID adaptiveBalancePID(25.0f, 0.5f, 0.2f);
// 自适应互补滤波
void adaptiveComplementaryFilter(float ax, float ay, float az,
float gx, float gy, float gz,
float dt) {
// 计算加速度置信度
float acc_mag = sqrt(ax*ax + ay*ay + az*az);
float acc_confidence = 1.0f;
if (abs(acc_mag - 1.0f) > 0.2f) { // 加速度异常
acc_confidence = 0.1f; // 降低置信度
}
// 计算动态滤波系数
float dynamic_alpha = adaptiveFilter.alpha;
if (acc_confidence < 0.5f) {
// 加速度不可靠，增加陀螺权重
dynamic_alpha = 0.995f;
} else {
// 正常情况
dynamic_alpha = 0.98f;
}
// 限制范围
dynamic_alpha = constrain(dynamic_alpha,
adaptiveFilter.alpha_min,
adaptiveFilter.alpha_max);
// 计算加速度计角度
float acc_roll = atan2(ay, az);
float acc_pitch = atan2(-ax, sqrt(ay*ay + az*az));
// 陀螺仪积分
float gyro_roll = adaptiveFilter.roll + gx * dt;
float gyro_pitch = adaptiveFilter.pitch + gy * dt;
float gyro_yaw = adaptiveFilter.yaw + gz * dt;
// 互补滤波
adaptiveFilter.roll = dynamic_alpha * gyro_roll +
(1.0f - dynamic_alpha) * acc_roll;
adaptiveFilter.pitch = dynamic_alpha * gyro_pitch +
(1.0f - dynamic_alpha) * acc_pitch;
// 航向角（没有磁力计，只积分）
adaptiveFilter.yaw = gyro_yaw;
// 更新角速度
adaptiveFilter.angularVelocity[0] = gx;
adaptiveFilter.angularVelocity[1] = gy;
adaptiveFilter.angularVelocity[2] = gz;
// 计算新息
float roll_innovation = acc_roll - adaptiveFilter.roll;
float pitch_innovation = acc_pitch - adaptiveFilter.pitch;
adaptiveFilter.innovation = sqrt(roll_innovation*roll_innovation +
pitch_innovation*pitch_innovation);
// 自适应beta
if (adaptiveFilter.innovation > adaptiveFilter.innovation_threshold) {
// 新息大，增加beta
adaptiveFilter.beta = min(BETA_DEF * 2.0f, 0.5f);
} else {
adaptiveFilter.beta = BETA_DEF;
}
// 更新方差估计
varianceEst.update(ax, ay, az, gx, gy, gz);
}
// Madgwick滤波实现
void madgwickFilterUpdate(float gx, float gy, float gz,
float ax, float ay, float az,
float mx, float my, float mz,
float dt) {
// 更新Madgwick滤波器
madgwick.beta = adaptiveFilter.beta;
madgwick.update(gx, gy, gz, ax, ay, az, mx, my, mz);
// 获取四元数
adaptiveFilter.q0 = madgwick.q0;
adaptiveFilter.q1 = madgwick.q1;
adaptiveFilter.q2 = madgwick.q2;
adaptiveFilter.q3 = madgwick.q3;
// 转换为欧拉角
quaternionToEuler(adaptiveFilter.q0, adaptiveFilter.q1,
adaptiveFilter.q2, adaptiveFilter.q3,
adaptiveFilter.roll, adaptiveFilter.pitch, adaptiveFilter.yaw);
}
void quaternionToEuler(float q0, float q1, float q2, float q3,
float &roll, float &pitch, float &yaw) {
// 四元数转欧拉角
roll = atan2(2.0f * (q0 * q1 + q2 * q3),
1.0f - 2.0f * (q1 * q1 + q2 * q2));
pitch = asin(2.0f * (q0 * q2 - q3 * q1));
yaw = atan2(2.0f * (q0 * q3 + q1 * q2),
1.0f - 2.0f * (q2 * q2 + q3 * q3));
}
// 传感器融合主循环
void sensorFusionLoop(float dt) {
// 读取原始数据
int16_t ax_raw, ay_raw, az_raw, gx_raw, gy_raw, gz_raw;
readRawIMU(ax_raw, ay_raw, az_raw, gx_raw, gy_raw, gz_raw);
// 校准和转换
float ax = (ax_raw - calib.accX_offset) / 16384.0f;
float ay = (ay_raw - calib.accY_offset) / 16384.0f;
float az = (az_raw - calib.accZ_offset) / 16384.0f;
float gx = (gx_raw - calib.gyroX_offset) * PI / (131.0f * 180.0f);
float gy = (gy_raw - calib.gyroY_offset) * PI / (131.0f * 180.0f);
float gz = (gz_raw - calib.gyroZ_offset) * PI / (131.0f * 180.0f);
// 选择滤波算法
static bool use_madgwick = false;
if (use_madgwick) {
// 使用Madgwick滤波
madgwickFilterUpdate(gx, gy, gz, ax, ay, az, 0, 0, 0, dt);
} else {
// 使用自适应互补滤波
adaptiveComplementaryFilter(ax, ay, az, gx, gy, gz, dt);
}
// 切换条件
static int stationary_count = 0;
float motion_level = sqrt(gx*gx + gy*gy + gz*gz);
if (motion_level < 0.1f) { // 静止
stationary_count++;
if (stationary_count > 50) { // 静止0.5秒
use_madgwick = true; // 静止时用Madgwick
}
} else {
stationary_count = 0;
use_madgwick = false; // 运动时用互补滤波
}
}
6、多传感器融合卡尔曼滤波
场景：高精度姿态估计，需要最优估计
核心逻辑：扩展卡尔曼滤波 + 多传感器数据融合
#include <SimpleFOC.h>
#include <BasicLinearAlgebra.h> // 线性代数库
#include <math.h>
// 使用BLA库
using namespace BLA;
// 扩展卡尔曼滤波器
class ExtendedKalmanFilter {
private:
// 状态: [q0, q1, q2, q3, gx_bias, gy_bias, gz_bias]
static const int STATE_DIM = 7;
static const int MEAS_DIM = 6; // 加速度计+磁力计
// 状态矩阵
Matrix<STATE_DIM, 1> x; // 状态向量
Matrix<STATE_DIM, STATE_DIM> P; // 误差协方差
Matrix<STATE_DIM, STATE_DIM> F; // 状态转移雅可比
Matrix<STATE_DIM, STATE_DIM> Q; // 过程噪声
Matrix<MEAS_DIM, STATE_DIM> H; // 测量雅可比
Matrix<MEAS_DIM, MEAS_DIM> R; // 测量噪声
// 四元数工具
float dt = 0.01f;
public:
ExtendedKalmanFilter() {
init();
}
void init() {
// 初始状态
x = {1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
// 初始协方差
P.Identity();
P *= 0.1f;
// 过程噪声
Q.Identity();
Q(0,0) = Q(1,1) = Q(2,2) = Q(3,3) = 1e-6f; // 四元数噪声
Q(4,4) = Q(5,5) = Q(6,6) = 1e-8f; // 零偏噪声
// 测量噪声
R.Identity();
R(0,0) = R(1,1) = R(2,2) = 0.1f; // 加速度噪声
R(3,3) = R(4,4) = R(5,5) = 0.5f; // 磁力计噪声
// 初始化雅可比矩阵
updateJacobians();
}
void update(float gx, float gy, float gz,
float ax, float ay, float az,
float mx, float my, float mz, float dt) {
this->dt = dt;
// 预测步骤
predict(gx, gy, gz);
// 更新步骤
updateMeasurement(ax, ay, az, mx, my, mz);
}
void getEulerAngles(float &roll, float &pitch, float &yaw) {
// 从四元数获取欧拉角
float q0 = x(0), q1 = x(1), q2 = x(2), q3 = x(3);
// 归一化
float norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 /= norm; q1 /= norm; q2 /= norm; q3 /= norm;
// 计算欧拉角
roll = atan2(2.0f * (q0 * q1 + q2 * q3),
1.0f - 2.0f * (q1 * q1 + q2 * q2));
pitch = asin(2.0f * (q0 * q2 - q3 * q1));
yaw = atan2(2.0f * (q0 * q3 + q1 * q2),
1.0f - 2.0f * (q2 * q2 + q3 * q3));
}
float getAngularVelocity(int axis) {
if (axis >= 0 && axis < 3) {
return x(4 + axis); // 返回估计的角速度
}
return 0.0f;
}
private:
void predict(float gx, float gy, float gz) {
// 状态预测
// 去除零偏的角速度
float wx = gx - x(4);
float wy = gy - x(5);
float wz = gz - x(6);
// 四元数导数
float q0 = x(0), q1 = x(1), q2 = x(2), q3 = x(3);
float q0_dot = 0.5f * (-q1 * wx - q2 * wy - q3 * wz);
float q1_dot = 0.5f * ( q0 * wx - q3 * wy + q2 * wz);
float q2_dot = 0.5f * ( q3 * wx + q0 * wy - q1 * wz);
float q3_dot = 0.5f * (-q2 * wx + q1 * wy + q0 * wz);
// 更新四元数
x(0) += q0_dot * dt;
x(1) += q1_dot * dt;
x(2) += q2_dot * dt;
x(3) += q3_dot * dt;
// 零偏保持不变
// 更新状态转移雅可比
updateJacobians();
// 协方差预测: P = F * P * F' + Q
P = F * P * ~F + Q;
}
void updateMeasurement(float ax, float ay, float az,
float mx, float my, float mz) {
// 测量更新
// 构建测量向量
Matrix<MEAS_DIM, 1> z = {ax, ay, az, mx, my, mz};
// 预测测量
Matrix<MEAS_DIM, 1> z_pred = measurementModel();
// 计算新息
Matrix<MEAS_DIM, 1> y = z - z_pred;
// 计算卡尔曼增益: K = P * H' * (H * P * H' + R)^-1
Matrix<MEAS_DIM, MEAS_DIM> S = H * P * ~H + R;
Matrix<STATE_DIM, MEAS_DIM> K = P * ~H * Inverse(S);
// 状态更新: x = x + K * y
x = x + K * y;
// 协方差更新: P = (I - K * H) * P
Matrix<STATE_DIM, STATE_DIM> I;
I.Identity();
P = (I - K * H) * P;
// 四元数归一化
normalizeQuaternion();
}
Matrix<MEAS_DIM, 1> measurementModel() {
// 测量模型
Matrix<MEAS_DIM, 1> z_pred;
float q0 = x(0), q1 = x(1), q2 = x(2), q3 = x(3);
// 加速度计测量模型 (重力方向)
z_pred(0) = 2.0f * (q1 * q3 - q0 * q2);
z_pred(1) = 2.0f * (q0 * q1 + q2 * q3);
z_pred(2) = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
// 磁力计测量模型 (简化)
z_pred(3) = 2.0f * (q0 * q3 + q1 * q2);
z_pred(4) = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3;
z_pred(5) = 2.0f * (q2 * q3 - q0 * q1);
return z_pred;
}
void updateJacobians() {
// 更新状态转移雅可比矩阵
F.Identity();
float q0 = x(0), q1 = x(1), q2 = x(2), q3 = x(3);
float wx = 0, wy = 0, wz = 0; // 简化处理
// 四元数部分
F(0,0) = 1.0f; F(0,1) = -wx*dt/2; F(0,2) = -wy*dt/2; F(0,3) = -wz*dt/2;
F(1,0) = wx*dt/2; F(1,1) = 1.0f; F(1,2) = wz*dt/2; F(1,3) = -wy*dt/2;
F(2,0) = wy*dt/2; F(2,1) = -wz*dt/2; F(2,2) = 1.0f; F(2,3) = wx*dt/2;
F(3,0) = wz*dt/2; F(3,1) = wy*dt/2; F(3,2) = -wx*dt/2; F(3,3) = 1.0f;
// 零偏部分
F(0,4) = q1*dt/2; F(0,5) = q2*dt/2; F(0,6) = q3*dt/2;
F(1,4) = -q0*dt/2; F(1,5) = q3*dt/2; F(1,6) = -q2*dt/2;
F(2,4) = -q3*dt/2; F(2,5) = -q0*dt/2; F(2,6) = q1*dt/2;
F(3,4) = q2*dt/2; F(3,5) = -q1*dt/2; F(3,6) = -q0*dt/2;
// 更新测量雅可比
updateMeasurementJacobian();
}
void updateMeasurementJacobian() {
// 更新测量雅可比矩阵
H.Fill(0.0f);
float q0 = x(0), q1 = x(1), q2 = x(2), q3 = x(3);
// 加速度计雅可比
H(0,0) = -2*q2; H(0,1) = 2*q3; H(0,2) = -2*q0; H(0,3) = 2*q1;
H(1,0) = 2*q1; H(1,1) = 2*q0; H(1,2) = 2*q3; H(1,3) = 2*q2;
H(2,0) = 2*q0; H(2,1) = -2*q1; H(2,2) = -2*q2; H(2,3) = 2*q3;
// 磁力计雅可比
H(3,0) = 2*q3; H(3,1) = 2*q2; H(3,2) = 2*q1; H(3,3) = 2*q0;
H(4,0) = 2*q0; H(4,1) = 2*q1; H(4,2) = -2*q2; H(4,3) = -2*q3;
H(5,0) = -2*q1; H(5,1) = 2*q0; H(5,2) = 2*q3; H(5,3) = -2*q2;
}
void normalizeQuaternion() {
// 四元数归一化
float q0 = x(0), q1 = x(1), q2 = x(2), q3 = x(3);
float norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
if (norm > 0.0001f) {
x(0) /= norm;
x(1) /= norm;
x(2) /= norm;
x(3) /= norm;
} else {
x(0) = 1.0f;
x(1) = x(2) = x(3) = 0.0f;
}
}
};
ExtendedKalmanFilter ekf;
// 级联PID控制器
class CascadePID {
private:
// 内环PID (角度)
struct InnerPID {
float kp, ki, kd;
float integral = 0;
float lastError = 0;
float output = 0;
} innerPID = {25.0f, 0.5f, 0.2f};
// 外环PID (角速度)
struct OuterPID {
float kp, ki, kd;
float integral = 0;
float lastError = 0;
float output = 0;
} outerPID = {2.0f, 0.1f, 0.05f};
// 前馈
float feedforward = 0.8f;
public:
float calculate(float targetAngle, float currentAngle,
float angularVelocity, float dt) {
// 外环：角度控制
float angleError = targetAngle - currentAngle;
outerPID.integral += angleError * dt;
outerPID.integral = constrain(outerPID.integral, -1.0f, 1.0f);
float angleDeriv = (angleError - outerPID.lastError) / dt;
outerPID.lastError = angleError;
// 外环输出 = 目标角速度
float targetAngularVel = outerPID.kp * angleError +
outerPID.ki * outerPID.integral +
outerPID.kd * angleDeriv;
// 内环：角速度控制
float velocityError = targetAngularVel - angularVelocity;
innerPID.integral += velocityError * dt;
innerPID.integral = constrain(innerPID.integral, -5.0f, 5.0f);
float velocityDeriv = (velocityError - innerPID.lastError) / dt;
innerPID.lastError = velocityError;
// 内环输出
innerPID.output = innerPID.kp * velocityError +
innerPID.ki * innerPID.integral +
innerPID.kd * velocityDeriv;
// 前馈补偿
innerPID.output += feedforward * targetAngularVel;
return innerPID.output;
}
void reset() {
innerPID.integral = 0;
innerPID.lastError = 0;
outerPID.integral = 0;
outerPID.lastError = 0;
}
};
CascadePID cascadeController;
// 多传感器融合控制
void multiSensorControlLoop(float dt) {
// 1. 读取所有传感器
float ax, ay, az, gx, gy, gz, mx, my, mz;
readAllSensors(ax, ay, az, gx, gy, gz, mx, my, mz);
// 2. 扩展卡尔曼滤波
ekf.update(gx, gy, gz, ax, ay, az, mx, my, mz, dt);
// 3. 获取估计状态
float roll, pitch, yaw;
ekf.getEulerAngles(roll, pitch, yaw);
float angularVelX = ekf.getAngularVelocity(0);
float angularVelY = ekf.getAngularVelocity(1);
float angularVelZ = ekf.getAngularVelocity(2);
// 4. 级联PID控制
float targetAngle = 0.0f; // 垂直位置
float currentAngle = pitch; // 俯仰角控制
float control = cascadeController.calculate(
targetAngle, currentAngle, angularVelY, dt
);
// 5. 应用控制
applyControl(control, dt);
// 6. 传感器有效性检查
checkSensorHealth(ax, ay, az, gx, gy, gz, mx, my, mz, dt);
}
// 传感器健康监测
class SensorHealthMonitor {
private:
struct SensorStats {
float mean[3] = {0};
float variance[3] = {0};
float health = 1.0f; // 健康度
int errorCount = 0;
};
SensorStats accStats, gyroStats, magStats;
// 滑动窗口
static const int WINDOW_SIZE = 20;
float accHistory[3][WINDOW_SIZE] = {0};
float gyroHistory[3][WINDOW_SIZE] = {0};
float magHistory[3][WINDOW_SIZE] = {0};
int historyIndex = 0;
public:
void update(float ax, float ay, float az,
float gx, float gy, float gz,
float mx, float my, float mz) {
// 更新历史数据
accHistory[0][historyIndex] = ax;
accHistory[1][historyIndex] = ay;
accHistory[2][historyIndex] = az;
gyroHistory[0][historyIndex] = gx;
gyroHistory[1][historyIndex] = gy;
gyroHistory[2][historyIndex] = gz;
magHistory[0][historyIndex] = mx;
magHistory[1][historyIndex] = my;
magHistory[2][historyIndex] = mz;
historyIndex = (historyIndex + 1) % WINDOW_SIZE;
// 计算统计
calculateStats(accStats, accHistory);
calculateStats(gyroStats, gyroHistory);
calculateStats(magStats, magHistory);
// 更新健康度
updateHealth();
}
float getHealth(int sensorType) {
switch(sensorType) {
case 0: return accStats.health;
case 1: return gyroStats.health;
case 2: return magStats.health;
default: return 0.0f;
}
}
private:
void calculateStats(SensorStats &stats, float history[3][WINDOW_SIZE]) {
// 计算均值和方差
for (int i = 0; i < 3; i++) {
float sum = 0, sumSq = 0;
for (int j = 0; j < WINDOW_SIZE; j++) {
sum += history[i][j];
sumSq += history[i][j] * history[i][j];
}
stats.mean[i] = sum / WINDOW_SIZE;
stats.variance[i] = (sumSq / WINDOW_SIZE) - (stats.mean[i] * stats.mean[i]);
}
}
void updateHealth() {
// 加速度计健康度
float accMag = sqrt(accStats.mean[0]*accStats.mean[0] +
accStats.mean[1]*accStats.mean[1] +
accStats.mean[2]*accStats.mean[2]);
if (abs(accMag - 1.0f) < 0.2f) {
accStats.health = 1.0f;
accStats.errorCount = 0;
} else {
accStats.errorCount++;
accStats.health = max(0.0f, 1.0f - accStats.errorCount * 0.1f);
}
// 陀螺仪健康度
float gyroVar = gyroStats.variance[0] + gyroStats.variance[1] + gyroStats.variance[2];
if (gyroVar < 0.1f) {
gyroStats.health = 1.0f;
} else {
gyroStats.health = 1.0f / (1.0f + gyroVar);
}
// 磁力计健康度
float magMag = sqrt(magStats.mean[0]*magStats.mean[0] +
magStats.mean[1]*magStats.mean[1] +
magStats.mean[2]*magStats.mean[2]);
if (magMag > 0.1f && magMag < 2.0f) {
magStats.health = 1.0f;
} else {
magStats.health = 0.5f;
}
}
};
要点解读
注意，以上案例只是为了拓展思路，仅供参考。它们可能有错误、不适用或者无法编译。您的硬件平台、使用场景和Arduino版本可能影响使用方法的选择。实际编程时，您要根据自己的硬件配置、使用场景和具体需求进行调整，并多次实际测试。您还要正确连接硬件，了解所用传感器和设备的规范和特性。涉及硬件操作的代码，您要在使用前确认引脚和电平等参数的正确性和安全性。
关注微信