当前位置：首页 > news >正文

无人机关键算法分析（ MPU6050 ,PID,滤波，四元数，欧拉角,IMU 姿态解算）

news 来源：原创 2025/6/27 9:42:33

一、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_heightintegral += errorderivative = error - previous_erroroutput = K_P * error + K_I * integral + K_D * derivativeapply_output(output)previous_error = error

二、MPU6050 传感器的原理和算法

MPU6050传感器结构

传感器类型	测量物理量	特性类比	缺陷类比	补偿方法
三轴加速度计	线加速度	像稳重老人：步伐慢但方向准	跑步时易晃：高频运动干扰大	低通滤波 + 动态加速度补偿
三轴陀螺仪	角速度	像灵活少年：反应快转向灵敏	蒙眼走路会偏：积分累积误差	零偏校准 + 温度补偿

姿态解算核心算法

A. 互补滤波

原理解析（双人合作类比）

想象两位盲人合作走路：

加速度计 = 稳重老人（低频准）：通过脚底压力感知坡度，但跑步时会误判
陀螺仪 = 敏捷少年（高频准）：通过步伐转向感知角度，但长距离会走偏
互补滤波让老人每走10步就纠正少年方向："少年负责短期路径，老人负责长期校准"

数学实现
c
// 权重分配：信任陀螺仪97%，信任加速度计3%
#define ALPHA 0.03void 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));
}