3.2 飞行控制算法

> "飞行控制算法是无人机的智慧大脑，让机器拥有了精确飞行的能力"

---

## 🎛️ PID控制器原理

### 📊 PID控制基础理论

PID控制器是无人机飞行控制系统的核心，通过比例（P）、积分（I）、微分（D）三个环节的协调工作，实现对飞行姿态的精确控制。

**图：PID控制系统框图**

![](/media/202507/PID控制框图_1753538197.png#size=500)

**PID控制方程**：
```latex
u(t) = K_p e(t) + K_i \int_0^t e(\tau) d\tau + K_d \frac{de(t)}{dt}
```

其中：
- u(t)：控制输出
- e(t)：误差信号（目标值-实际值）
- $K_p$：比例增益
- $K_i$：积分增益
- $K_d$：微分增益

| PID参数 | 作用机制 | 影响特性 | 调节效果 |
|---------|----------|----------|----------|
| 比例P | 当前误差响应 | 响应速度 | 减小稳态误差 |
| 积分I | 历史误差累积 | 稳态精度 | 消除稳态误差 |
| 微分D | 误差变化趋势 | 系统稳定性 | 减小超调量 |
| 组合PID | 综合控制效果 | 整体性能 | 最优控制响应 |

### 🔧 PID参数调节方法

无人机的PID参数调节需要根据具体的飞行特性和控制需求进行优化，常用的调节方法包括经验法、试验法和理论计算法。

**经典调节步骤**：

1. **初始设置**：
   - K_p = 0.5-2.0（根据系统特性）
   - K_i = 0（先不加积分）
   - K_d = 0（先不加微分）

2. **比例调节**：
   - 逐步增加K_p直到系统出现轻微振荡
   - 然后适当减小K_p值

3. **积分调节**：
   - 缓慢增加K_i消除稳态误差
   - 注意避免积分饱和现象

4. **微分调节**：
   - 适当增加K_d改善系统稳定性
   - 过大会导致系统对噪声敏感

| 调节方法 | 适用场景 | 优点 | 缺点 |
|----------|----------|------|------|
| 经验法 | 常见系统 | 快速简便 | 精度有限 |
| 试验法 | 实际系统 | 效果直观 | 耗时较长 |
| 理论法 | 已知模型 | 精度较高 | 需要数学模型 |
| 自适应法 | 变化系统 | 自动优化 | 算法复杂 |

### 🎯 多轴PID控制实现

多旋翼无人机需要对滚转、俯仰、偏航三个轴向分别进行PID控制，实现稳定的姿态保持。

**三轴控制结构**：
```
滚转轴（Roll）：  PID_roll  → 电机1,3差速
俯仰轴（Pitch）： PID_pitch → 电机2,4差速  
偏航轴（Yaw）：   PID_yaw   → 对角电机差速
高度轴（Alt）：   PID_alt   → 四电机同步
```

**控制输出分配**：
```latex
\begin{cases}
M_1 = \text{基础推力} + \text{Roll} - \text{Pitch} + \text{Yaw} \\
M_2 = \text{基础推力} - \text{Roll} - \text{Pitch} - \text{Yaw} \\
M_3 = \text{基础推力} - \text{Roll} + \text{Pitch} + \text{Yaw} \\
M_4 = \text{基础推力} + \text{Roll} + \text{Pitch} - \text{Yaw}
\end{cases}
```

> "PID控制就像一个经验丰富的飞行员，能够根据当前状态、历史经验和未来趋势做出最佳决策"

---

## 🧭 姿态解算与卡尔曼滤波

### 📐 姿态角度表示方法

无人机的姿态通常用欧拉角（滚转角、俯仰角、偏航角）来描述，这是最直观的姿态表示方法。

| 姿态角 | 英文名称 | 旋转轴 | 角度范围 | 物理意义 |
|--------|----------|--------|----------|----------|
| 滚转角φ | Roll | X轴 | ±180° | 左右倾斜 |
| 俯仰角θ | Pitch | Y轴 | ±90° | 前后倾斜 |
| 偏航角ψ | Yaw | Z轴 | ±180° | 左右转向 |

**图：欧拉角定义**
![](/media/202507/欧拉角_1753538555.jpg#size=350)

**旋转矩阵表示**：
```latex
R = R_z(\psi) R_y(\theta) R_x(\phi)
```

```latex
R = \begin{bmatrix}
c\psi c\theta & c\psi s\theta s\phi - s\psi c\phi & c\psi s\theta c\phi + s\psi s\phi \\
s\psi c\theta & s\psi s\theta s\phi + c\psi c\phi & s\psi s\theta c\phi - c\psi s\phi \\
-s\theta & c\theta s\phi & c\theta c\phi
\end{bmatrix}
```

其中：c = cos，s = sin

### 🔄 传感器数据融合

无人机通过多种传感器获取姿态信息，需要通过数据融合算法得到准确可靠的姿态角度。

| 传感器类型 | 测量参数 | 优点 | 缺点 | 应用场景 |
|------------|----------|------|------|----------|
| 陀螺仪 | 角速度 | 响应快、无漂移 | 积分误差累积 | 短期姿态测量 |
| 加速度计 | 重力方向 | 长期稳定 | 受加速度干扰 | 水平姿态参考 |
| 磁力计 | 磁北方向 | 绝对方位 | 受磁干扰 | 偏航角参考 |
| GPS | 位置速度 | 绝对定位 | 更新频率低 | 位置保持 |

**互补滤波算法**：
```latex
\theta_{融合} = \alpha \cdot (\theta_{陀螺仪} + \omega \cdot dt) + (1-\alpha) \cdot \theta_{加速度计}
```

其中：
- α：滤波系数（通常取0.95-0.99）
- ω：陀螺仪角速度
- dt：采样时间间隔

### 🎯 卡尔曼滤波原理

卡尔曼滤波是一种最优估计算法，能够在有噪声的环境中准确估计系统状态，是现代无人机姿态解算的核心算法。

**卡尔曼滤波流程**：

1. **预测步骤**：
   ```latex
   \hat{x}_{k|k-1} = F_k \hat{x}_{k-1|k-1} + B_k u_k
   ```
   ```latex
   P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k
   ```

2. **更新步骤**：
   ```latex
   K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R_k)^{-1}
   ```
   ```latex
   \hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k (z_k - H_k \hat{x}_{k|k-1})
   ```
   ```latex
   P_{k|k} = (I - K_k H_k) P_{k|k-1}
   ```

**参数说明**：
- x̂：状态估计值
- P：误差协方差矩阵
- F：状态转移矩阵
- H：观测矩阵
- Q：过程噪声协方差
- R：观测噪声协方差
- K：卡尔曼增益

**图：卡尔曼滤波工作流程**
![](/media/202507/卡尔曼滤波_1753584001.png#size=500)

| 滤波算法 | 计算复杂度 | 精度 | 实时性 | 适用场景 |
|----------|------------|------|--------|----------|
| 互补滤波 | 低 | 中等 | 优秀 | 简单应用 |
| 卡尔曼滤波 | 中等 | 高 | 良好 | 精确控制 |
| 扩展卡尔曼 | 高 | 很高 | 一般 | 复杂系统 |
| 粒子滤波 | 很高 | 很高 | 较差 | 研究应用 |

> "卡尔曼滤波就像一位智慧的法官，能够在众多证据中找出最可信的真相"

---

## 🎯 自动悬停与位置保持

### ⚖️ 悬停控制原理

自动悬停是多旋翼无人机的基本功能，需要同时控制位置、高度和姿态，实现在空中的稳定悬停。

**悬停控制层次结构**：
```
位置控制层 → 姿态控制层 → 角速度控制层 → 电机输出层
     ↓            ↓            ↓            ↓
   位置PID      姿态PID      角速度PID     PWM信号
```

| 控制层次 | 输入信号 | 输出信号 | 控制周期 | 主要功能 |
|----------|----------|----------|----------|----------|
| 位置控制 | 目标位置 | 目标姿态角 | 50-100Hz | 位置保持 |
| 姿态控制 | 目标姿态 | 目标角速度 | 200-500Hz | 姿态稳定 |
| 角速度控制 | 目标角速度 | 电机推力 | 500-1000Hz | 快速响应 |
| 电机控制 | 推力指令 | PWM信号 | 1000-2000Hz | 执行输出 |

### 📍 GPS位置保持算法

GPS位置保持通过比较当前位置与目标位置的差异，生成相应的控制指令来维持无人机在指定位置悬停。

**位置控制算法**：

1. **位置误差计算**：
   ```latex
   \begin{cases}
   e_x = x_{target} - x_{current} \\
   e_y = y_{target} - y_{current} \\
   e_z = z_{target} - z_{current}
   \end{cases}
   ```

2. **PID位置控制**：
   ```latex
   \begin{cases}
   \theta_{target} = K_{p\_pos} \cdot e_y + K_{i\_pos} \int e_y dt + K_{d\_pos} \frac{de_y}{dt} \\
   \phi_{target} = K_{p\_pos} \cdot e_x + K_{i\_pos} \int e_x dt + K_{d\_pos} \frac{de_x}{dt} \\
   T_{target} = K_{p\_alt} \cdot e_z + K_{i\_alt} \int e_z dt + K_{d\_alt} \frac{de_z}{dt}
   \end{cases}
   ```

### 🌊 气压高度保持

气压传感器通过测量大气压力变化来估算高度，是实现高度保持的重要传感器。

**气压高度计算**：
```latex
h = \frac{T_0}{L} \left[ \left(\frac{P_0}{P}\right)^{\frac{RL}{gM}} - 1 \right]
```

其中：
- h：高度（m）
- T₀：海平面标准温度（288.15K）
- L：温度递减率（0.0065 K/m）
- P₀：海平面标准气压（101325 Pa）
- P：当前气压（Pa）
- R：气体常数（8.314 J/(mol·K)）
- g：重力加速度（9.8 m/s²）
- M：空气摩尔质量（0.029 kg/mol）

| 高度传感器 | 精度 | 响应速度 | 工作范围 | 应用特点 |
|------------|------|----------|----------|----------|
| 气压计 | ±1m | 快 | 0-10000m | 相对高度测量 |
| 超声波 | ±0.1m | 很快 | 0-5m | 近地面精确测高 |
| 激光测距 | ±0.01m | 很快 | 0-100m | 高精度测高 |
| GPS高度 | ±3m | 慢 | 全球 | 绝对高度参考 |

### 🎮 光流定位技术

光流传感器通过分析地面图像的变化来估算无人机的水平移动，在GPS信号较弱的环境下提供位置保持能力。

**光流速度计算**：
```latex
\begin{cases}
v_x = \frac{\text{光流}_x \cdot h}{f \cdot dt} \\
v_y = \frac{\text{光流}_y \cdot h}{f \cdot dt}
\end{cases}
```

其中：
- vₓ, vᵧ：水平速度分量
- 光流ₓ,ᵧ：光流像素位移
- h：离地高度
- f：传感器焦距
- dt：时间间隔

**图：光流定位**
![](/media/202507/光流定位_1753584838.png#size=450)

| 定位技术 | 精度 | 环境要求 | 成本 | 适用场景 |
|----------|------|----------|------|----------|
| GPS定位 | 3-5m | 开阔天空 | 低 | 户外飞行 |
| 光流定位 | 0.1-1m | 有纹理地面 | 中等 | 室内飞行 |
| 视觉SLAM | 0.01-0.1m | 丰富特征 | 高 | 精确导航 |
| UWB定位 | 0.1-0.3m | 基站部署 | 高 | 室内精确定位 |

### 🔄 多传感器融合定位

现代无人机通常采用多传感器融合的方式来提高定位精度和可靠性，综合利用各种传感器的优势。

**融合定位架构**：
```
GPS → ┐
光流 → ├→ 卡尔曼滤波 → 融合位置估计 → 位置控制器
惯导 → ┘
```

**权重分配策略**：
- **GPS可用时**：GPS权重70%，光流20%，惯导10%
- **GPS失效时**：光流权重60%，惯导40%
- **室内环境**：光流权重80%，惯导20%

> "自动悬停就像让无人机学会了'定海神针'的本领，无论外界如何变化都能稳如泰山"

---

## 🚀 控制算法优化与调试

### 🔧 参数调试方法

飞行控制算法的参数调试是一个系统性工程，需要遵循科学的调试流程和方法。

| 调试阶段 | 主要任务 | 调试重点 | 安全措施 |
|----------|----------|----------|----------|
| 地面测试 | 基础功能验证 | 传感器校准 | 螺旋桨拆除 |
| 悬停测试 | 姿态控制调试 | PID参数优化 | 保护绳约束 |
| 低速飞行 | 位置控制验证 | 定位精度测试 | 空旷场地 |
| 正常飞行 | 综合性能测试 | 整体优化 | 应急预案 |

**调试工具与方法**：
- **实时监控**：地面站软件显示飞行数据
- **数据记录**：飞行日志分析
- **仿真验证**：参数预调试
- **渐进调试**：逐步增加复杂度

### 📊 性能评估指标

控制算法的性能需要通过定量指标来评估，确保满足飞行安全和任务要求。

| 性能指标 | 评估标准 | 测试方法 | 目标值 |
|----------|----------|----------|--------|
| 姿态精度 | 角度误差 | 悬停测试 | ±2° |
| 位置精度 | 位置偏差 | 定点悬停 | ±0.5m |
| 响应速度 | 调节时间 | 阶跃响应 | <2s |
| 稳定性 | 振荡幅度 | 扰动恢复 | <5% |

通过系统的算法设计、参数调试和性能评估，无人机能够实现稳定可靠的自主飞行，为各种应用任务提供坚实的技术基础。

---

## 🤔 思考与讨论

1. PID控制器的三个参数（P、I、D）各自起什么作用？在实际调试中应该按什么顺序进行调节？
2. 为什么需要多传感器融合来获取准确的姿态信息？单一传感器存在哪些局限性？
3. 卡尔曼滤波相比简单的互补滤波有什么优势？在什么情况下值得使用更复杂的算法？
4. 自动悬停功能需要哪些传感器的配合？GPS失效时还有哪些备用定位方案？
5. 控制系统的层次结构为什么要分为位置、姿态、角速度等多个层次？这样设计有什么好处？
6. 在无人机控制算法中，如何平衡响应速度和系统稳定性？过快的响应可能带来什么问题？
7. 随着人工智能技术的发展，传统的PID控制是否会被机器学习算法完全替代？

---

<center>
<a href="../../doc/16">
<button style="background: linear-gradient(135deg, #6e8efb, #a777e3); border: none; color: white; padding: 15px 32px; text-align: center; text-decoration: none; display: inline-block; font-size: 16px; margin: 4px 2px; cursor: pointer; border-radius: 12px; box-shadow: 0 4px 8px rgba(0,0,0,0.1);">🎮 继续探索第二部分：无人机操作 🎮</button>
</a>
</center>