基于MATLAB的自动驾驶车辆轨迹跟踪程序实现

一、系统架构设计

轨迹跟踪系统分为四大核心模块，结合了纯追踪算法、模型预测控制（MPC）和动态窗口法（DWA）的混合控制策略：

1. 路径生成模块 支持双移线、圆形、五次多项式等多种路径生成方式 提供动态障碍物模拟功能（基于的DWA算法）

2. 控制器设计 纯追踪算法（Pure Pursuit）：处理全局路径跟踪（基于） MPC控制器：实现局部轨迹优化（基于） DWA动态避障：实时避障策略（基于）

3. 车辆动力学模型

   • 采用自行车模型：

• 支持四轮驱动/转向动力学扩展

1. 仿真与可视化 实时轨迹绘制与误差分析 控制输入平滑度监测

二、核心代码实现

1. 纯追踪算法（Pure Pursuit）

function [delta, target_idx] = pure_pursuit(waypoints, pose, Ld)
    % 输入: waypoints - 路径点(Nx2), pose - [x,y,ψ], Ld - 前视距离
    x = pose(1); y = pose(2); ψ = pose(3);

    % 寻找目标点
    distances = sqrt((waypoints(:,1)-x).^2 + (waypoints(:,2)-y).^2);
    [~, target_idx] = min(abs(distances - Ld));
    target = waypoints(target_idx,:);

    % 计算转向角
    α = atan2(target(2)-y, target(1)-x) - ψ;
    delta = atan(2*L* sin(α)/Ld);  % L为轴距
end

2. MPC轨迹跟踪控制器

function [u_opt] = mpc_controller(x0, ref_traj, N, dt)
    % 参数设置
    Q = diag([10, 10, 1]);  % 状态权重
    R = 0.1;                % 控制权重

    % 构建优化问题
    opti = opti('f', 0, 'variables', {
   'u', 'x'}, 'lb', [-30, -30, -0.5, -0.5], ...
                'ub', [30, 30, 0.5, 0.5]);

    % 状态方程
    for k = 1:N
        x_next = x(k,:) + dt*[0,0,cos(x(k,3))*u(k,1), sin(x(k,3))*u(k,1)];
        opti.subject(@(u,x) x_next == vehicle_dynamics(x, u));
    end

    % 目标函数
    opti.minimize(sum(sum((x - ref_traj).^2 * Q + u.^2 * R)));

    % 求解
    sol = opti.solve();
    u_opt = sol.u;
end

3. 动态窗口法（DWA）避障

function [v_opt, w_opt] = dwa_control(x, goal, obstacles)
    % 参数配置
    v_min = 0; v_max = 5;
    w_min = -0.5; w_max = 0.5;
    dt = 0.1; predict_time = 3;

    % 速度空间采样
    v_samples = linspace(v_min, v_max, 20);
    w_samples = linspace(w_min, w_max, 40);

    % 轨迹评估
    best_score = inf;
    for v = v_samples
        for w = w_samples
            traj = simulate_trajectory(x, v, w, predict_time, dt);
            score = calculate_cost(traj, goal, obstacles);
            if score < best_score
                best_score = score;
                v_opt = v; w_opt = w;
            end
        end
    end
end

三、完整仿真流程

%% 参数初始化
L = 2.5;        % 车辆轴距
dt = 0.05;      % 时间步长
max_time = 20;  % 仿真时长

%% 路径生成（双移线）
t = linspace(0, 2*pi, 100);
x_path = t; y_path = sin(t);
waypoints = [x_path', y_path'];

%% 车辆动力学模型
function x_next = vehicle_dynamics(x, u)
    L = 2.5; 
    x_next = x + [u(1)*cos(x(3))*dt, 
                  u(1)*sin(x(3))*dt, 
                  u(2)*dt, 
                  u(1)/L * tan(u(2)) * dt];
end

%% 主循环
current_pose = [0, 0, 0, 0](@ref);  % [x,y,ψ,v]
trajectory = [];
for t = 0:dt:max_time
    % 获取参考轨迹（前10个路径点）
    ref_traj = waypoints(1:min(end,10),:);

    % MPC控制
    u_mpc = mpc_controller(current_pose, ref_traj, 10, dt);

    % DWA动态避障
    [v_opt, w_opt] = dwa_control(current_pose, waypoints(end,:), []);

    % 选择最优控制
    u = [mean([u_mpc(1), v_opt]), mean([u_mpc(2), w_opt])];

    % 更新状态
    current_pose = vehicle_dynamics(current_pose, u);
    trajectory = [trajectory; current_pose(1:2)];

    % 可视化
    plot(waypoints(:,1), waypoints(:,2),'b', trajectory(:,1), trajectory(:,2),'r');
    drawnow;
end

参考代码 自动驾驶车辆轨迹跟踪程序 www.youwenfan.com/contentalf/63662.html

四、优化

1. 并行计算加速

   parfor k = 1:N
       % 并行执行轨迹模拟
   end
   

2. GPU加速方案

   % 将轨迹数据迁移至GPU
   traj_gpu = gpuArray(traj);
   

3. 自适应采样

   % 根据曲率动态调整控制采样频率
   curvature = abs(2*waypoints(i+1,:) - waypoints(i,:) - waypoints(i-1,:)) / ...
                (1 + (diff(waypoints(i,:) - waypoints(i-1,:))).^2)^1.5;
   dt_adaptive = dt / (1 + curvature);
   

五、工程应用扩展

1. 硬件在环测试 通过Simulink Coder生成C代码部署到dSPACE平台 使用CarSim进行硬件协同仿真（参考的联合仿真步骤）

2. 多传感器融合

   % 激光雷达+视觉SLAM融合
   lidar_data = readLidarData();
   vision_data = processCameraImage();
   fused_data = sensorFusion(lidar_data, vision_data);
   

3. 安全冗余设计 双控制器热备份架构 故障诊断模块实时监测传感器失效