2、无人机线性化模型建立

各位同学，欢迎来到第二章。

上一章我们聊了LQR控制的基本框架。说实话，那东西看着挺美，但真要往无人机上套，你会发现——咦？怎么对不上？

原因很简单：无人机是非线性系统。而LQR，它天生就是为线性系统准备的。所以这一章，我们得先把无人机这个「刺头」给捋顺了。

2.1 四旋翼无人机非线性动力学方程推导

先说说我的习惯。每次拿到一个新机型，我第一件事就是画受力分析图。别笑，这步省了，后面全是坑。

四旋翼无人机，说白了就是四个电机带着四个桨叶转。但它的运动，远比「四个电机转」要复杂得多。

我们通常把无人机运动分成两部分：

• 平动：前后左右上下，也就是位置变化
• 转动：俯仰、横滚、偏航，也就是姿态变化

这两部分还互相耦合。你想想看，想让无人机往前飞，你得先让机身倾斜。倾斜了，升力方向就变了，位置自然跟着动。

完整的动力学方程长这样：

m * a = F_gravity + F_thrust + F_drag
J * α = M_thrust + M_gyro + M_aero

其中：

• m 是无人机质量，J 是转动惯量矩阵
• a 是加速度，α 是角加速度
• F_thrust 是四个电机产生的总升力
• M_thrust 是升力差产生的力矩

嗯，这里要注意。很多初学者会把转动惯量当成一个常数。其实它是3x3的矩阵，而且随着无人机姿态变化，它在世界坐标系下的值也在变。我在项目中就吃过这个亏，当时仿真跑得好好的，一上真机就抖得跟筛子似的。后来才发现，是我把转动惯量简化过头了。

展开写的话，位置动力学是：

m * d²r/dt² = [0, 0, -mg] + R * [0, 0, T]

姿态动力学是：

J * dω/dt = -ω × (J * ω) + M

这里R是旋转矩阵，T是总升力，M是合力矩。ω是角速度向量。

看着是不是有点头大？别急，我们马上要干一件「偷懒」的事。

2.2 悬停状态下的线性化处理

为什么要线性化？

因为LQR只能处理线性系统。而无人机在悬停点附近，其实表现得很「乖」。

什么叫悬停点？就是无人机悬在空中不动，姿态水平，高度不变。这时候：

• 俯仰角 ≈ 0，横滚角 ≈ 0，偏航角 ≈ 常数
• 线速度 ≈ 0，角速度 ≈ 0
• 总升力 ≈ 重力

在这个点附近，我们可以用泰勒展开把非线性方程近似成线性方程。说白了，就是砍掉高阶小量，只保留一阶项。

我建议你记住这个核心思想：线性化不是精确等价，而是在小范围内足够近似。

具体操作是这样的：

1. 把状态变量写成「平衡点 + 小扰动」的形式
2. 代入原方程，消去平衡点项
3. 忽略二阶及以上的小量

举个例子，对于俯仰角θ，我们做近似：

sin(θ) ≈ θ
cos(θ) ≈ 1
θ * φ ≈ 0  （两个小量相乘，忽略）

这样做完以后，原本复杂的三角函数全变成了线性项。整个方程一下子就清爽了。

2.3 得到状态空间表达式

线性化之后，我们就可以把方程写成标准的状态空间形式：

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

对于四旋翼无人机，我常用的状态向量是12维的：

x = [x, y, z, φ, θ, ψ, u, v, w, p, q, r]^T

解释一下：

• x, y, z：位置（世界坐标系）
• φ, θ, ψ：姿态角（横滚、俯仰、偏航）
• u, v, w：线速度（机体坐标系）
• p, q, r：角速度（机体坐标系）

控制输入u通常是4维的：

u = [T, τ_φ, τ_θ, τ_ψ]^T

即总升力和三个轴的力矩。

经过线性化后，A矩阵和B矩阵会变成常数矩阵。这里我直接给出悬停点附近的简化形式：

A = [0_3x3, 0_3x3, I_3x3, 0_3x3;
     0_3x3, 0_3x3, 0_3x3, I_3x3;
     0_3x3, g*e3,  0_3x3, 0_3x3;
     0_3x3, 0_3x3, 0_3x3, 0_3x3]

B = [0_3x1,  0_3x3;
     0_3x1,  0_3x3;
     1/m,    0_3x1;
     0_3x1,  inv(J)]

其中e3 = [0, 0, 1]^T，g是重力加速度。

说实话，这个矩阵看着有点吓人。但你仔细看，其实很多块都是零矩阵。这是因为在悬停点附近，很多耦合项都被线性化掉了。

输出方程y = C*x + D*u就简单了。如果你只关心位置和姿态，C矩阵可以取：

C = [I_6x6, 0_6x6]

D矩阵通常取零矩阵，因为控制输入不会直接出现在输出中。

好了，到这里，我们就把无人机的非线性模型，变成了一个标准的线性状态空间模型。下一章，我们就可以拿着这个模型去设计LQR控制器了。

最后送你一句话：模型是控制的上限，算法只是逼近这个上限的手段。模型建得准，后面事半功倍。模型建得糙，后面全是补丁。

以上就是本章的全部内容。从非线性方程到线性状态空间，我们走完了最关键的一步。模型在手，控制不愁。