文章目录
- [1. PX4飞控PID简介](#1. PX4飞控PID简介)
-
- [1.1 位置控制器](#1.1 位置控制器)
- [1.2 速度控制器](#1.2 速度控制器)
- [1.3 加速度和yaw转到姿态](#1.3 加速度和yaw转到姿态)
- [1.4 姿态控制器](#1.4 姿态控制器)
- [1.5 角速率控制器](#1.5 角速率控制器)
- [2. 线性二次型优化（LQR）控制](#2. 线性二次型优化（LQR）控制)
- [3. 模型预测控制MPC/NMPC](#3. 模型预测控制MPC/NMPC)
-
- [3.1 MPC](#3.1 MPC)
- [3.2 NMPC](#3.2 NMPC)
1. PX4飞控PID简介
相关链接：PX4官方中文文档、PID概念(不了解PID的可以看看这个或者翻自控书吧)
PX4使用了串级PID控制器，串级PID通过分而治之的策略，将复杂控制问题分解为多个简单子问题，在响应速度、抗干扰性、稳定性和工程实现上均优于单级PID。比如姿态环处理的IMU噪声不会影响到位置环，同时各环PID参数可独立调谐，内环保证局部稳定（如机体角速率收敛），外环在此基础上优化全局性能。避免单级PID因参数耦合导致的振荡或发散。
1.1 位置控制器
在位置外环中只使用了一个P控制器和一个限幅函数。使用当前的位置估计和期望位置相减得到误差，然后经过P控制器和限幅得到期望速度，输出到速度内环。
为什么外环只需要一个P控制环呢？
那是因为外环的核心任务是"设定目标"，而非快速跟踪。核心任务是将位置误差线性转化为速度指令，无需积分或微分，速度内环的PID会补偿外环的不足，避免振荡和噪声放大。若外环加入I或D控制，积分会导致超调或延迟，微分会放大噪声，反而破坏系统稳定性，因此简化外环、强化内环是最优策略。
1.2 速度控制器
速度内环控制器就熟悉一点，PID大套餐上齐了。首先估计速度直接进入LPF(低通滤波器，防止高频干扰微分结果)然后计算微分，进入D环节，这一部分是防止输出突变 以及提前控制防止震荡 ，其实也不难理解，就是对当前的估计做了微分反映了数据的运动趋势；然后估计速度与期望速度作差得到误差进入积分环节，这部分是**消除稳态误差**（如抵抗恒定风阻导致的速度偏差），其实就是识别到误差长时间没有归零就加大矫正力度；最后是误差进入P环节**快速响应速度误差**。
**为什么内环使用PID控制器？**速度内环用PID控制，因为它需要快速精准地跟踪速度指令，比例项响应误差，积分项消除静差，微分项抑制扰动，确保内环动态性能足够支撑外环。
**总结：**外环P控制像"指挥官"设定宏观目标，内环PID像"执行者"处理细节，二者分工明确，避免单级PID的复杂性和不稳定性。
1.3 加速度和yaw转到姿态
由于无人机的yaw和速度加速度在动力学上天然解耦，yaw角在规划器中是可以单独规划的，不过常见规划器还是按照轨迹的切向方向来的。图中的yaw角也是直接从规划器来的，而不是像pitch、roll需要从加速度中提取。
1.4 姿态控制器
多旋翼姿态控制器基于四元数（quaternion）设计，通过误差四元数计算期望角速率，最终输出饱和的角速率命令。
1.误差计算（通过四元数乘法实现）：
四元数乘法如下（实部虚部分开）：
这里分为两路，提取了实部的符号乘以虚部保证最短旋转路径（具体可以参考四元数的几何表示，q和-q表示一样的终端状态，但是旋转方向有大小区别，q可以保证旋转最短角度）：
2.P控制器构造
角速度 Ω 的本质是旋转偏差随时间的变化率。在控制系统中，我们希望角速度命令与姿态误差成比例关系，即**角速度的方向应对准旋转轴，大小应与旋转角度偏差成正比**。
观察四元数的几何表示：
可以看到w是影响旋转角度大小，虚部影响旋转轴方向还有旋转角度大小。当误差特别小时，可以使用小角度假设，此时的虚部等于1/2θn，那么根据前面的构造，可以进一步构造一个有物理意义的P系数即2P。
这个时候P的物理意义是姿态误差每增加一弧度，角速度增加P弧度每秒。最后再限幅保证安全。
这里为什么选择四元数而不选择欧拉角呢？
主要有下列几点原因：1.欧拉角有万向节死锁问题。2.四元数是线性计算，欧拉角是三角函数计算，更加复杂。3.通过四元数插值（如SLERP）可平滑过渡姿态，且误差四元数的实部符号能自动选择最短旋转路径。
这一环可以自动调参：自动调参，但是感觉对大轴距的飞机效果不好。
1.5 角速率控制器
K-PID：比例项（K）独立于积分（I）和微分（D）路径，直接作用于误差信号。无人机需要快速响应姿态误差（比例项K直接作用），同时通过积分消除稳态误差（如风力干扰），微分抑制振荡。
这一环可以自动调参：自动调参，但是感觉对大轴距的飞机效果不好。
计算完期望的体速率和推力之后，使用旋翼的模型计算每个电机的转速：
由电机转速得到力矩和推力公式：
对上式求逆得到每个电机转速：
2. 线性二次型优化（LQR）控制
无人机的底层控制算法中，LQR（Linear Quadratic Regulator，线性二次调节器）是一种基于状态空间模型的优化控制方法，广泛应用于姿态、位置等底层控制环节。LQR是一种最优控制算法，通过最小化一个"代价函数"来求解最优控制输入。其核心思想是：
- 线性系统：无人机动力学模型在平衡点附近可线性化（如悬停状态附近）。
- 二次代价函数：平衡状态误差和控制输入的能耗，实现"高性能+低能耗"的折中。
由于限制为一个线性系统，所以这个状态方程就不会太复杂，以无人机悬停为例。
无人机的状态方程（此时当做质点）：
设x为无人机的位置、速度，A为无人机的状态转移矩阵，B为输入矩阵（描述控制输入对状态的影响），u为无人机的输入，这里应该是加速度。
代价函数为：
最小化跟踪误差以及输入加速度（节省能量）。Q：增大对角元素会强制对应状态快速收敛。R：增大对角元素会限制控制输入的幅值。
这个目标函数是一个标准的二次型优化问题（可以类比于二次函数），是有解析解的。下面是推导过程：
1.先构建一个关于状态向量的值函数(其中P 是对称正定矩阵，需要求解)：
2.带入HJB方程（哈密顿-雅各比-贝尔曼方程）
这个方程表达了 从当前状态出发，若采取最优控制策略，所能达到的最小代价应满足某种微分方程。
再把值函数带入得到：
然后对这个HJB方程对输入u求导：
得到
再构造一个反馈增益矩阵K：
再将结果带入HJB方程得到：
最终求得
**总结：**LQR通过数学模型和优化理论，为无人机提供了一种高效、稳定的底层控制方案。其核心是状态反馈和代价函数最小化，适合需要精确建模和性能优化的场景。
3. 模型预测控制MPC/NMPC
MPC（Model Predictive Control，模型预测控制）是现代控制中非常重要的一个方法，特别适用于多变量系统、有约束系统或需要提前规划行为的场景，比如无人车路径跟踪、无人机避障、工业过程控制等。MPC 是一种滚动优化控制策略。它的基本思想是：
- 使用系统模型预测未来状态变化（例如未来几秒或几个采样周期）；
- 基于预测结果最小化一个代价函数（类似 LQR 的目标函数）；
- 求解一个有限时域的优化问题，得到一组未来控制序列；
- 只执行第一个控制输入，然后重新预测、重新优化，循环进行。
这就是 MPC 的"预测-优化-执行-滚动"机制。
如果系统方程是线性的就是MPC，如果非线性就是NMPC。
3.1 MPC
还是以无人机悬停为例，且暂时考虑一维模型，状态变量：
控制输入为加速度：
系统模型（离散）为：
代价函数为：
求解过程：
1.预测：
给定当前状态𝑥0，可以展开出整个时域的状态预测：
定义控制向量：
将整个状态预测写成矩阵形式：
2.优化求解
将预测状态代入代价函数：
代入状态展开：
展开为标准形式：
同时考虑一些必要的硬件约束，调用优化库进行求解得到数值解。
3.执行
执行求解后的u_0，即控制向量的第一个元素。
总流程
每个控制周期：
- 读取当前状态𝑥0建预测矩阵 𝑆𝑥,𝑆𝑢 构造代价函数和约束
- 解二次规划问题，得到控制向量 𝑢⋆
- 仅执行第一个控制输入 𝑢_0⋆
- 系统前进一步，滚动窗口，重复上述步骤
3.2 NMPC
仍然是无人机悬停为例，使用NMPC直接控制无人机的电机转速。
状态变量（12维）：
控制变量（4维）：
系统模型：
这个M就是1.5节的M矩阵。
代价函数：
然后再写出扩展过的系统模型，带入代价函数进行优化求解。
**总结：**LQR就是MPC无约束、无时域限制的特殊形式，而且由于形式简单，可以准确计算解析解，计算快、准。