CasADi与Matlab实现车辆轨迹跟踪MPC控制

1. 项目概述：当CasADi遇上车辆轨迹跟踪

在自动驾驶和智能车辆控制领域，模型预测控制（MPC）因其优秀的处理约束能力和前瞻性控制特点，成为轨迹跟踪问题的首选解决方案。而CasADi作为一款开源的符号计算框架，凭借其高效的自动微分能力和与多种求解器的无缝对接，正在成为MPC实现的新宠。这个项目将展示如何用Matlab结合CasADi框架，为质点车辆模型构建一个实时性良好的MPC控制器。

质点车辆模型虽然简化了车辆动力学，但保留了位置、速度和航向角等关键状态变量，非常适合作为MPC算法的教学案例和初步验证平台。通过这个项目，你不仅能掌握CasADi在MPC中的应用技巧，还能深入理解轨迹跟踪控制的核心思想。

2. 核心原理与技术选型

2.1 为什么选择CasADi+Matlab组合

CasADi是一个专门为优化控制问题设计的符号计算框架，相比直接使用Matlab的优化工具箱，它有三大优势：

1. 自动微分能力：自动生成高效的梯度计算代码，避免手动推导复杂模型的雅可比矩阵
2. 求解器无关性：支持IPOPT、WORHP、SNOPT等多种非线性求解器
3. 代码生成：可以导出高效的C代码，便于后续部署到实时系统

Matlab则提供了完善的数据可视化和算法验证环境，特别适合控制算法的快速原型开发。两者结合既能保证开发效率，又能获得不错的计算性能。

2.2 质点车辆模型的数学表达

我们采用的质点车辆模型状态空间表示为：

状态变量 x = [px, py, v, ψ]^T 控制输入 u = [a, δ]^T 其中： px, py - 车辆在全局坐标系中的位置 v - 车速 ψ - 航向角 a - 加速度 δ - 前轮转向角

动力学方程：

ẋ = v * cos(ψ) ẏ = v * sin(ψ) v̇ = a ψ̇ = v/L * tan(δ)

其中L为车辆轴距，这是一个简化的自行车模型。

3. MPC控制器设计与实现

3.1 MPC问题构建

MPC的核心是在每个控制周期求解一个有限时域的最优控制问题。我们的目标函数设计为：

min J = Σ( (x_k - x_ref)^T Q (x_k - x_ref) + u_k^T R u_k )

约束条件包括：

• 动力学约束（上述车辆模型）
• 控制量约束（加速度和转向角限制）
• 状态约束（如速度限制）

3.2 CasADi实现步骤详解

3.2.1 环境配置

首先确保已安装：

1. Matlab R2018b或更新版本
2. CasADi 3.5.5+ (可从官网下载)
3. IPOPT求解器（推荐3.12.13版本）

addpath('casadi') % 添加CasADi路径 import casadi.*

3.2.2 定义符号变量

% 定义系统维度 nx = 4; % 状态维度 nu = 2; % 控制维度 % 创建符号变量 x = SX.sym('x', nx); u = SX.sym('u', nu);

3.2.3 构建动力学函数

% 参数 L = 2.5; % 车辆轴距 % 动力学方程 xdot = [... x(3)*cos(x(4)); % px_dot x(3)*sin(x(4)); % py_dot u(1); % v_dot x(3)/L*tan(u(2)) % psi_dot ]; % 创建函数 f = Function('f', {x, u}, {xdot});

3.2.4 离散化模型

采用RK4方法进行离散化：

dt = 0.1; % 采样时间 k1 = f(x, u); k2 = f(x + dt/2*k1, u); k3 = f(x + dt/2*k2, u); k4 = f(x + dt*k3, u); xf = x + dt/6*(k1 + 2*k2 + 2*k3 + k4); F_rk4 = Function('F_rk4', {x, u}, {xf});

3.2.5 构建NLP问题

N = 10; % 预测步长 opti = casadi.Opti(); % 创建优化问题 % 决策变量 X = opti.variable(nx, N+1); % 状态轨迹 U = opti.variable(nu, N); % 控制轨迹 x0 = opti.parameter(nx,1); % 初始状态参数 X_ref = opti.parameter(nx,N+1); % 参考轨迹 % 目标函数 Q = diag([10,10,1,5]); % 状态权重 R = diag([1,5]); % 控制权重 J = 0; for k=1:N J = J + (X(:,k)-X_ref(:,k))'*Q*(X(:,k)-X_ref(:,k)) + U(:,k)'*R*U(:,k); end opti.minimize(J); % 动力学约束 for k=1:N opti.subject_to(X(:,k+1) == F_rk4(X(:,k), U(:,k))); end % 初始条件约束 opti.subject_to(X(:,1) == x0); % 控制量约束 opti.subject_to(-3 <= U(1,:) <= 3); % 加速度限制(m/s^2) opti.subject_to(-0.5 <= U(2,:) <= 0.5); % 转向角限制(rad) % 求解器设置 p_opts = struct('expand', true); s_opts = struct('max_iter', 100); opti.solver('ipopt', p_opts, s_opts);

