非线性模型预测控制（NMPC）基于CasADi的MATLAB实现
一、NMPC与CasADi在MATLAB中的优势
1.1 NMPC核心原理
非线性模型预测控制（NMPC）通过滚动时域优化实现闭环控制：在每个采样时刻，基于当前状态求解有限时域的最优控制问题，仅应用第一个控制量，下一时刻重复该过程。其数学形式为：
其中
1.2 CasADi在MATLAB中的优势
- 符号计算：用
SX/MX符号变量定义模型，自动生成优化问题的雅可比/海森矩阵。 - 多求解器支持：无缝集成IPOPT（非线性规划）、ACADO（实时优化）等。
- 代码生成：可将优化问题编译为C代码，提升实时性。
- MATLAB兼容：语法与MATLAB高度融合，支持矩阵运算和可视化。
二、NMPC实现步骤与MATLAB代码
2.1 系统建模（以倒立摆为例）
倒立摆动力学（状态
其中
MATLAB代码：定义符号模型
import casadi.*
% 1. 定义符号变量
x = SX.sym('x', 4); % 状态: [p, θ, v, ω]
u = SX.sym('u', 1); % 控制: 力F
% 2. 系统参数
M = 1.0; m = 0.1; l = 0.5; g = 9.81; f = 0.1; % 摩擦系数
% 3. 连续时间动力学
p = x(1); theta = x(2); v = x(3); omega = x(4);
xdot = SX.zeros(4,1);
xdot(1) = v; % dp/dt = v
xdot(2) = omega; % dθ/dt = ω
xdot(3) = (-m*g*l*sin(theta) + u) / (M + m); % dv/dt
xdot(4) = (-(M+m)*g*l*sin(theta) - f*l*omega + u*cos(theta)) / (l*(M + m*sin(theta)^2)); % dω/dt
% 4. 创建CasADi函数（连续模型）
f_cont = Function('f_cont', {x, u}, {xdot});
2.2 模型离散化（前向欧拉法）
将连续模型离散为
% 离散化参数
dt = 0.05; % 采样时间0.05s
% 离散模型（欧拉法）
x_next = x + dt * f_cont(x, u);
f_disc = Function('f_disc', {x, u}, {x_next}); % 离散模型函数
2.3 构建NMPC优化问题
使用 Opti 类定义优化变量、目标函数和约束：
function [opti, X, U, x0, x_ref] = build_nmpc(f_disc, N, dt, Q, R, P, x_min, x_max, u_min, u_max)
% 输入:
% f_disc: 离散模型函数, N: 预测时域, dt: 采样时间
% Q,R,P: 权重矩阵, x_min/x_max: 状态约束, u_min/u_max: 控制约束
% 输出:
% opti: 优化问题对象, X: 状态序列, U: 控制序列, x0: 初始状态参数, x_ref: 参考轨迹参数
opti = Opti(); % 创建优化问题
% 1. 决策变量
nx = f_disc.size1_in(0); % 状态维度
nu = f_disc.size1_in(1); % 控制维度
X = opti.variable(nx, N+1); % 状态序列 (N+1步)
U = opti.variable(nu, N); % 控制序列 (N步)
% 2. 参数（在线更新）
x0 = opti.parameter(nx, 1); % 当前状态
x_ref = opti.parameter(nx, N+1); % 参考轨迹
% 3. 目标函数: 跟踪误差+控制代价
J = 0;
for k = 1:N
% 状态误差代价
J = J + (X(:,k) - x_ref(:,k))' * Q * (X(:,k) - x_ref(:,k));
% 控制代价
J = J + U(:,k)' * R * U(:,k);
end
% 终端代价
J = J + (X(:,N+1) - x_ref(:,N+1))' * P * (X(:,N+1) - x_ref(:,N+1));
opti.minimize(J);
% 4. 约束条件
% 4.1 动力学约束: x_{k+1} = f_disc(x_k, u_k)
for k = 1:N
opti.subject_to(X(:,k+1) == f_disc(X(:,k), U(:,k)));
end
% 4.2 初始状态约束: x_0 = 当前状态
opti.subject_to(X(:,1) == x0);
% 4.3 控制输入约束
for k = 1:N
opti.subject_to(opti.bounded(u_min, U(:,k), u_max));
end
% 4.4 状态约束
for k = 1:N+1
opti.subject_to(opti.bounded(x_min, X(:,k), x_max));
end
% 5. 配置求解器（IPOPT）
opts = struct();
opts.ipopt.print_level = 0; % 关闭IPOPT输出
opts.print_time = 0;
opti.solver('ipopt', opts); % 使用IPOPT求解器
end
2.4 实时滚动优化与仿真
% 1. 参数设置
N = 20; % 预测时域
dt = 0.05; % 采样时间
nx = 4; % 状态维度
nu = 1; % 控制维度
% 权重矩阵
Q = diag([10, 100, 1, 10]); % 状态权重: [p, θ, v, ω]
R = 0.1; % 控制权重
P = 10*Q; % 终端权重
% 约束
x_min = [-2, -pi/2, -2, -10]; % 状态下限
x_max = [2, pi/2, 2, 10]; % 状态上限
u_min = -10; % 控制下限 (N)
u_max = 10; % 控制上限 (N)
% 2. 构建NMPC优化器
[f_disc] = define_inverted_pendulum_model(); % 调用前面定义的模型
[opti, X, U, x0, x_ref] = build_nmpc(f_disc, N, dt, Q, R, P, x_min, x_max, u_min, u_max);
% 3. 仿真参数
sim_time = 10; % 总仿真时间
n_steps = sim_time / dt; % 仿真步数
x_current = [0; 0.1; 0; 0]; % 初始状态: 小车位置0，摆角0.1rad，速度0，角速度0
% 4. 存储历史数据
X_hist = zeros(nx, n_steps+1);
U_hist = zeros(nu, n_steps);
X_hist(:,1) = x_current;
% 5. 主仿真循环
for k = 1:n_steps
% 5.1 生成参考轨迹（目标: 摆竖直向上，小车位置0）
x_ref_traj = zeros(nx, N+1);
x_ref_traj(1,:) = 0; % 小车位置参考0
x_ref_traj(2,:) = 0; % 摆角参考0 (竖直向上)
x_ref_traj(3,:) = 0; % 速度参考0
x_ref_traj(4,:) = 0; % 角速度参考0
% 5.2 设置优化器参数
opti.set_value(x0, x_current); % 当前状态
opti.set_value(x_ref, x_ref_traj); % 参考轨迹
% 5.3 求解NMPC问题
try
sol = opti.solve(); % 求解优化问题
u_opt = sol.value(U(:,1)); % 取第一个控制量
X_pred = sol.value(X); % 预测状态轨迹
catch
warning('求解失败，使用零控制');
u_opt = 0;
end
% 5.4 应用控制，更新状态（前向仿真）
x_next = f_disc(x_current, u_opt);
x_current = full(x_next); % 转换为数值矩阵
% 5.5 存储数据
X_hist(:,k+1) = x_current;
U_hist(:,k) = u_opt;
% 5.6 显示进度
if mod(k, 20) == 0
fprintf('Step %d/%d, State: [%.2f, %.2f, %.2f, %.2f]\n', ...
k, n_steps, x_current(1), x_current(2), x_current(3), x_current(4));
end
end
2.5 结果可视化
% 1. 状态轨迹
t = 0:dt:(n_steps)*dt;
figure;
subplot(2,2,1); plot(t, X_hist(1,:), 'b-', 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('小车位置 p (m)'); title('小车位置'); grid on;
subplot(2,2,2); plot(t, X_hist(2,:), 'r-', 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('摆角 θ (rad)'); title('摆角'); grid on;
subplot(2,2,3); plot(t, X_hist(3,:), 'g-', 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('速度 v (m/s)'); title('小车速度'); grid on;
subplot(2,2,4); plot(t, X_hist(4,:), 'm-', 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('角速度 ω (rad/s)'); title('摆角速度'); grid on;
sgtitle('倒立摆NMPC控制结果');
% 2. 控制输入
figure;
plot(t(1:end-1), U_hist, 'b-', 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('控制力 F (N)'); title('控制输入'); grid on;
参考代码 非线性casadi模型预测控制NMPC_casadi www.youwenfan.com/contentcst/160561.html
三、关键优化与调试技巧
3.1 提升实时性
-
热启动（Warm Start）：用上一时刻解初始化当前优化问题
% 在循环中添加 if k > 1 opti.set_initial(X, X_pred_prev); % 用上一时刻预测状态初始化 opti.set_initial(U, U_pred_prev); % 用上一时刻控制序列初始化 end X_pred_prev = X_pred; % 缓存当前预测 U_pred_prev = sol.value(U); -
代码生成：将优化问题编译为C代码
% 生成C代码 codegen_opts = struct('mex', false, 'cpp', true); opti.generate_dependencies('nmpc_solver.c', codegen_opts);
3.2 约束处理
-
软约束：对易违反的约束（如状态约束）添加松弛变量
% 示例：状态约束软处理 slack = opti.variable(nx, N+1); % 松弛变量 for k = 1:N+1 opti.subject_to(X(:,k) <= x_max + slack(:,k)); opti.subject_to(X(:,k) >= x_min - slack(:,k)); opti.subject_to(slack(:,k) >= 0); end opti.minimize(J + 1e3 * sum(slack(:).^2)); % 惩罚松弛变量
3.3 调试工具
- IPOPT日志：设置
opts.ipopt.print_level = 5查看详细求解过程。 - 可行解检查：用
opti.debug检查约束可行性。 - 可视化预测轨迹：绘制
X_pred观察优化器输出是否合理。
四、扩展应用：移动机器人轨迹跟踪
将倒立摆模型替换为移动机器人模型，参考轨迹改为圆形轨迹，即可实现轨迹跟踪NMPC。核心代码修改如下：
% 移动机器人模型（状态[x,y,θ], 控制[v,ω]）
x = SX.sym('x',3); u = SX.sym('u',2);
xdot = [u(1)*cos(x(3)); u(1)*sin(x(3)); u(2)];
f_cont = Function('f_cont', {x,u}, {xdot});
% 参考轨迹：圆形
t = (0:N)*dt;
x_ref = 2*cos(t); y_ref = 2*sin(t); theta_ref = t + pi/2;
x_ref_traj = [x_ref; y_ref; theta_ref];
五、总结
本文基于CasADi在MATLAB中实现了一个完整的NMPC控制器，以倒立摆稳定控制为例，涵盖模型定义→离散化→优化问题构建→实时求解→可视化全流程。关键步骤包括：
- 用CasADi符号变量定义非线性动力学；
- 通过
Opti类构建带约束的二次规划问题； - 结合IPOPT求解器实现实时滚动优化；
- 通过热启动、代码生成提升实时性。
