1. 项目概述
轨迹跟踪是自动驾驶和机器人控制领域的核心问题之一。最近我在研究如何利用CasADi框架实现模型预测控制(MPC)来解决质点车辆模型的轨迹跟踪问题,经过多次实验和调参,终于得到了一个稳定可靠的解决方案。这个方案在Matlab环境下实现,代码简洁高效,控制效果令人满意。
模型预测控制作为一种先进的控制策略,能够很好地处理系统约束和优化问题。而CasADi作为一个强大的符号计算框架,为MPC的实现提供了极大便利。本文将详细分享我的实现过程和经验心得,包括模型建立、控制器设计、参数调优等关键环节。
2. 核心原理与技术选型
2.1 模型预测控制基础
MPC的核心思想可以类比为"边走边看"的导航策略。不同于传统控制方法,MPC在每个控制周期都会:
- 基于当前状态预测未来一段时间内的系统行为
- 求解一个有限时域的最优控制问题
- 只执行第一个控制量,然后重复这个过程
这种滚动优化的方式使MPC能够:
- 显式处理各种约束(输入约束、状态约束等)
- 考虑系统的未来行为
- 适应模型不确定性和外部扰动
2.2 CasADi框架优势
在众多优化工具中,我选择CasADi主要基于以下考虑:
- 符号计算能力:支持自动微分,简化了梯度计算
- 高效求解:可生成高效的C代码
- 接口丰富:支持多种求解器(IPOPT、SNOPT等)
- 跨平台:在Matlab和Python中都能使用
特别是对于MPC问题,CasADi提供的Opti栈使得问题表述非常直观,几乎可以像写数学公式一样自然地描述优化问题。
2.3 质点车辆模型
我们采用经典的质点车辆模型(kinematic bicycle model),其状态方程为:
code复制ẋ = v * cos(θ + β)
ẏ = v * sin(θ + β)
θ̇ = (v / L) * sin(β)
v̇ = a
β = atan(0.5 * tan(δ))
其中:
- (x,y)为车辆位置
- θ为航向角
- v为速度
- a为加速度(控制输入)
- δ为前轮转角(控制输入)
- L为轴距
- β为滑移角
这个模型虽然简化了动力学特性,但对于低速场景的轨迹跟踪已经足够,且计算量小,适合实时控制。
3. MPC控制器设计与实现
3.1 问题建模
在MPC框架下,我们需要定义:
- 状态变量:x = [x, y, θ, v]ᵀ
- 控制输入:u = [a, δ]ᵀ
- 目标函数:跟踪误差 + 控制代价
- 约束条件:速度限制、加速度限制等
具体实现时,我采用了以下目标函数形式:
code复制J = Σ(||x-x_ref||²_Q + ||u||²_R) + ||x_N-x_ref_N||²_P
其中Q、R、P分别为状态、输入和终端状态的权重矩阵。
3.2 CasADi实现步骤
- 初始化Opti栈:
matlab复制opti = casadi.Opti();
- 定义决策变量:
matlab复制X = opti.variable(4, N+1); % 状态序列
U = opti.variable(2, N); % 控制序列
- 设置目标函数:
matlab复制obj = 0;
for k = 1:N
obj = obj + (X(:,k)-X_ref(:,k))'*Q*(X(:,k)-X_ref(:,k))...
+ U(:,k)'*R*U(:,k);
end
obj = obj + (X(:,N+1)-X_ref(:,N+1))'*P*(X(:,N+1)-X_ref(:,N+1));
opti.minimize(obj);
- 添加动力学约束:
matlab复制for k = 1:N
x_next = rk4(@vehicle_model, X(:,k), U(:,k), dt);
opti.subject_to(X(:,k+1) == x_next);
end
- 添加其他约束:
matlab复制opti.subject_to(-a_max <= U(1,:) <= a_max);
opti.subject_to(-delta_max <= U(2,:) <= delta_max);
opti.subject_to(0 <= X(4,:) <= v_max);
- 求解问题:
matlab复制opti.solver('ipopt');
sol = opti.solve();
3.3 关键参数设置
经过多次调试,我发现以下参数组合效果较好:
- 预测时域:N=10(对应2秒,dt=0.2s)
- 权重矩阵:
- Q = diag([10, 10, 5, 1]) % 位置误差权重较大
- R = diag([0.1, 0.1]) % 控制量权重较小
- P = 10*Q % 终端状态权重较大
- 约束限制:
- a_max = 2 m/s²
- delta_max = 0.5 rad
- v_max = 5 m/s
4. 实现技巧与问题排查
4.1 数值稳定性处理
在实际实现中,我发现几个常见的数值问题及解决方法:
-
角度不连续问题:
- 现象:当θ从2π跳变到0时,会导致优化失败
- 解决:在目标函数中使用角度差函数
angle_diff(θ,θ_ref)代替直接相减
-
求解器收敛问题:
- 现象:IPOPT有时会报告收敛失败
- 解决:添加适当的初始猜测,如:
matlab复制opti.set_initial(X, X_guess); opti.set_initial(U, zeros(2,N)); -
计算效率问题:
- 现象:求解时间超过控制周期
- 解决:限制最大迭代次数,并启用热启动:
matlab复制opts = struct; opts.ipopt.max_iter = 50; opti.solver('ipopt', opts);
4.2 参考轨迹生成
参考轨迹的质量直接影响控制效果。我推荐两种生成方法:
-
多项式轨迹:
matlab复制t = 0:dt:T; x_ref = polyval(polyfit([0,T], [x0,xf], 3), t); y_ref = polyval(polyfit([0,T], [y0,yf], 3), t); -
样条插值:
matlab复制pp = spline([0,t1,t2,T], [x0,x1,x2,xf]); x_ref = ppval(pp, t);
4.3 实时性优化
为提高实时性能,可以采取以下措施:
-
代码生成:
matlab复制opts = struct('mex', true); F = casadi.Function('F', {X, U}, {obj}, opts); F.generate('F.c'); -
简化模型:
- 当计算资源有限时,可进一步简化模型,如忽略滑移角β
-
多线程处理:
- 将轨迹预测和优化求解并行化
5. 完整实现与测试结果
5.1 Matlab代码结构
完整的实现包含以下主要文件:
main.m- 主程序,设置参数并运行仿真mpc_controller.m- MPC控制器实现vehicle_model.m- 车辆动力学模型plot_results.m- 结果可视化
核心控制器代码如下:
matlab复制function [u_opt, X_opt] = mpc_controller(x0, X_ref, params)
% 初始化
opti = casadi.Opti();
% 定义变量
X = opti.variable(4, params.N+1);
U = opti.variable(2, params.N);
% 目标函数
obj = 0;
for k = 1:params.N
obj = obj + (X(:,k)-X_ref(:,k))'*params.Q*(X(:,k)-X_ref(:,k))...
+ U(:,k)'*params.R*U(:,k);
end
obj = obj + (X(:,params.N+1)-X_ref(:,params.N+1))'*params.P...
* (X(:,params.N+1)-X_ref(:,params.N+1));
opti.minimize(obj);
% 动力学约束
for k = 1:params.N
x_next = rk4(@vehicle_model, X(:,k), U(:,k), params.dt);
opti.subject_to(X(:,k+1) == x_next);
end
% 其他约束
opti.subject_to(X(:,1) == x0);
opti.subject_to(params.u_min <= U <= params.u_max);
opti.subject_to(params.x_min <= X <= params.x_max);
% 求解
opti.solver('ipopt', struct('print_time',0));
sol = opti.solve();
% 返回结果
u_opt = sol.value(U(:,1));
X_opt = sol.value(X);
end
5.2 仿真结果分析
在多种测试场景下,控制器表现出色:
-
直线跟踪:
- 最大位置误差:<0.05m
- 稳态误差:≈0
-
圆形轨迹:
- 最大跟踪误差:<0.1m
- 控制量平滑无抖动
-
S形曲线:
- 在曲率变化剧烈处误差稍大(≈0.15m)
- 但仍能保持稳定跟踪
从计算效率看,单次优化平均耗时约20ms(i7-9750H),满足实时性要求。
6. 扩展与改进方向
在实际应用中,还可以考虑以下改进:
-
考虑动力学特性:
- 引入更复杂的动力学模型,如考虑轮胎力、载荷转移等
-
障碍物避碰:
- 在优化问题中添加避碰约束
-
参数自适应:
- 根据车速自动调整预测时域和权重
-
硬件部署:
- 将代码移植到嵌入式平台,如ROS或AutoSAR
这个实现虽然基于质点模型,但框架可以方便地扩展到更复杂的场景。CasADi的强大功能使得MPC的实现变得直观高效,特别适合控制算法的快速原型开发。