4. 仿真测试与结果分析

4.1 参考轨迹生成

我们设计一个8字形的参考轨迹：

T = 20; % 总时间 t = 0:dt:T; px_ref = 10*sin(0.2*t); py_ref = 10*sin(0.1*t); v_ref = sqrt((0.2*10*cos(0.2*t)).^2 + (0.1*10*cos(0.1*t)).^2); psi_ref = atan2(0.1*10*cos(0.1*t), 0.2*10*cos(0.2*t)); Xref = [px_ref; py_ref; v_ref; psi_ref];

4.2 闭环仿真实现

% 初始化 x_hist = zeros(nx, length(t)); u_hist = zeros(nu, length(t)-1); x_current = [0; 0; 0.1; 0]; % 初始状态 % 主循环 for i = 1:length(t)-1 % 设置参数 opti.set_value(x0, x_current); opti.set_value(X_ref, Xref(:,i:i+N)); % 求解 sol = opti.solve(); % 获取控制量 u_opt = sol.value(U(:,1)); u_hist(:,i) = u_opt; % 状态更新 x_current = full(F_rk4(x_current, u_opt)); x_hist(:,i+1) = x_current; % 更新初始猜测 opti.set_initial(X, sol.value(X)); opti.set_initial(U, sol.value(U)); end

4.3 结果可视化

figure; plot(x_hist(1,:), x_hist(2,:), 'b-', 'LineWidth', 2); hold on; plot(px_ref, py_ref, 'r--', 'LineWidth', 1.5); xlabel('px (m)'); ylabel('py (m)'); legend('实际轨迹', '参考轨迹'); grid on; figure; subplot(2,1,1); plot(t(1:end-1), u_hist(1,:)); ylabel('加速度 (m/s^2)'); subplot(2,1,2); plot(t(1:end-1), u_hist(2,:)); ylabel('转向角 (rad)'); xlabel('时间 (s)');

5. 性能优化与工程实践

5.1 实时性优化技巧

1. 热启动：使用上一周期的解作为当前优化的初始猜测，可以显著减少迭代次数
2. 代码生成：将CasADi生成的NLP问题导出为C代码，可提升5-10倍计算速度

% 代码生成示例 opts = struct('main', true, 'mex', true); gen = CodeGenerator('mpc_nlp.c', opts); gen.add(opti); gen.generate();

3. 降低预测步长：在保证控制性能的前提下，适当减少N值

5.2 参数调节经验

1. 权重矩阵调节：

   • 增大Q(1:2)会使轨迹跟踪更紧密，但可能导致控制量剧烈变化
   • 增大R(2)可以抑制转向角的剧烈变化，使控制更平滑

2. 采样时间选择：

   • 通常取车辆响应时间的1/5~1/10
   • 过大的dt会导致离散化误差，过小会增加计算负担

5.3 常见问题排查

1. 求解器不收敛：

   • 检查约束是否可行（特别是初始状态是否满足约束）
   • 尝试放宽约束或调整初始猜测
   • 增加求解器最大迭代次数

2. 控制量抖动：

   • 增大控制权重R
   • 添加控制量变化率约束

   % 添加控制量变化率约束 for k=1:N-1 opti.subject_to(-0.1 <= U(:,k+1)-U(:,k) <= 0.1); end

3. 轨迹跟踪偏差大：

   • 检查参考轨迹是否满足车辆动力学约束（如急转弯时速度是否过高）
   • 调整预测时域长度N，通常N*dt应覆盖车辆响应主要动态

6. 扩展与进阶方向

1. 考虑轮胎力学的更精确模型：

   • 引入Pacejka轮胎模型
   • 考虑车辆侧偏特性

2. 障碍物避碰：

   % 添加障碍物约束示例 for k=1:N+1 opti.subject_to((X(1,k)-obs_x)^2 + (X(2,k)-obs_y)^2 >= r^2); end

3. 多车协同MPC：

   • 在优化问题中考虑其他车辆的预测轨迹
   • 添加防碰撞约束

4. 硬件在环测试：

   • 将MPC控制器部署到实时系统（如dSPACE）
   • 与车辆动力学模型（如CarSim）联合仿真

在实际项目中，我发现MPC参数调节是一个需要耐心和经验的过程。建议从简单的场景开始，逐步增加复杂度。同时，记录每次参数调整的效果，形成自己的参数调节经验库。对于实时性要求高的应用，代码生成和热启动技巧是必须掌握的。