机器人路径规划算法之模型预测控制（MPC）算法详解＋MATLAB代码实现
·
目录
基于优化的路径规划（Optimization-based Planning）是目前最主流的高质量轨迹生成方法。它不再像传统图搜索那样只找“能走”的离散路径，而是直接求解一条平滑、动态可行、符合动力学的连续轨迹。
一、核心思想：从“找路”到“造路”
-
传统方法（A、RRT）*：在离散的地图上搜索一条“通过”的路径，往往是一串折线，需要后处理平滑。
-
基于优化的方法：直接将其建模为一个数学优化问题。它的目标是找出一条在物理上机器人能执行（满足动力学约束）、且舒适安全（平滑、避障）的轨迹。
二、关键实现：模型预测控制（MPC）
MPC 是这类方法的典型代表。它本质上是一个滚动优化的过程，可以理解为“边走边看，不断修正”。
MPC 的工作流程（三步循环）：
预测（Predict）：基于机器人当前的状态（位置、速度）和动力学模型，预测未来一小段时间（预测时域）内可能走过的轨迹。
优化（Optimize）：在预测的轨迹空间里，求解一个优化问题，目标是让这条轨迹：
代价最小（如：距离目标近、控制量小、加速度小）。
满足约束（如：不能撞到障碍物、速度不能超限、加速度在电机能力范围内）。
执行（Control）：只取优化结果的第一段控制指令执行，然后回到步骤1，根据新的传感器数据重新预测和优化。
💡 核心优势：MPC 能很好地处理不确定性和动态环境，因为它总是基于最新的状态进行重新规划。
三、数学本质：非线性优化问题
这类方法最终都会归结为求解如下形式的数学问题：
-
决策变量：通常是机器人的状态 x（位置、速度）和控制量 u（加速度、转向角）。
-
目标函数：衡量轨迹的好坏（如：距离目标点的误差 + 控制能量的消耗）。
-
约束条件：这是优化的精髓，确保解是可行的。
四、优缺点与应用场景
|
维度 |
说明 |
|---|---|
|
优点 |
轨迹质量极高（平滑、连续） |
|
缺点 |
计算量大（在线求解优化，比搜索慢） |
|
适用 |
自动驾驶（平滑换道、跟车） |
五、与搜索/采样的关系
在实际系统中，通常采用“分层规划”策略：
-
上层：用 A* 或 RRT 快速找一条粗略的路径（作为初值）。
-
下层：用基于优化的 MPC 将这条粗路径优化、平滑成可执行的轨迹。
一句话总结：基于优化的方法不是简单地找一条路，而是在物理规则的约束下，计算出一条机器人能“舒服”且“安全”地走完的最佳轨迹。
六、MATLAB实现
%% MPC路径规划 - 完整实现
clear; clc; close all;
%% 1. 问题定义与参数设置
dt = 0.1; % 时间步长
N = 20; % 预测时域
Q = diag([10, 10]); % 状态权重
R = 0.1; % 控制权重
P = 100; % 终端权重
% 机器人动力学模型（二阶积分器）
% 状态: [x, y, vx, vy]^T, 控制: [ax, ay]^T
% x_{k+1} = A * x_k + B * u_k
A = [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1];
B = [0.5*dt^2 0; 0 0.5*dt^2; dt 0; 0 dt];
% 起点和目标
x0 = [0; 0; 0; 0]; % 初始状态
goal = [5; 5; 0; 0]; % 目标状态
% 障碍物定义 [x, y, 半径]
obstacles = [2.0, 1.5, 0.5;
3.0, 3.0, 0.6;
1.5, 3.5, 0.4;
4.0, 2.0, 0.5];
%% 5. 主循环 - MPC滚动优化
max_iter = 100;
x_traj = zeros(4, max_iter+1);
u_traj = zeros(2, max_iter);
x_traj(:,1) = x0;
% 绘图初始化
figure('Position', [100, 100, 1200, 400]);
for iter = 1:max_iter
fprintf('=== 迭代 %d ===\n', iter);
% 当前状态
x_current = x_traj(:,iter);
% 如果接近目标则停止
if norm(x_current(1:2) - goal(1:2)) < 0.1
fprintf('到达目标点！\n');
break;
end
% 调用MPC求解器
[u_opt, x_pred] = mpc_solve(x_current, goal, A, B, N, Q, R, P, obstacles);
% 应用第一个控制量
u_traj(:,iter) = u_opt(:,1);
x_traj(:,iter+1) = A * x_current + B * u_opt(:,1);
%% 可视化
subplot(1,3,1);
cla; hold on; grid on;
% 绘制障碍物
for i = 1:size(obstacles,1)
rectangle('Position', [obstacles(i,1)-obstacles(i,3), ...
obstacles(i,2)-obstacles(i,3), ...
2*obstacles(i,3), 2*obstacles(i,3)], ...
'Curvature', [1,1], ...
'FaceColor', [0.8, 0.2, 0.2], 'EdgeColor', 'k');
end
% 绘制已走过的轨迹
plot(x_traj(1,1:iter+1), x_traj(2,1:iter+1), 'b-', 'LineWidth', 2);
% 绘制当前预测轨迹
plot(x_pred(1,:), x_pred(2,:), 'g--', 'LineWidth', 1.5);
% 标记起点和终点
plot(x0(1), x0(2), 'go', 'MarkerSize', 10, 'LineWidth', 2);
plot(goal(1), goal(2), 'rx', 'MarkerSize', 15, 'LineWidth', 2);
% 绘制机器人当前位置
plot(x_current(1), x_current(2), 'bo', 'MarkerSize', 8, 'LineWidth', 2);
xlabel('X位置'); ylabel('Y位置');
title('轨迹规划结果');
axis equal; axis([-1 6 -1 6]);
legend('障碍物', '实际轨迹', '预测轨迹', '起点', '目标', '机器人');
%% 状态和控制量可视化
subplot(2,3,2);
plot(0:iter-1, x_traj(3,1:iter), 'b-', 'LineWidth', 2);
hold on; grid on;
plot(0:iter-1, x_traj(4,1:iter), 'r-', 'LineWidth', 2);
xlabel('时间步'); ylabel('速度');
title('速度变化');
legend('Vx', 'Vy');
subplot(2,3,5);
plot(0:iter-1, u_traj(1,1:iter), 'b-', 'LineWidth', 2);
hold on; grid on;
plot(0:iter-1, u_traj(2,1:iter), 'r-', 'LineWidth', 2);
xlabel('时间步'); ylabel('控制输入');
title('控制输入(加速度)');
legend('ax', 'ay');
%% 代价函数值可视化
subplot(1,3,3);
if iter > 1
% 计算每个时间步的代价
costs = zeros(1, iter);
for t = 1:iter
xt = x_traj(:,t);
ut = u_traj(:,t);
pos_error = xt(1:2) - goal(1:2);
costs(t) = pos_error' * Q * pos_error + ut' * R * ut;
end
plot(0:iter-1, costs, 'm-', 'LineWidth', 2);
grid on;
xlabel('时间步'); ylabel('代价值');
title('代价函数变化');
end
drawnow;
pause(0.1);
end
%% 6. 最终结果分析
fprintf('\n====== 规划结果统计 ======\n');
fprintf('总迭代步数: %d\n', iter);
fprintf('最终位置误差: %.4f\n', norm(x_traj(1:2,iter+1) - goal(1:2)));
fprintf('最大加速度: %.4f\n', max(abs(u_traj(:))));
fprintf('平均加速度: %.4f\n', mean(abs(u_traj(:,1:iter)), 'all'));
% 绘制最终轨迹
figure('Position', [100, 100, 800, 600]);
hold on; grid on;
% 绘制障碍物
for i = 1:size(obstacles,1)
rectangle('Position', [obstacles(i,1)-obstacles(i,3), ...
obstacles(i,2)-obstacles(i,3), ...
2*obstacles(i,3), 2*obstacles(i,3)], ...
'Curvature', [1,1], ...
'FaceColor', [0.8, 0.2, 0.2], 'EdgeColor', 'k');
end
% 绘制完整轨迹
plot(x_traj(1,1:iter+1), x_traj(2,1:iter+1), 'b-', 'LineWidth', 3);
plot(x0(1), x0(2), 'go', 'MarkerSize', 15, 'LineWidth', 3);
plot(goal(1), goal(2), 'rx', 'MarkerSize', 20, 'LineWidth', 3);
% 标记轨迹点
plot(x_traj(1,1:5:end), x_traj(2,1:5:end), 'ko', 'MarkerSize', 6);
xlabel('X位置'); ylabel('Y位置');
title('MPC路径规划 - 最终结果');
legend('障碍物', '规划轨迹', '起点', '目标点');
axis equal; axis([-1 6 -1 6]);
%% 7. 轨迹平滑性分析
figure;
subplot(2,2,1);
plot(diff(x_traj(3,1:iter)), 'b-', 'LineWidth', 2);
grid on; title('加速度变化(平滑性)'); ylabel('ΔVx');
xlabel('时间步');
subplot(2,2,2);
plot(diff(x_traj(4,1:iter)), 'r-', 'LineWidth', 2);
grid on; title('加速度变化(平滑性)'); ylabel('ΔVy');
xlabel('时间步');
subplot(2,2,3);
plot(u_traj(1,1:iter), 'b-', 'LineWidth', 2);
hold on; grid on;
plot(u_traj(2,1:iter), 'r-', 'LineWidth', 2);
title('控制输入序列'); ylabel('加速度');
xlabel('时间步'); legend('ax', 'ay');
subplot(2,2,4);
traj_length = sum(sqrt(diff(x_traj(1,1:iter+1)).^2 + diff(x_traj(2,1:iter+1)).^2));
fprintf('轨迹总长度: %.4f\n', traj_length);
bar([norm(x_traj(1:2,iter+1) - goal(1:2)), ...
max(abs(u_traj(:))), ...
traj_length]);
set(gca, 'XTickLabel', {'位置误差', '最大加速度', '轨迹长度'});
title('性能指标');
%% ====================================================
%% 以下是函数定义（必须放在文件末尾）
%% ====================================================
%% 2. MPC优化求解函数
function [u_opt, x_opt] = mpc_solve(x_current, goal, A, B, N, Q, R, P, obstacles)
% 优化变量: U = [u0; u1; ...; u_{N-1}]
n_u = 2; % 控制维度
n_x = 4; % 状态维度
% 使用fmincon求解非线性优化
U0 = zeros(n_u * N, 1); % 初始猜测
% 设置优化选项
options = optimoptions('fmincon', ...
'Algorithm', 'interior-point', ...
'MaxIterations', 100, ...
'Display', 'iter-detailed');
% 定义目标函数
objective = @(U) mpc_cost(U, x_current, goal, A, B, N, Q, R, P, obstacles);
% 定义非线性约束
nonlcon = @(U) mpc_constraints(U, x_current, A, B, N, obstacles);
% 控制量上下界
lb = -2.0 * ones(n_u * N, 1); % 最大加速度
ub = 2.0 * ones(n_u * N, 1);
% 求解优化
[U_opt, ~] = fmincon(objective, U0, [], [], [], [], lb, ub, nonlcon, options);
% 提取最优控制序列
u_opt = reshape(U_opt, n_u, N);
% 计算对应的状态轨迹
x_opt = zeros(n_x, N+1);
x_opt(:,1) = x_current;
for k = 1:N
x_opt(:,k+1) = A * x_opt(:,k) + B * u_opt(:,k);
end
end
%% 3. MPC代价函数
function cost = mpc_cost(U, x0, goal, A, B, N, Q, R, P, obstacles)
n_u = 2; n_x = 4;
% 从U中提取控制序列
u_seq = reshape(U, n_u, N);
% 前向模拟得到状态轨迹
x = zeros(n_x, N+1);
x(:,1) = x0;
for k = 1:N
x(:,k+1) = A * x(:,k) + B * u_seq(:,k);
end
% 计算总代价
cost = 0;
% 阶段代价
for k = 1:N
state_error = x(1:2,k) - goal(1:2);
control_effort = u_seq(:,k);
cost = cost + state_error' * Q * state_error ...
+ control_effort' * R * control_effort;
end
% 终端代价
terminal_error = x(1:2,end) - goal(1:2);
cost = cost + terminal_error' * P * terminal_error;
% 避障代价（障碍物惩罚）
obstacle_cost = 0;
for k = 1:N+1
pos = x(1:2,k);
for i = 1:size(obstacles,1)
obs_pos = obstacles(i,1:2)';
obs_radius = obstacles(i,3);
dist = norm(pos - obs_pos);
safe_dist = obs_radius + 0.3; % 安全距离
if dist < safe_dist
% 指数惩罚函数
obstacle_cost = obstacle_cost + 100 * exp(-(dist - obs_radius) * 10);
end
end
end
cost = cost + obstacle_cost;
end
%% 4. 约束函数
function [c, ceq] = mpc_constraints(U, x0, A, B, N, obstacles)
n_u = 2; n_x = 4;
% 从U中提取控制序列
u_seq = reshape(U, n_u, N);
% 前向模拟
x = zeros(n_x, N+1);
x(:,1) = x0;
for k = 1:N
x(:,k+1) = A * x(:,k) + B * u_seq(:,k);
end
% 不等式约束：避障距离约束
n_obs = size(obstacles,1);
c = zeros((N+1) * n_obs, 1);
idx = 1;
for k = 1:N+1
pos = x(1:2,k);
for i = 1:n_obs
obs_pos = obstacles(i,1:2)';
obs_radius = obstacles(i,3);
safe_dist = obs_radius + 0.3; % 安全距离
% 障碍物约束：距离必须大于安全距离
c(idx) = safe_dist^2 - sum((pos - obs_pos).^2);
idx = idx + 1;
end
end
% 等式约束：动力学约束（已经在状态转移中满足）
ceq = [];
end1、总体结构
MPC_Path_Planning.m
├── 1. 主脚本（主程序执行流）
├── 2. 核心函数定义
├── 3. 优化求解器接口
└── 4. 代价与约束函数2、详细模块结构
1. 主程序执行流程
%% MPC路径规划 - 完整实现
clear; clc; close all; % 环境初始化
%% 1. 问题定义与参数设置
├── 系统参数设置 (dt, N, Q, R, P)
├── 机器人动力学模型 (A, B矩阵)
├── 初始条件定义 (x0, goal)
└── 环境设置 (obstacles障碍物)
%% 5. 主循环 - MPC滚动优化
├── 变量初始化 (x_traj, u_traj存储数组)
├── 图形界面初始化
├── for循环: MPC在线规划 (1:max_iter)
├── 状态获取与终止条件检查
├── 调用mpc_solve求解优化问题
├── 应用控制量，状态更新
├── 实时可视化
├── 子图1: 轨迹规划图
├── 子图2: 速度变化图
├── 子图3: 控制输入图
├── 子图4: 代价函数图
└── 动态显示与暂停
└── 循环结束条件判断
%% 6. 最终结果分析
├── 性能统计输出
├── 最终轨迹绘制
└── 障碍物与路径可视化
%% 7. 轨迹平滑性分析
├── 速度变化分析
├── 控制输入分析
└── 综合性能指标柱状图2. 核心函数模块
%% 以下是函数定义（必须放在文件末尾）
%% 2. MPC优化求解函数: mpc_solve()
├── 功能: 封装优化求解过程
├── 输入参数:
├── x_current: 当前状态
├── goal: 目标状态
├── A, B: 系统矩阵
├── N: 预测时域
├── Q, R, P: 权重矩阵
└── obstacles: 障碍物信息
├── 内部流程:
├── 优化变量定义 (控制序列U)
├── 优化器设置 (fmincon配置)
├── 目标函数定义 (@mpc_cost)
├── 约束函数定义 (@mpc_constraints)
├── 边界约束设置 (lb, ub)
└── 求解与结果提取
├── 输出:
├── u_opt: 最优控制序列
└── x_opt: 预测状态轨迹
%% 3. MPC代价函数: mpc_cost()
├── 功能: 计算控制序列的总代价
├── 代价组成:
├── 状态跟踪代价: Σ(state_error'*Q*state_error)
├── 控制能量代价: Σ(control_effort'*R*control_effort)
├── 终端代价: terminal_error'*P*terminal_error
└── 避障惩罚代价: obstacle_cost
├── 计算流程:
├── 控制序列展开
├── 前向模拟状态轨迹
├── 计算各项代价分量
└── 返回总代价值
%% 4. 约束函数: mpc_constraints()
├── 功能: 定义优化问题的约束条件
├── 约束类型:
├── 不等式约束(c): 避障安全距离
└── 等式约束(ceq): 动力学约束(已隐含)
├── 实现方法:
├── 前向状态预测
├── 计算与障碍物的距离约束
└── 返回约束向量3、算法流程图
开始
↓
初始化系统参数
↓
设置起点、目标、障碍物
↓
while 未到达目标
│
├─ 获取当前状态
│
├─ 调用mpc_solve:
│ ├─ 构建优化问题
│ ├─ 调用mpc_cost计算代价
│ ├─ 调用mpc_constraints检查约束
│ └─ 使用fmincon求解
│
├─ 应用最优控制第一步
│
├─ 更新机器人状态
│
└─ 实时可视化
↓
到达目标 → 结束4、数据结构说明
|
变量名 |
维度 |
含义 |
用途 |
|---|---|---|---|
|
|
4×1 |
当前状态向量 |
优化问题的初始条件 |
|
|
4×1 |
目标状态 |
代价函数跟踪目标 |
|
|
n×3 |
障碍物信息 |
避障约束输入 |
|
|
4×(max_iter+1) |
历史状态轨迹 |
记录与可视化 |
|
|
2×max_iter |
历史控制输入 |
记录与分析 |
|
|
4×(N+1) |
预测状态轨迹 |
优化结果展示 |
5、关键算法特点
-
滚动优化：每个时刻重新求解优化问题
-
模型预测：基于系统模型预测未来状态
-
在线避障：实时考虑障碍物约束
-
平滑性保证：通过最小化控制能量实现
-
多目标平衡：目标跟踪+能量优化+避障
6、扩展接口
可以通过修改以下部分扩展功能：
-
替换
mpc_cost函数 → 自定义代价函数 -
修改
mpc_constraints→ 添加新的约束 -
调整A、B矩阵 → 适配不同动力学模型
-
增加障碍物检测 → 支持动态障碍物
7、结果
命令行：
====== 规划结果统计 ======
总迭代步数: 47
最终位置误差: 7.0711
最大加速度: 2.0000
平均加速度: 1.7378
轨迹总长度: 15.5043可视化：
AtomGit 是由开放原子开源基金会联合 CSDN 等生态伙伴共同推出的新一代开源与人工智能协作平台。平台坚持“开放、中立、公益”的理念，把代码托管、模型共享、数据集托管、智能体开发体验和算力服务整合在一起，为开发者提供从开发、训练到部署的一站式体验。
更多推荐
19
0- 0
已为社区贡献1条内容
所有评论(0)