1. 项目概述当CasADi遇上车辆轨迹跟踪在自动驾驶和智能车辆控制领域模型预测控制MPC因其优秀的处理约束能力和前瞻性控制特点成为轨迹跟踪问题的首选解决方案。而CasADi作为一款开源的符号计算框架凭借其高效的自动微分能力和与多种求解器的无缝对接正在成为MPC实现的新宠。这个项目将展示如何用Matlab结合CasADi框架为质点车辆模型构建一个实时性良好的MPC控制器。质点车辆模型虽然简化了车辆动力学但保留了位置、速度和航向角等关键状态变量非常适合作为MPC算法的教学案例和初步验证平台。通过这个项目你不仅能掌握CasADi在MPC中的应用技巧还能深入理解轨迹跟踪控制的核心思想。2. 核心原理与技术选型2.1 为什么选择CasADiMatlab组合CasADi是一个专门为优化控制问题设计的符号计算框架相比直接使用Matlab的优化工具箱它有三大优势自动微分能力自动生成高效的梯度计算代码避免手动推导复杂模型的雅可比矩阵求解器无关性支持IPOPT、WORHP、SNOPT等多种非线性求解器代码生成可以导出高效的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 环境配置首先确保已安装Matlab R2018b或更新版本CasADi 3.5.5 (可从官网下载)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, N1); % 状态轨迹 U opti.variable(nu, N); % 控制轨迹 x0 opti.parameter(nx,1); % 初始状态参数 X_ref opti.parameter(nx,N1); % 参考轨迹 % 目标函数 Q diag([10,10,1,5]); % 状态权重 R diag([1,5]); % 控制权重 J 0; for k1:N J J (X(:,k)-X_ref(:,k))*Q*(X(:,k)-X_ref(:,k)) U(:,k)*R*U(:,k); end opti.minimize(J); % 动力学约束 for k1:N opti.subject_to(X(:,k1) 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:iN)); % 求解 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(:,i1) x_current; % 更新初始猜测 opti.set_initial(X, sol.value(X)); opti.set_initial(U, sol.value(U)); end4.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 实时性优化技巧热启动使用上一周期的解作为当前优化的初始猜测可以显著减少迭代次数代码生成将CasADi生成的NLP问题导出为C代码可提升5-10倍计算速度% 代码生成示例 opts struct(main, true, mex, true); gen CodeGenerator(mpc_nlp.c, opts); gen.add(opti); gen.generate();降低预测步长在保证控制性能的前提下适当减少N值5.2 参数调节经验权重矩阵调节增大Q(1:2)会使轨迹跟踪更紧密但可能导致控制量剧烈变化增大R(2)可以抑制转向角的剧烈变化使控制更平滑采样时间选择通常取车辆响应时间的1/5~1/10过大的dt会导致离散化误差过小会增加计算负担5.3 常见问题排查求解器不收敛检查约束是否可行特别是初始状态是否满足约束尝试放宽约束或调整初始猜测增加求解器最大迭代次数控制量抖动增大控制权重R添加控制量变化率约束% 添加控制量变化率约束 for k1:N-1 opti.subject_to(-0.1 U(:,k1)-U(:,k) 0.1); end轨迹跟踪偏差大检查参考轨迹是否满足车辆动力学约束如急转弯时速度是否过高调整预测时域长度N通常N*dt应覆盖车辆响应主要动态6. 扩展与进阶方向考虑轮胎力学的更精确模型引入Pacejka轮胎模型考虑车辆侧偏特性障碍物避碰% 添加障碍物约束示例 for k1:N1 opti.subject_to((X(1,k)-obs_x)^2 (X(2,k)-obs_y)^2 r^2); end多车协同MPC在优化问题中考虑其他车辆的预测轨迹添加防碰撞约束硬件在环测试将MPC控制器部署到实时系统如dSPACE与车辆动力学模型如CarSim联合仿真在实际项目中我发现MPC参数调节是一个需要耐心和经验的过程。建议从简单的场景开始逐步增加复杂度。同时记录每次参数调整的效果形成自己的参数调节经验库。对于实时性要求高的应用代码生成和热启动技巧是必须掌握的。