无人机动力学基础：四旋翼运动方程、坐标系变换与状态空间模型建立

四旋翼无人机是一个典型的欠驱动、强耦合、非线性系统。在设计和调试LQR（线性二次型调节器）控制器之前，必须首先建立能够准确描述其物理行为的数学模型。本章将系统梳理四旋翼的动力学基础，从坐标系定义与变换入手，推导刚体运动方程，并最终建立适用于LQR控制器设计的线性状态空间模型。这一过程是后续所有调参工作的理论基石——如果模型本身存在偏差，无论LQR权重矩阵如何调整，都无法获得理想的轨迹跟踪效果。

2.1 坐标系定义与变换：从地球系到机体系

四旋翼动力学建模首先需要明确两个核心坐标系：地球固连坐标系（惯性系E）和机体坐标系（体轴系B）。惯性系E以地面某点为原点，x轴指向北（或东），z轴垂直向下（或向上），用于描述无人机在空间中的位置和姿态。机体坐标系B则固定在无人机机体上，原点位于质心，x轴指向机头方向，y轴指向右翼，z轴垂直机体向下（或向上）。两个坐标系之间的转换关系是建立运动方程的基础。

姿态描述通常采用欧拉角（φ, θ, ψ），分别对应滚转（Roll）、俯仰（Pitch）和偏航（Yaw）。从惯性系到机体系的旋转矩阵R(φ,θ,ψ)由三个基本旋转矩阵按Z-Y-X顺序复合而成：

R = Rz(ψ) · Ry(θ) · Rx(φ)

其中：
Rx(φ) = [1, 0, 0; 0, cosφ, -sinφ; 0, sinφ, cosφ]
Ry(θ) = [cosθ, 0, sinθ; 0, 1, 0; -sinθ, 0, cosθ]
Rz(ψ) = [cosψ, -sinψ, 0; sinψ, cosψ, 0; 0, 0, 1]

在实际工程中，当俯仰角接近±90°时欧拉角会出现万向节锁问题，因此许多飞控系统（如PX4、ArduPilot）在姿态解算内部使用四元数，仅在输出层转换为欧拉角。但在LQR控制器设计阶段，由于工作点通常设定在悬停附近（φ≈0, θ≈0, ψ≈0），欧拉角模型完全够用，且物理意义更直观。

2.2 四旋翼刚体运动方程：平动与转动解耦

四旋翼的完整运动方程由平动动力学和转动动力学两部分组成。平动方程描述质心在惯性系中的加速度，转动方程描述机体绕质心的角加速度。两者通过旋转矩阵和推力/力矩分配耦合。

平动方程（牛顿第二定律）：

m · ẍ_E = R · F_B + m · g_E

展开为：
m·ẍ = (cosφ·sinθ·cosψ + sinφ·sinψ) · T
m·ÿ = (cosφ·sinθ·sinψ - sinφ·cosψ) · T
m·z̈ = (cosφ·cosθ) · T - m·g

其中T为四个电机产生的总推力，g为重力加速度。注意推力方向始终沿机体z轴，需要经过旋转矩阵投影到惯性系。

转动方程（欧拉方程）：

I · ω̇_B + ω_B × (I · ω_B) = τ_B

其中：
I = diag(Ixx, Iyy, Izz) 为惯性张量（假设对称）
ω_B = [p, q, r]ᵀ 为机体角速度
τ_B = [τ_φ, τ_θ, τ_ψ]ᵀ 为控制力矩

对于四旋翼，控制力矩由四个电机的差动推力产生。定义电机转速为ω₁~ω₄（逆时针编号），则推力与力矩的分配矩阵为：

[T]   [ 1  1  1  1 ] [ω₁²]
[τφ] = [ 0 -l  0  l ] [ω₂²]
[τθ]   [ l  0 -l  0 ] [ω₃²]
[τψ]   [ c -c  c -c ] [ω₄²]

其中 l 为机臂长度，c 为反扭矩系数

该分配矩阵是LQR控制中控制分配环节的核心，它将控制器输出的期望推力/力矩转换为四个电机的转速指令。

2.3 状态空间模型建立：线性化与标准形式

LQR控制器要求系统模型为线性时不变（LTI）状态空间形式：

ẋ = A·x + B·u
y = C·x + D·u

四旋翼模型是非线性的，因此需要在悬停工作点（φ=θ=ψ=0, 线速度=0, 角速度=0, T=mg）附近进行泰勒展开线性化。定义状态向量为12维：

x = [x, y, z, ẋ, ẏ, ż, φ, θ, ψ, p, q, r]ᵀ
u = [T, τ_φ, τ_θ, τ_ψ]ᵀ

线性化后的A矩阵（12×12）和B矩阵（12×4）具有明显的分块结构：

A = [ 0₃ₓ₃   I₃ₓ₃   0₃ₓ₃   0₃ₓ₃ ]
    [ 0₃ₓ₃   0₃ₓ₃   G₃ₓ₃   0₃ₓ₃ ]
    [ 0₃ₓ₃   0₃ₓ₃   0₃ₓ₃   I₃ₓ₃ ]
    [ 0₃ₓ₃   0₃ₓ₃   0₃ₓ₃   0₃ₓ₃ ]

B = [ 0₃ₓ₄ ]
    [ B₁₃ₓ₄ ]
    [ 0₃ₓ₄ ]
    [ B₂₃ₓ₄ ]

其中G矩阵包含重力项与姿态的耦合（如∂(g·φ)/∂φ = g），B₁和B₂分别对应推力对线加速度和力矩对角加速度的影响。具体数值取决于无人机质量、惯性矩和机臂长度。

2.4 案例拆解：某四旋翼平台的模型参数辨识与验证

以一款轴距450mm、起飞重量1.5kg的四旋翼为例，说明模型参数获取与验证流程。

验证过程：在仿真环境中输入阶跃推力指令，对比非线性模型与线性模型的响应。在悬停点附近（φ<10°），两者响应误差小于5%；当φ达到30°时，误差增大至15%，验证了线性模型的适用范围。

调参启示：惯性矩参数误差对LQR增益影响显著。若Ixx估计偏小20%，则LQR计算出的滚转通道增益会偏大，导致实际飞行中出现高频振荡。建议在真机调试前，用频率响应法（扫频激励）在线辨识惯性参数。

2.5 常见误区与总结复盘

误区一：忽略电机动态延迟。LQR设计时若假设电机转速能瞬时响应，实际系统中电机一阶惯性环节（时间常数约20-50ms）会引入相位滞后，导致控制裕度不足。解决方法是在B矩阵后串联电机模型，或提高R矩阵中对应控制量的权重以降低增益。

误区二：旋转矩阵方向约定不一致。不同飞控代码（PX4、ROS、自研）对坐标系正方向定义不同，导致重力补偿方向错误。建议在代码中统一使用NED（北东地）坐标系，并添加单元测试验证R矩阵。

误区三：线性化时忽略陀螺力矩耦合。欧拉方程中的ω×(I·ω)项在高速旋转时不可忽略，但在悬停线性化中通常被省略。若无人机需要做高速偏航运动，建议保留该项或采用非线性LQR（如SDRE方法）。