自动驾驶横纵向耦合控制：MPC实现与优化

1. 自动驾驶横纵向耦合控制概述

横纵向耦合控制是自动驾驶系统的核心技术之一，它负责将路径规划模块输出的理想轨迹转化为车辆的实际运动。与传统的横向、纵向独立控制不同，耦合控制通过一个统一的控制器同时处理转向和速度控制，能够更好地协调车辆运动，提升控制精度和乘坐舒适性。

在Apollo开源自动驾驶框架中，横纵向耦合控制采用了基于模型预测控制（MPC）的解决方案。这种方法的优势在于能够显式处理系统约束，并通过滚动优化来补偿模型误差。我在复现过程中发现，要实现工业级的控制效果，需要重点关注三个关键点：精确的车辆动力学建模、合理的约束条件设置以及高效的求解算法选择。

2. 动力学误差模型构建

2.1 车辆运动学模型

建立准确的车辆模型是控制算法的基础。我采用了经典的自行车模型作为基础框架，该模型假设车辆前后轮可以等效为单个轮子，适用于低速场景下的控制问题。模型状态方程如下：

code复制dx/dt = v * cos(θ + β)
dy/dt = v * sin(θ + β)
dθ/dt = v * tan(δ)/L * cos(β)
dv/dt = a

其中β=arctan(lr*tan(δ)/(lf+lr))为滑移角，L为轴距，δ为前轮转角，a为加速度。

2.2 误差状态方程

为了便于控制器设计，需要将全局坐标系下的模型转换为相对于参考轨迹的误差模型。定义跟踪误差状态量为：

code复制e = [e_y, e_θ, e_v, e_a]^T

其中e_y为横向位置误差，e_θ为航向角误差，e_v为速度误差，e_a为加速度误差。通过线性化处理，可以得到离散时间的误差状态方程：

code复制e(k+1) = A_k e(k) + B_k u(k) + W_k

这个方程构成了MPC控制器设计的基础。在实际实现时，我发现模型参数的准确性对控制效果影响很大，特别是轮胎侧偏刚度的估计需要根据实际车辆特性进行标定。

3. MPC控制器设计

3.1 预测模型构建

基于误差状态方程，构建预测时域内的系统行为。设预测时域为Np，控制时域为Nc，则预测方程可表示为：

code复制E = Ψ e(k) + Φ U

其中E为预测时域内的状态序列，U为控制序列，Ψ和Φ为由系统矩阵构成的预测矩阵。

3.2 目标函数设计

目标函数需要平衡跟踪精度和控制平顺性。我采用的标准二次型代价函数包含三个部分：

code复制J = E^T Q E + U^T R U + ΔU^T S ΔU

其中Q、R、S分别为状态误差、控制量和控制增量的权重矩阵。经过多次调试，我发现横向误差的权重应该比纵向误差大20%-30%，这样能在保证安全性的同时提供舒适的乘坐体验。

3.3 约束条件处理

约束条件包括控制量约束和控制增量约束：

code复制u_min ≤ u ≤ u_max
Δu_min ≤ Δu ≤ Δu_max

在Apollo的实现中，还考虑了轮胎摩擦圆约束，确保车辆不会超出物理极限。我将这些约束转化为QP问题的线性不等式约束，使用有效集方法进行求解。

4. 仿真环境搭建

4.1 MATLAB/Simulink联合仿真

搭建了完整的仿真环境，包括：

• 车辆动力学模型（CarSim接口）
• 参考轨迹生成模块
• MPC控制器模块
• 执行器模型（包含延迟和饱和特性）

在Simulink中配置固定步长求解器（步长50ms），与MATLAB控制器进行联合仿真。这种设置能够较好地模拟实际车载控制器的运行环境。

4.2 参考轨迹生成

采用五次多项式生成换道轨迹：

code复制y(x) = a0 + a1x + a2x² + a3x³ + a4x⁴ + a5x⁵

这种曲线能保证位置、速度和加速度的连续性。在实际应用中，我发现轨迹的曲率变化率需要限制在0.25m^-1/s以内，以避免方向盘转动过快。

5. 代码实现与优化

5.1 面向对象实现

设计了三种实现方式，其中面向对象的版本更具扩展性。核心控制器类结构如下：

matlab复制classdef MPCController < handle
    properties
        Np = 10;    % 预测时域
        Nc = 5;     % 控制时域
        Q = diag([10,5,1,1]); % 状态权重
        R = 0.1*eye(2);      % 控制权重
        S = 0.01*eye(2);     % 控制增量权重
    end
    
    methods
        function u = solve(self, x0, refTraj)
            % 构建QP问题
            H = self.buildHessian();
            f = self.buildGradient(x0, refTraj);
            [A, b] = self.buildConstraints(x0);
            
            % 调用QP求解器
            options = optimoptions('quadprog','Display','off');
            u = quadprog(H,f,A,b,[],[],[],[],[],options);
        end
    end
end

5.2 实时性优化

为提高实时性能，采用了以下优化措施：

1. 热启动：使用上一周期的解作为初始猜测
2. 代码生成：将MATLAB代码转为C++代码
3. 稀疏矩阵：利用预测矩阵的稀疏性

实测表明，这些优化能将单次求解时间从15ms降低到5ms以内，满足实时控制要求。

6. 标定与验证

6.1 油门刹车标定

建立了基于查表的纵向控制接口：

matlab复制function throttle = getThrottle(desired_accel, current_speed)
    % 二维插值表
    persistent throttleMap;
    if isempty(throttleMap)
        load('throttleCalib.mat');
    end
    throttle = interp2(speedGrid,accelGrid,throttleMap,...
                      current_speed,desired_accel,'linear');
end

标定过程需要在不同速度下测量加速度响应，建立完整的映射关系。我发现至少需要覆盖0-120km/h的速度范围和-3~2m/s²的加速度范围。

6.2 测试场景设计

设计了多种测试场景验证控制器性能：

1. 双移线测试（ISO标准）
2. 正弦扫频测试（0.1-2Hz）
3. 紧急制动测试
4. 曲率连续变化测试

测试结果显示，横向位置误差能控制在0.1m以内，纵向速度误差小于0.5m/s，满足L2级自动驾驶要求。

7. 实际应用中的挑战

7.1 模型失配问题

当车辆负载变化或轮胎磨损时，动力学特性会发生变化。我采用的解决方案是：

• 在线参数估计：利用递归最小二乘法实时更新关键参数
• 鲁棒MPC：在目标函数中加入松弛变量

7.2 执行器延迟

实测发现从控制指令发出到实际执行存在约200ms的延迟。通过以下方法补偿：

1. 在预测模型中增加延迟环节
2. 使用Smith预估器结构
3. 在状态观测器中考虑延迟影响

8. 扩展与改进方向

基于当前实现，还可以进一步优化：

1. 非线性MPC：直接处理非线性模型，提高大曲率工况下的精度
2. 学习型MPC：结合强化学习自动调整权重参数
3. 车路协同：利用V2X信息扩展预测时域

在实际项目中，我发现将MPC与经典的PID控制结合使用效果更好——MPC负责全局优化，PID处理局部扰动。这种分层架构既保证了控制品质，又提高了系统鲁棒性。