从几何约束到控制指令：无人车运动学模型的线性化与离散化实践

1. 无人车运动学模型的基础认知

第一次接触无人车运动学模型时，我被那些复杂的公式弄得头晕眼花。直到有一天，我在停车场观察普通汽车的转向过程，突然明白了阿克曼转向几何的本质——就像我们小时候玩的遥控车，前轮转向时内外轮转角不同，这样才能保证车辆平稳转弯不侧滑。

无人车常用的自行车模型，可以理解为把四个轮子简化成两个轮子的单车。这个模型虽然简单，但包含了三个关键状态量：位置（x,y）、航向角ψ和速度v。在实际项目中，我习惯用后轴中心作为车辆坐标系原点，这样建模时计算会方便很多。这里要注意的是，航向角（车辆朝向）和轨迹切线方向在理想情况下是一致的，但遇到湿滑路面时就需要考虑动力学模型了。

为什么运动学模型如此重要？去年我们团队在做自动泊车项目时，直接使用原始GPS坐标生成的轨迹，结果车辆转弯时频繁出现"画龙"现象。后来引入运动学约束重新规划轨迹，问题立刻得到解决。这让我深刻体会到：运动学模型就是车辆运动的几何法则，违背它就会导致控制失效。

2. 非线性模型的线性化实战

记得第一次尝试直接给非线性模型设计MPC控制器时，求解器总是报错。导师提醒我："非线性模型就像一头野兽，需要先驯服它"。这个"驯服"过程就是线性化——在工作点附近用泰勒展开进行近似。

具体操作时，我总结出三个关键点：

1. 小偏差假设：就像用直尺测量很短的距离，只有在参考轨迹附近极小范围内，线性近似才有效。我们团队曾做过测试，当横向偏差超过0.5米时，线性化误差会急剧增大。
2. 工作点选择：每个轨迹点都要单独线性化。我习惯在代码里维护一个滑动窗口，实时更新当前参考点的状态。
3. 增量式表达：线性化后的模型应该使用状态量的变化值（Δx, Δy, Δψ），而不是绝对数值。这就像用相对坐标而非世界坐标来思考问题。

在Python实现中，我推荐使用SymPy库进行符号运算。下面是个简化版的线性化示例：

python复制from sympy import symbols, Matrix, sin, cos, tan

# 定义符号变量
x, y, ψ, v, δ, L, dt = symbols('x y ψ v δ L dt')

# 原始非线性模型
f_x = x + v*cos(ψ)*dt
f_y = y + v*sin(ψ)*dt
f_ψ = ψ + (v/L)*tan(δ)*dt

# 在参考点处求雅可比矩阵
A = Matrix([f_x, f_y, f_ψ]).jacobian([x, y, ψ])
B = Matrix([f_x, f_y, f_ψ]).jacobian([δ, v])

3. 离散化处理的工程细节

离散化是把连续时间模型转化为计算机能处理的差分方程。这个过程看似简单，但有几个坑我不得不提醒：

采样时间选择：去年调试一辆无人小巴时，发现控制器在dt=0.1s时很稳定，但改成0.05s反而出现震荡。后来发现是离散化误差与传感器延迟产生了耦合效应。经验法则是：dt应该大于等于状态更新周期。

离散化方法对比：

• 前向欧拉法：最简单但精度低，适合快速原型开发
• 后向欧拉法：数值稳定但计算量大
• 零阶保持法：保持输入不变性，MPC控制器的首选

这里分享一个C++实现的技巧——使用Eigen库的矩阵指数功能：

cpp复制#include <Eigen/Dense>
using namespace Eigen;

MatrixXd continuous_A(3,3), continuous_B(3,2);
//...填充连续系统矩阵...

// 零阶保持法离散化
MatrixXd discrete_A = (continuous_A*dt).exp();
MatrixXd discrete_B = continuous_A.inverse()*(discrete_A - MatrixXd::Identity(3,3))*continuous_B;

4. 与MPC控制器的完整对接

完整的工程实现就像组装乐高积木，需要处理好各个模块的接口。我们的标准流程是：

1. 轨迹生成器输出参考路径
2. 在每个控制周期：
   • 获取当前车辆状态
   • 找到最近参考点
   • 在该点线性化模型
   • 离散化处理
   • 传入MPC求解器
3. 执行控制指令

在实车测试中，我们发现两个常见问题：

• 线性化点滞后：当车辆偏离较大时，仍使用旧参考点会导致控制发散。解决方法是用预测状态来选择线性化点。
• 雅可比矩阵更新频率：没必要每个控制周期都重新计算，可以缓存结果直到偏离阈值。

最后给出一段完整的Python伪代码框架：

python复制class LinearizedMPC:
    def __init__(self, vehicle_params):
        self.L = vehicle_params['wheelbase']
        self.dt = vehicle_params['control_period']
        
    def update_model(self, ref_state, ref_input):
        """在线性化点更新模型"""
        self.A, self.B = self._linearize(ref_state, ref_input)
        self.Ad, self.Bd = self._discretize(self.A, self.B)
        
    def solve_control(self, current_state):
        """求解MPC问题"""
        # 构建优化问题
        prob = cvxpy.Problem(...)
        prob.solve()
        return optimal_input
        
# 主控制循环
while vehicle.running:
    nearest_ref = find_reference_point(trajectory)
    mpc.update_model(nearest_ref.state, nearest_ref.input)
    u = mpc.solve_control(sensor.get_state())
    actuator.execute(u)

在冬季测试中，这套方案成功让无人车在雪地赛道完成了稳定循迹。关键是要记住：线性化不是一次性工作，而是需要持续更新的动态过程。当看到车辆在复杂路况下依然能精准跟踪轨迹时，你会觉得所有的公式推导都是值得的。