基于ACADO工具包的自主车道跟踪与避障MPC控制
基于ACADO工具包的自主车道跟踪和避障车辆模型预测控制（MPC）实现方案。
1. 系统架构与模型定义
%% ACADO-based Autonomous Lane Tracking and Obstacle Avoidance MPC
% 文件名: main_acado_mpc_lane_following.m
% 功能: 基于ACADO工具包的自主驾驶MPC控制器
% 依赖: ACADO Toolkit (需提前安装配置)
clear; clc; close all;
import acado.*;
%% 1. 车辆动力学模型定义
% 使用非线性自行车模型
function model = defineVehicleModel()
% 状态变量: [x, y, psi, v, delta] (位置, 航向角, 速度, 转向角)
% 控制变量: [a, delta_dot] (加速度, 转向角速度)
DifferentialState x; % X位置
DifferentialState y; % Y位置
DifferentialState psi; % 航向角
DifferentialState v; % 速度
DifferentialState delta; % 转向角
Control a; % 加速度
Control delta_dot; % 转向角速度
% 模型参数
L = 2.8; % 轴距(m)
Ts = 0.1; % 采样时间(s)
% 非线性动力学方程
f = acado.DifferentialEquation();
f.add( dot(x) == v * cos(psi) );
f.add( dot(y) == v * sin(psi) );
f.add( dot(psi) == v * tan(delta) / L );
f.add( dot(v) == a );
f.add( dot(delta) == delta_dot );
% 离散化
disc = acado.GaussLegendre4(f, Ts);
model = struct();
model.f = f;
model.disc = disc;
model.states = [x, y, psi, v, delta];
model.controls = [a, delta_dot];
model.L = L;
model.Ts = Ts;
end
%% 2. 参考轨迹生成
function [refTrajectory, obstacles] = generateReferenceTrajectory()
% 生成车道跟踪参考轨迹
N = 100; % 轨迹点数
% 直线车道
x_ref = linspace(0, 500, N)';
y_ref = 3.5 * ones(N, 1); % 车道中心线在y=3.5m
psi_ref = zeros(N, 1); % 航向角0
v_ref = 20 * ones(N, 1); % 参考速度20m/s
refTrajectory.x = x_ref;
refTrajectory.y = y_ref;
refTrajectory.psi = psi_ref;
refTrajectory.v = v_ref;
% 障碍物定义 (静态障碍物)
obstacles = struct();
obstacles(1).x = 150; % 障碍物1位置
obstacles(1).y = 3.5;
obstacles(1).radius = 2.5; % 障碍物半径
obstacles(2).x = 300; % 障碍物2位置
obstacles(2).y = 3.5;
obstacles(2).radius = 2.5;
% 动态障碍物 (移动车辆)
obstacles(3).x = 250;
obstacles(3).y = 3.5;
obstacles(3).radius = 2.0;
obstacles(3).vx = -5; % 向左移动
obstacles(3).vy = 0;
end
2. ACADO MPC控制器设计
%% 3. MPC控制器设计
function [mpcController, ocp] = designMPCController(model, N, refTrajectory, obstacles)
% 创建MPC优化问题
% 获取模型和变量
states = model.states;
controls = model.controls;
disc = model.disc;
% 预测时域
Np = N; % 预测步数
% 创建优化问题
ocp = acado.OCP(0.0, Np * model.Ts, Np);
% 添加动力学约束
ocp.subjectTo(disc);
% 状态约束
ocp.subjectTo(-0.5 <= states(1)); % X位置下界
ocp.subjectTo(states(1) <= 500); % X位置上界
ocp.subjectTo(0.0 <= states(2)); % Y位置下界
ocp.subjectTo(states(2) <= 7.0); % Y位置上界 (车道宽度)
ocp.subjectTo(-pi <= states(3)); % 航向下界
ocp.subjectTo(states(3) <= pi); % 航向上界
ocp.subjectTo(0.0 <= states(4)); % 速度下界
ocp.subjectTo(states(4) <= 30.0); % 速度上界
ocp.subjectTo(-0.5 <= states(5)); % 转向角下界
ocp.subjectTo(states(5) <= 0.5); % 转向角上界
% 控制约束
ocp.subjectTo(-3.0 <= controls(1)); % 加速度下界
ocp.subjectTo(controls(1) <= 3.0); % 加速度上界
ocp.subjectTo(-0.5 <= controls(2)); % 转向角速度下界
ocp.subjectTo(controls(2) <= 0.5); % 转向角速度上界
% 添加避障约束
for obs_idx = 1:length(obstacles)
obs = obstacles(obs_idx);
% 安全距离约束: ||[x - obs_x, y - obs_y]|| >= safe_distance
safe_dist = obs.radius + 1.0; % 安全距离为障碍物半径+1m
% 对于每个预测步添加约束
for k = 0:Np-1
% 创建符号表达式
x_state = states(1);
y_state = states(2);
% 障碍物位置（考虑动态障碍物）
if isfield(obs, 'vx')
obs_x = obs.x + obs.vx * (k * model.Ts);
obs_y = obs.y + obs.vy * (k * model.Ts);
else
obs_x = obs.x;
obs_y = obs.y;
end
% 距离约束
dist_sq = (x_state - obs_x)^2 + (y_state - obs_y)^2;
ocp.subjectTo(dist_sq >= safe_dist^2);
end
end
% 车道边界约束
lane_width = 3.5; % 车道宽度
left_bound = 0.0;
right_bound = lane_width;
for k = 0:Np-1
y_state = states(2);
ocp.subjectTo(left_bound + 0.5 <= y_state); % 左边界+余量
ocp.subjectTo(y_state <= right_bound - 0.5); % 右边界-余量
end
% 代价函数设计
% 跟踪误差代价
tracking_cost = 0;
for k = 0:Np-1
x_ref = refTrajectory.x(k+1);
y_ref = refTrajectory.y(k+1);
psi_ref = refTrajectory.psi(k+1);
v_ref = refTrajectory.v(k+1);
% 位置跟踪误差
tracking_cost = tracking_cost + ...
10.0 * (states(1) - x_ref)^2 + ...
10.0 * (states(2) - y_ref)^2 + ...
5.0 * (states(3) - psi_ref)^2 + ...
1.0 * (states(4) - v_ref)^2;
end
% 控制量代价
control_cost = 0;
for k = 0:Np-1
control_cost = control_cost + ...
0.1 * controls(1)^2 + ... % 加速度代价
0.1 * controls(2)^2; % 转向角速度代价
end
% 控制变化率代价（平滑性）
smoothness_cost = 0;
for k = 1:Np-1
% 这里需要使用差分，但在ACADO中可以通过状态导数表示
% 简化处理：直接使用控制量
smoothness_cost = smoothness_cost + ...
0.05 * controls(1)^2 + ...
0.05 * controls(2)^2;
end
% 总代价
total_cost = tracking_cost + control_cost + smoothness_cost;
% 设置代价函数
ocp.minimize(total_cost);
% 设置初始状态约束
ocp.subjectTo('AT_START', states == [0.0, 3.5, 0.0, 20.0, 0.0]');
% 创建MPC控制器
mpcController = acado.MPC(ocp, 0.0, Np * model.Ts, Np);
% 设置求解器参数
mpcController.set( 'MAX_NUM_ITERATIONS', 50 );
mpcController.set( 'KKT_TOL', 1e-6 );
mpcController.set( 'HESSIAN_APPROXIMATION', 'EXACT' );
mpcController.set( 'DISCRETIZATION_TYPE', 'MULTIPLE_SHOOTING' );
end
3. 仿真与可视化
%% 4. 主仿真循环
function runSimulation()
% 初始化
model = defineVehicleModel();
[refTrajectory, obstacles] = generateReferenceTrajectory();
% 设计MPC控制器
Np = 20; % 预测步数
[mpcController, ocp] = designMPCController(model, Np, refTrajectory, obstacles);
% 仿真参数
T_sim = 50; % 仿真时间(s)
N_sim = T_sim / model.Ts; % 仿真步数
% 初始化状态
x0 = [0.0, 3.5, 0.0, 20.0, 0.0]'; % 初始状态
% 存储仿真结果
simResults.time = zeros(N_sim, 1);
simResults.states = zeros(N_sim, length(model.states));
simResults.controls = zeros(N_sim, length(model.controls));
simResults.refTrajectory = refTrajectory;
simResults.obstacles = obstacles;
% 仿真循环
fprintf('开始MPC仿真...\n');
for k = 1:N_sim
% 更新参考轨迹（随时间移动）
current_time = (k-1) * model.Ts;
ref_idx = min(round(current_time / model.Ts) + 1, length(refTrajectory.x));
% 设置当前参考状态
current_ref = [refTrajectory.x(ref_idx), ...
refTrajectory.y(ref_idx), ...
refTrajectory.psi(ref_idx), ...
refTrajectory.v(ref_idx), ...
0.0]';
% 更新MPC参考轨迹
for i = 0:Np-1
future_idx = min(ref_idx + i, length(refTrajectory.x));
ref_state = [refTrajectory.x(future_idx), ...
refTrajectory.y(future_idx), ...
refTrajectory.psi(future_idx), ...
refTrajectory.v(future_idx), ...
0.0]';
mpcController.setReferenceTrajectory(i, ref_state);
end
% 设置当前状态
mpcController.setState(x0);
% 求解MPC问题
try
[x_opt, u_opt] = mpcController.solve();
% 应用第一个控制输入
u0 = u_opt(1, :)';
% 存储结果
simResults.time(k) = current_time;
simResults.states(k, :) = x0';
simResults.controls(k, :) = u0';
% 更新状态（使用车辆模型）
x_next = simulateStep(x0, u0, model);
x0 = x_next;
% 显示进度
if mod(k, 10) == 0
fprintf('仿真进度: %.1f%% (时间: %.1fs)\n', ...
100*k/N_sim, current_time);
end
catch ME
fprintf('MPC求解失败: %s\n', ME.message);
break;
end
end
% 可视化结果
visualizeResults(simResults, model);
end
function x_next = simulateStep(x, u, model)
% 使用欧拉法模拟一步
Ts = model.Ts;
L = model.L;
% 状态变量
x_pos = x(1);
y_pos = x(2);
psi = x(3);
v = x(4);
delta = x(5);
% 控制输入
a = u(1);
delta_dot = u(2);
% 更新状态
x_next = zeros(5, 1);
x_next(1) = x_pos + v * cos(psi) * Ts;
x_next(2) = y_pos + v * sin(psi) * Ts;
x_next(3) = psi + v * tan(delta) / L * Ts;
x_next(4) = v + a * Ts;
x_next(5) = delta + delta_dot * Ts;
end
%% 5. 可视化函数
function visualizeResults(results, model)
% 创建可视化界面
figure('Position', [100, 100, 1400, 900], ...
'Name', 'ACADO MPC 车道跟踪与避障', ...
'NumberTitle', 'off');
% 子图1: 轨迹跟踪
subplot(3, 3, 1);
hold on; grid on; axis equal;
% 绘制车道
lane_width = 3.5;
x_range = [0, max(results.states(:,1)) + 50];
plot(x_range, [0, 0], 'k--', 'LineWidth', 1.5, 'DisplayName', '左车道线');
plot(x_range, [lane_width, lane_width], 'k--', 'LineWidth', 1.5, 'DisplayName', '右车道线');
plot(x_range, [lane_width/2, lane_width/2], 'k-', 'LineWidth', 2, 'DisplayName', '车道中心线');
% 绘制参考轨迹
plot(results.refTrajectory.x, results.refTrajectory.y, 'b--', ...
'LineWidth', 2, 'DisplayName', '参考轨迹');
% 绘制实际轨迹
plot(results.states(:,1), results.states(:,2), 'r-', ...
'LineWidth', 2, 'DisplayName', '实际轨迹');
% 绘制障碍物
for i = 1:length(results.obstacles)
obs = results.obstacles(i);
rectangle('Position', [obs.x-obs.radius, obs.y-obs.radius, ...
2*obs.radius, 2*obs.radius], ...
'Curvature', [1, 1], 'FaceColor', 'r', 'FaceAlpha', 0.3, ...
'EdgeColor', 'r', 'LineWidth', 2);
text(obs.x, obs.y, sprintf('障碍物%d', i), ...
'HorizontalAlignment', 'center', 'FontWeight', 'bold');
end
% 绘制车辆当前位置
scatter(results.states(end,1), results.states(end,2), 100, 'r', ...
'filled', 'MarkerEdgeColor', 'k', 'LineWidth', 2);
xlabel('X位置 (m)');
ylabel('Y位置 (m)');
title('车道跟踪与避障轨迹');
legend('Location', 'best');
xlim(x_range);
ylim([-1, lane_width+1]);
% 子图2: 航向角跟踪
subplot(3, 3, 2);
hold on; grid on;
plot(results.time, rad2deg(results.states(:,3)), 'r-', 'LineWidth', 2, ...
'DisplayName', '实际航向角');
plot(results.time, rad2deg(results.refTrajectory.psi), 'b--', 'LineWidth', 2, ...
'DisplayName', '参考航向角');
xlabel('时间 (s)');
ylabel('航向角 (deg)');
title('航向角跟踪');
legend('Location', 'best');
% 子图3: 速度跟踪
subplot(3, 3, 3);
hold on; grid on;
plot(results.time, results.states(:,4), 'r-', 'LineWidth', 2, ...
'DisplayName', '实际速度');
plot(results.time, results.refTrajectory.v, 'b--', 'LineWidth', 2, ...
'DisplayName', '参考速度');
xlabel('时间 (s)');
ylabel('速度 (m/s)');
title('速度跟踪');
legend('Location', 'best');
% 子图4: 转向角
subplot(3, 3, 4);
hold on; grid on;
plot(results.time, rad2deg(results.states(:,5)), 'g-', 'LineWidth', 2);
xlabel('时间 (s)');
ylabel('转向角 (deg)');
title('转向角变化');
ylim([-30, 30]);
% 子图5: 控制输入 - 加速度
subplot(3, 3, 5);
hold on; grid on;
plot(results.time, results.controls(:,1), 'b-', 'LineWidth', 2);
xlabel('时间 (s)');
ylabel('加速度 (m/s²)');
title('加速度控制输入');
ylim([-3.5, 3.5]);
% 子图6: 控制输入 - 转向角速度
subplot(3, 3, 6);
hold on; grid on;
plot(results.time, rad2deg(results.controls(:,2)), 'm-', 'LineWidth', 2);
xlabel('时间 (s)');
ylabel('转向角速度 (deg/s)');
title('转向角速度控制输入');
ylim([-30, 30]);
% 子图7: 与障碍物距离
subplot(3, 3, 7);
hold on; grid on;
% 计算每个时刻到最近障碍物的距离
min_distances = zeros(length(results.time), 1);
for i = 1:length(results.time)
x_pos = results.states(i,1);
y_pos = results.states(i,2);
min_dist = inf;
for j = 1:length(results.obstacles)
obs = results.obstacles(j);
dist = sqrt((x_pos - obs.x)^2 + (y_pos - obs.y)^2) - obs.radius;
if dist < min_dist
min_dist = dist;
end
end
min_distances(i) = max(min_dist, 0);
end
plot(results.time, min_distances, 'r-', 'LineWidth', 2);
yline(1.0, 'g--', 'LineWidth', 2, 'Label', '安全距离');
xlabel('时间 (s)');
ylabel('最小距离 (m)');
title('与障碍物最小距离');
ylim([0, 5]);
% 子图8: 跟踪误差
subplot(3, 3, 8);
hold on; grid on;
% 横向误差 (y方向)
lateral_error = results.states(:,2) - results.refTrajectory.y(1:length(results.time));
plot(results.time, lateral_error, 'b-', 'LineWidth', 2);
xlabel('时间 (s)');
ylabel('横向误差 (m)');
title('横向跟踪误差');
% 子图9: 控制量变化率
subplot(3, 3, 9);
hold on; grid on;
% 加速度变化率
acc_change = [0; diff(results.controls(:,1))]/model.Ts;
plot(results.time, acc_change, 'r-', 'LineWidth', 2);
xlabel('时间 (s)');
ylabel('加速度变化率 (m/s³)');
title('控制平滑性');
% 添加总标题
sgtitle('基于ACADO的MPC车道跟踪与避障控制仿真结果', ...
'FontSize', 16, 'FontWeight', 'bold');
% 保存结果
save('acado_mpc_results.mat', 'results');
saveas(gcf, 'acado_mpc_simulation.png');
fprintf('仿真完成！结果已保存为 acado_mpc_simulation.png\n');
end
4. 高级功能扩展
%% 6. 高级功能: 变道避障策略
function [mpcController] = addLaneChangeStrategy(ocp, model, obstacles)
% 添加变道避障策略
states = model.states;
controls = model.controls;
% 检测前方障碍物
detection_range = 50; % 检测范围(m)
% 添加变道决策变量
lane_change = acado.Parameter(0); % 0: 不换道, 1: 向左换道, -1: 向右换道
% 变道约束
% 当需要换道时，允许越过车道边界
for k = 0:Np-1
y_state = states(2);
% 正常行驶时的车道约束
ocp.subjectTo(lane_change == 0, y_state >= 0.5);
ocp.subjectTo(lane_change == 0, y_state <= 3.0);
% 换道时的约束（允许进入相邻车道）
ocp.subjectTo(lane_change == 1, y_state >= -3.0); % 向左换道
ocp.subjectTo(lane_change == 1, y_state <= 3.5);
ocp.subjectTo(lane_change == -1, y_state >= 0.0); % 向右换道
ocp.subjectTo(lane_change == -1, y_state <= 6.5);
end
% 变道时的额外代价
lane_change_cost = 100 * lane_change^2;
% 更新代价函数
original_cost = ocp.getObjective();
ocp.minimize(original_cost + lane_change_cost);
return mpcController;
end
%% 7. 实时重规划功能
function runReplanningSimulation()
% 带实时重规划的仿真
model = defineVehicleModel();
[refTrajectory, obstacles] = generateReferenceTrajectory();
% 创建MPC控制器
Np = 15;
[mpcController, ocp] = designMPCController(model, Np, refTrajectory, obstacles);
% 仿真参数
T_sim = 60;
N_sim = T_sim / model.Ts;
% 初始化
x0 = [0.0, 3.5, 0.0, 20.0, 0.0]';
% 仿真循环
for k = 1:N_sim
current_time = (k-1) * model.Ts;
% 更新障碍物位置（模拟动态障碍物）
updateObstaclePositions(obstacles, current_time);
% 重新设计MPC问题（考虑新的障碍物位置）
[mpcController, ocp] = redesignWithNewObstacles(mpcController, model, obstacles);
% 求解MPC
mpcController.setState(x0);
[x_opt, u_opt] = mpcController.solve();
% 应用控制
u0 = u_opt(1, :)';
x0 = simulateStep(x0, u0, model);
% 每5步显示一次
if mod(k, 50) == 0
fprintf('时间: %.1fs, 位置: (%.1f, %.1f), 速度: %.1fm/s\n', ...
current_time, x0(1), x0(2), x0(4));
end
end
end
function updateObstaclePositions(obstacles, time)
% 更新动态障碍物位置
for i = 1:length(obstacles)
if isfield(obstacles(i), 'vx')
obstacles(i).x = obstacles(i).x + obstacles(i).vx * 0.1; % 每步更新
end
end
end
%% 8. 性能评估函数
function evaluatePerformance(results)
% 评估MPC控制性能
fprintf('\n========== MPC控制性能评估 ==========\n');
% 1. 跟踪精度
lateral_errors = results.states(:,2) - results.refTrajectory.y(1:length(results.time));
max_lateral_error = max(abs(lateral_errors));
rms_lateral_error = rms(lateral_errors);
fprintf('横向跟踪性能:\n');
fprintf(' 最大横向误差: %.3f m\n', max_lateral_error);
fprintf(' RMS横向误差: %.3f m\n', rms_lateral_error);
% 2. 速度跟踪
speed_errors = results.states(:,4) - results.refTrajectory.v(1:length(results.time));
max_speed_error = max(abs(speed_errors));
rms_speed_error = rms(speed_errors);
fprintf('\n速度跟踪性能:\n');
fprintf(' 最大速度误差: %.3f m/s\n', max_speed_error);
fprintf(' RMS速度误差: %.3f m/s\n', rms_speed_error);
% 3. 避障性能
min_distances = zeros(length(results.time), 1);
for i = 1:length(results.time)
x_pos = results.states(i,1);
y_pos = results.states(i,2);
min_dist = inf;
for j = 1:length(results.obstacles)
obs = results.obstacles(j);
dist = sqrt((x_pos - obs.x)^2 + (y_pos - obs.y)^2) - obs.radius;
if dist < min_dist
min_dist = dist;
end
end
min_distances(i) = max(min_dist, 0);
end
min_safe_distance = min(min_distances);
avg_safe_distance = mean(min_distances(min_distances > 0));
fprintf('\n避障性能:\n');
fprintf(' 最小安全距离: %.3f m\n', min_safe_distance);
fprintf(' 平均安全距离: %.3f m\n', avg_safe_distance);
% 4. 控制平滑性
acc_changes = [0; diff(results.controls(:,1))]/results.time(2);
steering_changes = [0; diff(results.controls(:,2))]/results.time(2);
acc_smoothness = rms(acc_changes);
steering_smoothness = rms(steering_changes);
fprintf('\n控制平滑性:\n');
fprintf(' 加速度变化RMS: %.3f m/s³\n', acc_smoothness);
fprintf(' 转向角速度变化RMS: %.3f rad/s²\n', steering_smoothness);
% 5. 总体评分
tracking_score = 100 * exp(-rms_lateral_error - 0.1*rms_speed_error);
safety_score = 100 * min(1, min_safe_distance/2);
comfort_score = 100 * exp(-0.1*acc_smoothness - 0.1*steering_smoothness);
overall_score = 0.5*tracking_score + 0.3*safety_score + 0.2*comfort_score;
fprintf('\n总体评分:\n');
fprintf(' 跟踪评分: %.1f/100\n', tracking_score);
fprintf(' 安全评分: %.1f/100\n', safety_score);
fprintf(' 舒适评分: %.1f/100\n', comfort_score);
fprintf(' 总体评分: %.1f/100\n', overall_score);
fprintf('======================================\n');
end
5. 主程序入口
%% 主程序入口
function main()
% 检查ACADO工具包是否可用
try
import acado.*;
fprintf('ACADO工具包已成功加载！\n');
catch
error('ACADO工具包未找到！请确保已正确安装并配置ACADO。');
end
% 运行仿真
fprintf('开始ACADO MPC车道跟踪与避障仿真...\n');
fprintf('模型: 非线性自行车模型\n');
fprintf('预测时域: 20步 (2秒)\n');
fprintf('控制频率: 10Hz\n');
% 运行主仿真
runSimulation();
% 可选: 运行性能评估
% evaluatePerformance(results);
end
% 执行主程序
main();
参考代码 基于ACADO工具包的自主车道跟踪和避障车辆的模型预测控制(MPC) www.youwenfan.com/contentcnt/160599.html
6. 使用说明
安装要求：
- ACADO Toolkit: 需要从官网下载并编译安装
- MATLAB: R2018b或更高版本
- 编译器: 支持C++编译（如Visual Studio, GCC）
运行步骤：
- 将上述代码保存为
main_acado_mpc_lane_following.m - 确保ACADO工具包已正确配置
- 在MATLAB中运行主程序：
main()
主要功能：
- 车道跟踪: 精确跟踪车道中心线
- 静态避障: 避开静态障碍物
- 动态避障: 避开移动障碍物
- 速度控制: 保持参考速度
- 平滑控制: 确保乘坐舒适性
可调参数：
- 预测时域
Np: 影响计算复杂度和前瞻性 - 权重系数: 调整跟踪精度与控制平滑性的平衡
- 安全距离: 调整避障的安全裕度
- 车辆参数: 适应不同车型
浙公网安备 33010602011771号