4. 线性时变系统：非线性模型的线性化方法、泰勒展开、雅可比矩阵

说实话，搞自动驾驶路径跟踪这些年，我踩过最大的坑就是——把非线性系统当线性系统处理。你想想看，车辆动力学天生就是非线性的，轮胎侧偏力、横摆力矩、质心侧偏角，哪个不是弯弯绕绕的曲线关系？

但LQR控制器偏偏只认线性模型。怎么办？线性化。说白了，就是在某个工作点附近，用一条直线去近似那条曲线。虽然不完美，但够用。

4.1 为什么需要线性化？

我刚开始做LQR时，直接拿非线性模型去算控制律，结果仿真直接炸了。后来才明白，LQR的推导基础是线性系统理论。你给它一个非线性模型，它根本不知道怎么处理。

举个例子，车辆的运动方程：

ẋ = f(x, u)   // 非线性函数

这个f(x,u)里包含sin、cos、乘积项，甚至还有轮胎魔术公式。LQR看到这个，直接懵了。

所以我们需要在参考轨迹的每个点上，把这个非线性系统近似成线性时变系统：

ẋ = A(t)·x + B(t)·u

这里的A(t)和B(t)就是雅可比矩阵，随时间变化。嗯，这就是线性时变系统（LTV）的由来。

4.2 泰勒展开：线性化的数学工具

泰勒展开，大学高数课上都学过。但真正用起来，我建议你把它理解成「局部近似」。

对于一个非线性函数f(x)，在参考点x_ref附近展开：

f(x) ≈ f(x_ref) + f'(x_ref)·(x - x_ref) + 高阶项

我们只保留一阶项，忽略高阶项。为什么？因为LQR只关心线性部分。高阶项？那是非线性控制该操心的事。

具体到车辆模型，假设状态向量为：

x = [y, ψ, v, δ]^T   // 横向位移、航向角、速度、前轮转角

在参考点x_ref处做泰勒展开：

ẋ ≈ f(x_ref, u_ref) + ∂f/∂x|_(x_ref,u_ref) · (x - x_ref) 
     + ∂f/∂u|_(x_ref,u_ref) · (u - u_ref)

这里∂f/∂x和∂f/∂u就是雅可比矩阵。我习惯把它们记作A_d和B_d，d代表离散化后的矩阵。

4.3 雅可比矩阵：线性化的核心

雅可比矩阵，说白了就是多变量函数的导数矩阵。每个元素表示一个输出对某个输入的偏导数。

对于车辆模型，假设状态方程有4个状态，2个控制输入：

f(x,u) = [f1(x,u), f2(x,u), f3(x,u), f4(x,u)]^T

雅可比矩阵A = ∂f/∂x 是一个4×4矩阵：

A = [∂f1/∂x1  ∂f1/∂x2  ∂f1/∂x3  ∂f1/∂x4
     ∂f2/∂x1  ∂f2/∂x2  ∂f2/∂x3  ∂f2/∂x4
     ∂f3/∂x1  ∂f3/∂x2  ∂f3/∂x3  ∂f3/∂x4
     ∂f4/∂x1  ∂f4/∂x2  ∂f4/∂x3  ∂f4/∂x4]

B矩阵类似，是4×2矩阵。

我在项目中遇到过一个问题：雅可比矩阵的数值计算。手算太容易出错，尤其是车辆模型有十几个参数。后来我改用符号计算工具（比如SymPy）自动求导，再转成C代码。省心多了。

4.4 线性时变系统的离散化

得到连续时间的A(t)和B(t)后，还需要离散化才能用于LQR。为什么？因为控制器是数字的，采样周期是固定的。

常用的离散化方法有两种：

1. 前向欧拉法：简单，但精度一般
2. 零阶保持法：精度高，计算稍复杂

我一般用零阶保持法，尤其是采样周期较大时（比如50ms以上）。前向欧拉法在采样周期大时容易不稳定，吃过亏。

离散化公式：

A_d = I + A·Δt + (A·Δt)^2/2! + ...
B_d = B·Δt + A·B·Δt^2/2! + ...

实际工程中，取前两项就够用了。除非你的采样周期特别大（超过100ms），才需要考虑高阶项。

4.5 实战：车辆模型的线性化示例

拿一个简化的自行车模型举例：

状态：x = [y, ψ, v_y, ψ̇]^T
控制：u = [δ]  // 前轮转角

非线性方程：
ẏ = v_x·sin(ψ) + v_y·cos(ψ)
ψ̇ = ψ̇
v̇_y = -v_x·ψ̇ + (F_yf + F_yr)/m
ψ̈ = (l_f·F_yf - l_r·F_yr)/I_z

其中F_yf和F_yr是轮胎侧偏力，是v_y和ψ̇的非线性函数。

在参考点（直线行驶，ψ≈0，v_y≈0，ψ̇≈0）处线性化：

A = [0   v_x  1  0
     0   0    0  1
     0   0   -C_f/m  -C_f·l_f/v_x
     0   0   -C_f·l_f/I_z  -C_f·l_f^2/v_x]

B = [0, 0, C_f/m, C_f·l_f/I_z]^T

你看，线性化后的矩阵里出现了v_x（纵向速度）。这就是时变性的来源——车速变化，A矩阵也跟着变。

4.6 雅可比矩阵的数值计算技巧

手算雅可比矩阵容易出错，尤其是模型复杂时。我推荐两种方法：

方法	优点	缺点	适用场景
解析法	精度高，计算快	推导繁琐，易出错	模型简单，或使用符号计算工具
数值法（有限差分）	实现简单，通用性强	精度受步长影响，计算量大	模型复杂，或快速原型验证

数值法的核心代码：

def compute_jacobian(f, x0, u0, eps=1e-6):
    n = len(x0)
    m = len(u0)
    A = np.zeros((n, n))
    B = np.zeros((n, m))
    
    # 计算A矩阵
    f0 = f(x0, u0)
    for i in range(n):
        x_plus = x0.copy()
        x_plus[i] += eps
        f_plus = f(x_plus, u0)
        A[:, i] = (f_plus - f0) / eps
    
    # 计算B矩阵
    for j in range(m):
        u_plus = u0.copy()
        u_plus[j] += eps
        f_plus = f(x0, u_plus)
        B[:, j] = (f_plus - f0) / eps
    
    return A, B

步长eps的选择很关键。太大，近似误差大；太小，数值误差大。我一般取1e-6到1e-8之间，具体看状态量的量级。

4.7 线性时变系统的LQR适配

有了离散化的A_d(k)和B_d(k)，LQR就可以工作了。但要注意：

• Riccati方程需要反向递推：从终端时刻往前推，每一步都用当前的A_d和B_d
• 控制增益K(k)是时变的：每个时刻的K都不一样
• 计算量比时不变LQR大：因为每一步都要重新算Riccati方程

我做过一个测试：在100步的预测时域内，时变LQR比时不变LQR的跟踪误差小30%左右。代价是计算时间增加了5倍。嗯，这个trade-off值得权衡。

好了，线性化这块就聊到这儿。记住，雅可比矩阵是连接非线性系统和线性控制器的桥梁。搞懂了它，LQR实战就成功了一半。