基于CasADi的MPC实现自动驾驶轨迹跟踪-嵌云网-嵌入式AI开发资源站

基于CasADi的MPC实现自动驾驶轨迹跟踪

董云舟

markdown复制## 1. 项目概述：当模型预测控制遇上质点车辆模型

去年在开发自动驾驶小车时，我遇到了一个典型问题：如何在保证实时性的前提下，让车辆精准跟踪规划好的轨迹？传统PID控制在急转弯处总会出现超调，于是我把目光投向了模型预测控制（MPC）。这个基于CasADi框架实现的MPC方案，最终让小车实现了厘米级的跟踪精度。

CasADi作为符号计算框架，其优势在于：
- 自动微分功能简化了雅可比矩阵计算
- 支持多种非线性求解器（如IPOPT）
- 与Matlab无缝集成

质点车辆模型虽然简化了轮胎动力学，但保留了位置（x,y）和航向角φ三个核心状态量，非常适合验证控制算法的有效性。这个项目完整展示了从建模、优化问题构建到实时控制的闭环流程。

## 2. 核心原理拆解

### 2.1 质点车辆模型构建

在车辆坐标系下，我们定义状态向量：

x = [x位置, y位置, 航向角φ, 速度v]

code复制控制输入为前轮转角δ和加速度a。其运动学方程为：
```matlab
function dxdt = vehicle_model(~,x,u)
    dxdt = [
        x(4)*cos(x(3));       % x方向速度
        x(4)*sin(x(3));       % y方向速度
        x(4)*tan(u(1))/L;     % 航向角变化率
        u(2)                  % 加速度
    ];
end

其中L为轴距，这个简化模型忽略了：

轮胎侧偏特性
悬架动力学
质量分布影响

注意：当车速超过5m/s时，建议改用自行车模型以提高精度

2.2 MPC问题建模关键点

预测时域N=10，控制时域M=5时，优化问题包含：

matlab复制w = [x0; repmat(u0,N,1)];  % 决策变量初始化
nlp = struct(...
    'f', cost_function(w),...  % 代价函数
    'g', constraints(w),...    % 约束条件
    'x', w...                  % 优化变量
);

代价函数设计技巧：

matlab复制function J = cost_fn(X,U,ref)
    Q = diag([10,10,5,1]);    % 状态权重
    R = diag([0.1,0.01]);     % 控制量权重
    state_err = X - ref;
    J = state_err'*Q*state_err + U'*R*U;
end

3. CasADi实战全流程

3.1 环境配置要点

安装时特别注意：

bash复制# 必须匹配的版本组合
Matlab R2021a + 
CasADi 3.5.5 +
IPOPT 3.12.13

验证安装：

matlab复制import casadi.*
x = MX.sym('x');
disp(jacobian(sin(x),x))  % 应输出cos(x)

3.2 代码实现详解

核心优化循环结构：

matlab复制while t < sim_time
    % 1. 获取当前状态
    x0 = get_vehicle_state();  
    
    % 2. 求解MPC问题
    res = solver('x0',w0,'lbg',lbg,'ubg',ubg);
    
    % 3. 应用首个控制量
    u_opt = full(res.x(4:5)); 
    apply_control(u_opt);
    
    % 4. 热启动下一轮
    w0 = shift_initial_guess(res.x); 
end

参数调优经验：

预测时域N：10-20步（0.5-1秒）
采样周期dt：0.05-0.1秒
Q矩阵：y方向权重应大于x方向（横向误差更关键）

4. 典型问题排查指南

4.1 求解器报错处理

问题1：IPOPT返回"Restoration Failed"

检查约束是否冲突（如速度上限<0）
尝试放宽边界条件
增加求解器迭代次数：

matlab复制opts.ipopt.max_iter = 1000;

问题2：计算延迟超过采样周期

减少预测时域N
改用线性化模型
启用代码生成：

matlab复制solver.generate_dependencies('mpc_gen');
mex mpc_gen.c -DMATLAB_MEX_FILE

4.2 跟踪性能优化

现象：弯道跟踪滞后

解决方案：
1. 在代价函数中增加航向角误差权重
2. 引入前馈控制项：
```
matlab复制u_ff = atan(L*curvature);  % 曲率补偿
```

现象：直线段振荡

调整方案：

增大控制量权重R
添加速度相关权重：

matlab复制Q(4,4) = 1 + 0.5*abs(v);  % 速度越高，速度权重越大

5. 进阶扩展方向

5.1 考虑执行器延迟

在模型中加入延迟补偿：

matlab复制delay_steps = round(actuator_delay/dt);
U_delayed = [U(delay_steps:end,:); repmat(U(end,:),delay_steps,1)];

5.2 多车协同控制

扩展状态向量为：

code复制X = [x1; y1; φ1; v1; x2; y2; φ2; v2]

代价函数中加入车距约束：

matlab复制safe_dist = norm([x1-x2, y1-y2]) - 2*car_length;
nlp.g = [nlp.g; safe_dist];  % 添加安全距离约束

实测发现，在双车编队场景下，横向跟踪误差可控制在0.1m内。一个容易被忽视的细节是：在低附着力路面，需要将tan(δ)替换为更精确的轮胎模型，这时CasADi的自动微分优势就更加明显——只需修改模型方程，无需重写梯度计算。

code复制