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，即控制向量的第一个元素。

总流程

每个控制周期：

1. 读取当前状态𝑥0建预测矩阵 𝑆𝑥,𝑆𝑢 构造代价函数和约束
2. 解二次规划问题，得到控制向量 𝑢⋆
3. 仅执行第一个控制输入 𝑢_0⋆
4. 系统前进一步，滚动窗口，重复上述步骤

3.2 NMPC

仍然是无人机悬停为例，使用NMPC直接控制无人机的电机转速。

状态变量（12维）：

控制变量（4维）：

系统模型：

这个M就是1.5节的M矩阵。

代价函数：

然后再写出扩展过的系统模型，带入代价函数进行优化求解。

**总结：**LQR就是MPC无约束、无时域限制的特殊形式，而且由于形式简单，可以准确计算解析解，计算快、准。