自动驾驶汽车自动规划避障(动态障碍物)和跟踪算法程序规划算法采用Emplanner 控制算法采用离线LQR算法仿真平台采用 simulink carsim prescan 三者联合仿真-CSDN博客

本文链接：https://blog.csdn.net/2501_90884015/article/details/147573914

自动驾驶汽车自动规划避障(动态障碍物)和跟踪算法程序
规划算法采用Emplanner 控制算法采用离线LQR算法
仿真平台采用 simulink carsim prescan 三者联合仿真
包含所有程序代码 simulink m文件联合仿真详细步骤文档 lqr emplanner算法 ST图速度规划笔记
硬件配置：
matlab2020a
prescan8.5
carsim2019.1
visual studio 2019
在这里插入图片描述

文章目录

自动驾驶汽车的动态障碍物避障和路径跟踪算法是实现智能驾驶的核心技术之一。以下是一个基于 动态窗口法 (Dynamic Window Approach, DWA) 和 纯追踪算法 (Pure Pursuit) 的完整实现方案，包括 MATLAB 代码和详细讲解。

1. 算法概述

1.1 动态窗口法 (DWA)

目标：在动态环境中规划一条安全、可行的轨迹。
步骤：
1. 采样速度空间：生成候选的速度和角速度组合。
2. 模拟轨迹：根据当前状态和候选控制输入，模拟未来一段时间内的车辆轨迹。
3. 评价函数：计算每条轨迹的代价，包括碰撞风险、与目标的距离、速度等。
4. 选择最优轨迹：选择代价最小的轨迹作为最终控制输入。

1.2 纯追踪算法 (Pure Pursuit)

目标：跟踪规划出的轨迹。
步骤：
1. 找到预瞄点（离车辆一定距离的目标点）。
2. 根据预瞄点计算前轮转角。
3. 控制车辆沿轨迹行驶。

2. MATLAB 实现

2.1 动态窗口法 (DWA)

function [v_opt, w_opt] = dwa_planning(x, goal, obstacles, config)
    % 输入参数：
    % x: 当前状态 [x, y, yaw, v, w]
    % goal: 目标位置 [x_goal, y_goal]
    % obstacles: 动态障碍物列表 [x1, y1; x2, y2; ...]
    % config: 配置参数结构体

    % 初始化
    v_min = config.v_min;
    v_max = config.v_max;
    w_min = config.w_min;
    w_max = config.w_max;
    dt = config.dt;
    predict_time = config.predict_time;

    best_traj = [];
    best_cost = inf;

    % 采样速度和角速度
    for v = v_min:config.v_reso:v_max
        for w = w_min:config.w_reso:w_max
            % 模拟轨迹
            traj = simulate_trajectory(x, v, w, predict_time, dt);
            
            % 计算代价
            to_goal_cost = calc_to_goal_cost(traj, goal);
            speed_cost = calc_speed_cost(traj, v_max);
            obstacle_cost = calc_obstacle_cost(traj, obstacles, config);

            cost = config.to_goal_weight * to_goal_cost + ...
                   config.speed_weight * speed_cost + ...
                   config.obstacle_weight * obstacle_cost;

            % 更新最优解
            if cost < best_cost
                best_cost = cost;
                best_traj = traj;
                v_opt = v;
                w_opt = w;
            end
        end
    end
end

function traj = simulate_trajectory(x, v, w, predict_time, dt)
    % 模拟轨迹
    traj = [];
    t = 0;
    while t <= predict_time
        x = motion_model(x, v, w, dt);
        traj = [traj; x];
        t = t + dt;
    end
end

function cost = calc_to_goal_cost(traj, goal)
    % 到目标的距离代价
    dx = goal(1) - traj(end, 1);
    dy = goal(2) - traj(end, 2);
    cost = sqrt(dx^2 + dy^2);
end

function cost = calc_speed_cost(traj, v_max)
    % 速度代价
    cost = v_max - traj(end, 4); % 偏离最大速度的惩罚
end

function cost = calc_obstacle_cost(traj, obstacles, config)
    % 障碍物代价
    min_dist = inf;
    for i = 1:size(traj, 1)
        for j = 1:size(obstacles, 1)
            dist = norm(traj(i, 1:2) - obstacles(j, :));
            if dist < min_dist
                min_dist = dist;
            end
        end
    end
    if min_dist <= config.robot_radius
        cost = inf; % 碰撞
    else
        cost = 1 / min_dist; % 距离越小，代价越大
    end
end

— 在这里插入图片描述

2.2 纯追踪算法 (Pure Pursuit)

function delta = pure_pursuit_control(x, trajectory, L, ld)
    % 输入参数：
    % x: 当前状态 [x, y, yaw, v]
    % trajectory: 参考轨迹 [x_ref, y_ref]
    % L: 车辆轴距
    % ld: 预瞄距离

    % 找到预瞄点
    for i = 1:size(trajectory, 1)
        dx = trajectory(i, 1) - x(1);
        dy = trajectory(i, 2) - x(2);
        dist = sqrt(dx^2 + dy^2);
        if dist >= ld
            target_point = trajectory(i, :);
            break;
        end
    end

    % 计算前轮转角
    alpha = atan2(target_point(2) - x(2), target_point(1) - x(1)) - x(3);
    delta = atan2(2 * L * sin(alpha), ld);
end

2.3 主程序

% 参数配置
config.v_min = -1; % 最小速度
config.v_max = 2;  % 最大速度
config.w_min = -1; % 最小角速度
config.w_max = 1;  % 最大角速度
config.v_reso = 0.1; % 速度分辨率
config.w_reso = 0.1; % 角速度分辨率
config.dt = 0.1; % 时间步长
config.predict_time = 3; % 预测时间
config.to_goal_weight = 1.0; % 到目标权重
config.speed_weight = 0.5; % 速度权重
config.obstacle_weight = 1.0; % 障碍物权重
config.robot_radius = 0.5; % 车辆半径

% 初始化
x = [0, 0, 0, 0, 0]; % 初始状态 [x, y, yaw, v, w]
goal = [10, 10]; % 目标位置
obstacles = [5, 5; 6, 6; 7, 7]; % 动态障碍物
trajectory = [];

% 主循环
for t = 0:config.dt:20
    % 动态窗口法规划
    [v_opt, w_opt] = dwa_planning(x, goal, obstacles, config);

    % 更新状态
    x = motion_model(x, v_opt, w_opt, config.dt);
    trajectory = [trajectory; x(1:3)];

    % 可视化
    clf;
    plot_trajectory(trajectory, goal, obstacles);
    drawnow;
end

function x = motion_model(x, v, w, dt)
    % 运动模型
    x(1) = x(1) + v * cos(x(3)) * dt;
    x(2) = x(2) + v * sin(x(3)) * dt;
    x(3) = x(3) + w * dt;
    x(4) = v;
    x(5) = w;
end

function plot_trajectory(trajectory, goal, obstacles)
    % 绘制轨迹
    plot(trajectory(:, 1), trajectory(:, 2), 'b', 'LineWidth', 2);
    hold on;
    plot(goal(1), goal(2), 'r*', 'MarkerSize', 10);
    for i = 1:size(obstacles, 1)
        rectangle('Position', [obstacles(i, 1)-0.5, obstacles(i, 2)-0.5, 1, 1], ...
                  'FaceColor', 'k');
    end
    axis equal;
    xlim([-1, 11]);
    ylim([-1, 11]);
    title('Dynamic Obstacle Avoidance and Path Tracking');
end