一、PID 控制器的原理和算法
PID 控制原理
"PID（比例-积分-微分）控制是一种广泛应用于自动控制系统的技术，涉及到将误差反馈用于调整系统的输出，以达到期望的目标状态。"
PID 控制概述
- PID 控制是自动控制系统中常用的一种反馈控制策略。
- 它由三个部分组成：比例（P）、积分（I）和微分（D）。
开环控制
- 开环控制系统（Open Loop Control）指的是控制输出不依赖于系统的当前状态。
- 例子：假设有一个机器人需要沿着某个方向移动100米。
- 最简单的思路是让机器人一直前进，但这在遇到不平坦的路面或速度变化时可能会出问题。
闭环控制
- 闭环控制系统（Closed Loop Control）则是控制输出依赖于系统的当前状态。
- 例子：无人机的高度控制。
- 无人机通过传感器感知当前飞行高度，并根据目标高度进行调整。
PID 控制器的三个组成部分
比例控制（P）
- 比例控制通过计算当前误差（目标值与实际值之间的差距）来调整输出。
- 公式：
- KP 是比例增益，越大则响应速度越快，但可能导致震荡。
积分控制（I）
- 积分控制用于对长期存在的偏差进行修正。
- 功能：对误差进行累积，以提供更大的输出，从而消除稳态误差。
- 公式：
微分控制（D）
- 微分控制用于预测误差的变化速率，从而提前进行调整。
- 功能：减少系统的震荡。
- 公式：
| 参数 | 作用 | 不当后果 | 调参技巧 |
|---|---|---|---|
| Kp | 快速响应误差（如小车倾斜时强力扶正） | 振荡（过大）、响应迟钝（过小） | 从0增大至临界振荡点 |
| Ki | 消除静态误差（维持目标角度） | 积分饱和（翻车） | 为Kp的1/100~1/10 |
| Kd | 抑制超调（防止"点头"现象） | 高频噪声放大 | 用低通滤波处理微分项 |
代码示例
while True:
error = target_height - current_height
integral += error
derivative = error - previous_error
output = K_P * error + K_I * integral + K_D * derivative
apply_output(output)
previous_error = error二、MPU6050 传感器的原理和算法
MPU6050传感器结构
| 传感器类型 | 测量物理量 | 特性类比 | 缺陷类比 | 补偿方法 |
|---|---|---|---|---|
| 三轴加速度计 | 线加速度 | 像稳重老人：步伐慢但方向准 | 跑步时易晃：高频运动干扰大 | 低通滤波 + 动态加速度补偿 |
| 三轴陀螺仪 | 角速度 | 像灵活少年：反应快转向灵敏 | 蒙眼走路会偏：积分累积误差 | 零偏校准 + 温度补偿 |
姿态解算核心算法
A. 互补滤波
原理解析（双人合作类比）
想象两位盲人合作走路：
加速度计 = 稳重老人（低频准）：通过脚底压力感知坡度，但跑步时会误判
陀螺仪 = 敏捷少年（高频准）：通过步伐转向感知角度，但长距离会走偏
互补滤波让老人每走10步就纠正少年方向："少年负责短期路径，老人负责长期校准"
数学实现
c
// 权重分配：信任陀螺仪97%，信任加速度计3%
#define ALPHA 0.03
void ComplementaryFilter(float acc[3], float gyro[3], float dt) {
// 1. 加速度计计算姿态（低频稳定）
float acc_pitch = atan2(acc[Y], acc[Z]) * 180/PI;
// 2. 陀螺仪积分（高频响应）
gyro_pitch += gyro[X] * dt; // X轴角速度积分
// 3. 互补融合：陀螺仪为主，加速度计修正
pitch = (1 - ALPHA) * gyro_pitch + ALPHA * acc_pitch;
}B. 卡尔曼滤波
C. 四元数法
"四元数是一种用于表示三维空间旋转的数学工具，它可以克服欧拉角在旋转计算中出现的万向节锁问题。"
四元数的定义
四元数的优点
| 优点 | 说明 |
|---|---|
| 避免万向节锁 | 四元数不会出现万向节锁问题，能够实现平滑的旋转。 |
| 计算效率高 | 四元数运算相对简单，计算速度比矩阵运算更快。 |
| 存储空间小 | 四元数只需四个数值（一个实部和三个虚部），而旋转矩阵需要九个数值。 |
D.传感器误差补偿技术
1. 零偏校准（陀螺仪除漂）
类比：给指南针消磁
原理：静止时角速度应为零，实际输出即零偏
// 校准流程（需放置水平面）
for(int i=0; i<500; i++) {
gyro_offset_x += gyro_raw_x;
delay(10);
}
gyro_offset_x /= 500; // 得到X轴零偏2. 温度补偿（对抗热漂移）
类比：给金属尺子做热胀冷缩修正
原理：零偏与温度呈线性关系
// 读取MPU6050内置温度传感器
float temp = read_temp_sensor();
// 温度补偿公式：Offset_T = Offset_25℃ + K*(T-25)
gyro_x_calibrated = gyro_raw_x - (gyro_offset + 0.05*(temp - 25));3. 安装误差校正（坐标系对齐）
类比：给斜塔安装水平仪
原理：通过旋转矩阵修正机械偏差
// 安装倾斜补偿矩阵
const float R[3][3] = {
{0.998, 0.012, -0.005},
{-0.015, 0.987, 0.021},
{0.003, -0.018, 0.996}
};
// 坐标校正
void correct_installation(float vec[3]) {
float temp[3];
for(int i=0; i<3; i++) {
temp[i] = R[i][0]*vec[0] + R[i][1]*vec[1] + R[i][2]*vec[2];
}
memcpy(vec, temp, sizeof(temp));
}三、IMU 姿态解算的原理和算法
IMU姿态解算原理与算法深度解析
一、IMU姿态解算的本质问题
IMU（惯性测量单元）姿态解算的核心是解决三维空间中的航向参考问题：
输入：角速度（陀螺仪）、线加速度（加速度计）
输出：物体在空间中的三个姿态角（Roll横滚角, Pitch俯仰角, Yaw偏航角）
核心矛盾：陀螺仪的积分漂移 vs 加速度计的运动噪声
类比说明：想象你被蒙上眼睛坐在移动的汽车里：
陀螺仪 = 感受车辆转弯的离心力（短期准确，但方向感会逐渐迷失）
加速度计 = 感受座椅压力变化判断车身倾斜（长期可靠，但颠簸时会误判）
姿态解算 = 综合两种感觉判断车辆真实姿态
四、卡尔曼滤波（“预测未来” ）
一、卡尔曼滤波是什么？—— 给小白的类比解释
假设你要猜测一个房间的真实温度：
- 预测阶段：你根据昨天的温度和今天的天气，猜测今天温度是 25℃（这可能有误差，比如窗户开了）。
- 测量阶段：你用温度计测到 26℃，但温度计本身可能不准（比如误差 ±1℃）。
- 卡尔曼滤波：它会告诉你，真实温度更可能是 25.7℃—— 既参考了你的预测，也考虑了测量的结果，同时根据两者的可靠性自动调整权重。
核心思想：卡尔曼滤波是一种 “最优估计” 算法，它通过融合预测值和测量值，抵消噪声的影响，得到更接近真实值的结果。
二、卡尔曼滤波的核心原理：为什么能消除噪声？
1. 噪声的来源与特性
- 传感器噪声：比如 MPU6050 的加速度计和陀螺仪数据，每次测量都有随机误差（如电压波动、器件精度）。
- 系统噪声：比如无人机飞行时，风的干扰会导致实际运动与预测不符。
2. 卡尔曼滤波的 “魔法”：动态调整信任度
- 如果预测很准确（比如房间温度变化缓慢），它会更信任预测值；
- 如果测量很准确（比如高精度温度计），它会更信任测量值。
- 这种 “信任度” 由协方差矩阵表示，卡尔曼滤波会根据历史数据自动计算。
三、卡尔曼滤波的数学框架：五个公式
状态预测公式
误差协方差预测公式
卡尔曼增益公式
状态更新公式
误差协方差更新公式
五、欧拉角
一、欧拉角的本质定义
经典类比：想象一架飞机的运动：
偏航角(Yaw ψ)：飞机左右转向（绕垂直轴旋转）→ 改变前进方向
俯仰角(Pitch θ)：飞机爬升/俯冲（绕侧向轴旋转）→ 改变垂直角度
横滚角(Roll φ)：飞机倾斜转弯（绕纵轴旋转）→ 改变机身倾斜度
二、万向节死锁
现象：当俯仰角θ=±90°时，横滚和偏航轴重合，失去一个旋转自由度。
工程影响：
无人机俯冲90°时姿态失控
机械臂特定位置运动规划失效
FPS游戏角色抬头90°后左右晃动异常