3、线性化与状态空间表示：悬停点线性化、状态变量选取、输入输出定义
无人机动力学模型本质上是强非线性、强耦合的微分方程组。直接基于非线性模型设计LQR控制器，不仅推导复杂，而且难以获得解析的 Riccati 方程解。工程上最常用的做法是：在悬停工作点附近对非线性模型进行小扰动线性化，得到线性时不变（LTI）状态空间方程，再基于该线性模型设计LQR。本章将围绕“悬停点线性化—状态变量选取—输入输出定义”这一主线，讲清楚从非线性模型到可用的状态空间表达式的完整流程，并给出四旋翼无人机的具体推导结果和调参含义。
3.1 悬停点线性化的核心原理
四旋翼无人机的非线性动力学模型通常包含：牛顿-欧拉方程描述的刚体运动、陀螺力矩、气动阻力、电机-螺旋桨的推力与扭矩映射等。在悬停状态下，无人机处于一个平衡工作点：机体几乎静止，姿态角接近零，电机转速近似相等。此时，我们可以将非线性函数在该工作点附近进行一阶泰勒展开，忽略高阶小量，从而获得线性时不变模型。
设非线性系统的一般形式为：
ẋ = f(x, u)
y = g(x, u)在悬停工作点 (x₀, u₀) 处满足 f(x₀, u₀) = 0。定义小扰动变量：
Δx = x - x₀
Δu = u - u₀则线性化后的状态方程为：
Δẋ = A·Δx + B·Δu
Δy = C·Δx + D·Δu其中雅可比矩阵为：
A = ∂f/∂x |(x₀,u₀)
B = ∂f/∂u |(x₀,u₀)
C = ∂g/∂x |(x₀,u₀)
D = ∂g/∂u |(x₀,u₀)对于四旋翼无人机，悬停点的典型工作条件为：φ₀=θ₀=0，ψ₀=任意常值（通常取0），线速度u₀=v₀=w₀=0，角速度p₀=q₀=r₀=0，总推力U1₀=mg（m为质量，g为重力加速度）。
3.2 状态变量选取的原则与方法
状态变量的选取直接影响LQR控制器的设计难度和控制效果。对于四旋翼无人机轨迹跟踪问题，推荐采用12维状态向量，包含位置、姿态、线速度和角速度四个子集：
| 状态子集 | 符号 | 坐标系 | 物理含义 | 测量方式 |
|---|---|---|---|---|
| 位置 | x, y, z | 惯性系（NED） | 无人机在空间中的三维坐标 | GPS/视觉/激光雷达 |
| 姿态角 | φ, θ, ψ | 机体相对惯性系 | 滚转、俯仰、偏航角 | IMU（加速度计+陀螺仪） |
| 线速度 | u, v, w | 机体坐标系 | 沿机体轴的平移速度 | 空速计/视觉光流/状态估计 |
| 角速度 | p, q, r | 机体坐标系 | 绕机体轴的旋转角速度 | 陀螺仪 |
选取这12个状态变量的理由如下：
- 完整性：覆盖了无人机的位置、姿态、线速度和角速度，能够完整描述刚体运动状态。
- 可测性：所有状态均可通过IMU、GPS或状态估计器（如EKF）获得，满足LQR对状态反馈的需求。
- 可控性：四旋翼的4个输入（总推力+三轴力矩）能够独立控制这12个状态，系统满足可控性条件。
- 解耦性：在悬停点线性化后，系统可分解为水平位置环（x-y）、高度环（z）和姿态环（φ-θ-ψ），便于分环设计。
3.3 输入输出定义与系统分解
四旋翼无人机的控制输入通常定义为4个虚拟控制量：
U1 = F1 + F2 + F3 + F4 （总推力，控制高度）
U2 = (F2 - F4) · L （滚转力矩，控制φ）
U3 = (F3 - F1) · L （俯仰力矩，控制θ）
U4 = (F1 - F2 + F3 - F4) · C （偏航力矩，控制ψ）其中F1~F4为四个电机的推力，L为机臂长度，C为扭矩系数。输出变量根据控制目标定义：
- 轨迹跟踪模式：输出为位置x, y, z和偏航角ψ，即y = [x, y, z, ψ]ᵀ。
- 姿态控制模式：输出为姿态角φ, θ, ψ，即y = [φ, θ, ψ]ᵀ。
- 全状态反馈模式：输出为全部12个状态，即y = x。
在悬停点线性化后，系统可以分解为两个近似解耦的子系统：
| 子系统 | 状态变量 | 输入 | 输出 | 典型带宽 |
|---|---|---|---|---|
| 姿态子系统（内环） | φ, θ, ψ, p, q, r | U2, U3, U4 | φ, θ, ψ | 20~50 rad/s |
| 位置子系统（外环） | x, y, z, u, v, w | U1 + 姿态参考 | x, y, z | 2~5 rad/s |
这种分解的工程意义在于：内环（姿态环）响应速度快，外环（位置环）响应速度慢，两者带宽相差约10倍，可以独立设计LQR控制器，降低调参复杂度。
3.4 案例拆解：从非线性模型到LQR状态空间的完整推导
以某型四旋翼无人机为例，质量m=1.2kg，机臂长度L=0.25m，转动惯量Ixx=Iyy=0.015kg·m²，Izz=0.028kg·m²，重力加速度g=9.81m/s²。悬停时每个电机推力F=mg/4=2.943N。
步骤1：建立非线性模型
位置方程（惯性系）：
m·ẍ = (cosφ·sinθ·cosψ + sinφ·sinψ)·U1
m·ÿ = (cosφ·sinθ·sinψ - sinφ·cosψ)·U1
m·z̈ = (cosφ·cosθ)·U1 - m·g
姿态方程（机体系）：
Ixx·ṗ = (Iyy - Izz)·q·r + U2
Iyy·q̇ = (Izz - Ixx)·p·r + U3
Izz·ṙ = (Ixx - Iyy)·p·q + U4步骤2：在悬停点线性化
悬停点：φ₀=θ₀=ψ₀=0，u₀=v₀=w₀=0，p₀=q₀=r₀=0，U1₀=mg=11.772N，U2₀=U3₀=U4₀=0。
对位置方程求偏导，得到线性化位置方程：
ẍ = g·θ
ÿ = -g·φ
z̈ = (U1 - mg)/m = ΔU1/m对姿态方程求偏导，得到线性化姿态方程：
ṗ = U2/Ixx
q̇ = U3/Iyy
ṙ = U4/Izz步骤3：构建状态空间矩阵
选取状态向量 x = [x, y, z, φ, θ, ψ, u, v, w, p, q, r]ᵀ，输入 u = [ΔU1, U2, U3, U4]ᵀ，则状态矩阵A和输入矩阵B为：
A = [0 0 0 0 0 0 1 0 0 0 0 0;
0 0 0 0 0 0 0 1 0 0 0 0;
0 0 0 0 0 0 0 0 1 0 0 0;
0 0 0 0 0 0 0 0 0 1 0 0;
0 0 0 0 0 0 0 0 0 0 1 0;
0 0 0 0 0 0 0 0 0 0 0 1;
0 0 0 0 g 0 0 0 0 0 0 0;
0 0 0 -g 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0]
B = [0 0 0 0;
0 0 0 0;
1/m 0 0 0;
0 0 0 0;
0 0 0 0;
0 0 0 0;
0 0 0 0;
0 0 0 0;
0 0 0 0;
0 1/Ixx 0 0;
0 0 1/Iyy 0;
0 0 0 1/Izz]步骤4：验证可控性与可观测性
计算可控性矩阵的秩，rank([B AB A²B ... A¹¹B]) = 12，系统完全可控。输出矩阵C根据控制目标选取，若跟踪位置和偏航角，则C为4×12矩阵，选取x, y, z, ψ对应的行。
步骤5：LQR控制器设计
根据线性化模型，设计LQR增益K = R⁻¹BᵀP，其中P为Riccati方程的解。权重矩阵Q和R根据控制需求调整：
Q = diag([q1, q2, q3, q4, q5, q6, q7, q8, q9, q10, q11, q12])
R = diag([r1, r2, r3, r4])典型初始值：位置权重q1=q2=q3=10，姿态权重q4=q5=q6=5，速度权重q7=q8=q9=1，角速度权重q10=q11=q12=0.5，输入权重r1=r2=r3=r4=0.1。
3.5 常见误区与调参复盘
误区一：忽略欧拉角奇异点。当俯仰角θ接近±90°时，欧拉角表示存在奇异，线性化模型失效。工程上应限制飞行包线，或改用四元数表示姿态。
误区二：状态变量选取不当。有些初学者只选取6维状态（姿态+角速度），导致无法控制位置，无法实现轨迹跟踪。必须根据控制目标选择完整的状态向量。
误区三：输入输出定义混淆。将电机PWM值直接作为输入，而不是使用虚拟控制量U1~U4。这会导致系统耦合严重，LQR设计困难。
误区四：忽略执行器饱和。LQR计算出的控制量可能超出电机推力范围，导致实际控制效果与仿真差异大。需要在仿真中加入饱和环节，并在Q/R矩阵中考虑输入约束。
调参复盘案例：某次飞行测试中，无人机在悬停时出现低频振荡（约2Hz）。分析发现：位置环LQR的Q矩阵中位置权重过大（q1=q2=q3=100），导致位置环带宽过高，与姿态环带宽接近，引发耦合振荡。解决方案：将位置权重降低至10，姿态权重保持5，同时增大R矩阵中的输入权重（r1~r4从0.1增大到0.5），降低控制增益，最终振荡消除，悬停精度从±15cm提升至±5cm。