Matlab实现自动驾驶轨迹规划算法解析

1. 无人车轨迹规划概述

在自动驾驶技术中，轨迹规划是最核心的算法模块之一。它负责根据环境感知信息，为车辆规划出一条从当前位置到目标位置的安全、高效行驶路径。基于代价函数的轨迹规划方法因其直观性和灵活性，在实际工程中得到了广泛应用。

这个Matlab实现的轨迹规划程序，采用了一种典型的基于目标函数和障碍函数的规划方法。其核心思想是通过构建合理的代价函数，将轨迹规划问题转化为一个优化问题。程序模拟了车辆在二维平面内的运动过程，能够有效避开障碍物并到达目标点。

提示：在实际自动驾驶系统中，轨迹规划通常分为全局规划和局部规划两个层次。本程序展示的是局部规划的基本原理，适合作为学习轨迹规划算法的入门案例。

2. 程序架构与核心算法

2.1 系统初始化模块

程序初始化阶段需要设置多个关键参数，这些参数直接影响规划效果：

matlab复制% 运动参数
step_size = 0.1;       % 单次移动步长(m)
num_sensors = 36;      % 感知方向数量(建议36-72)
sensor_radius = 1.5;   % 感知半径(m)
max_iterations = 200;  % 最大迭代次数

% 环境参数
vehicle_position = [0, 0];   % 车辆初始位置
goal_position = [5, 5];      % 目标位置
obstacles = [1.5 1.5;        % 障碍物坐标
             2.5 3.0;
             3.5 1.8];

参数选择经验：

• 步长过大可能导致"过冲"障碍物，过小则规划效率低
• 感知方向数量影响计算精度和耗时，36方向(10°间隔)是较好折中
• 感知半径应大于步长3-5倍，确保提前发现障碍

2.2 代价函数设计原理

程序使用两个核心代价函数：

1. 目标函数：引导车辆向目标移动

matlab复制function goal_value = goalfunction(current_pos, goal_pos)
    % 欧式距离度量
    dist = norm(current_pos - goal_pos);
    % 距离越近值越小
    goal_value = dist^2;  % 平方项增强目标导向性
end

2. 障碍函数：使车辆远离障碍

matlab复制function obstacle_value = obstaclefunction(sensor_point, obstacles)
    % 计算与所有障碍物的距离
    distances = vecnorm(sensor_point - obstacles, 2, 2);
    % 距离越近值越大(使用反比例函数)
    obstacle_value = sum(1./(distances + 0.1)); % 加0.1避免除零
end

注意：实际应用中，障碍函数常加入安全距离阈值，当距离小于安全值时函数值急剧增大，确保绝对避障。

2.3 运动决策逻辑

主循环中的决策流程体现了基于采样的规划思想：

matlab复制for iter = 1:max_iterations
    % 1. 边界检查
    if ~inpolygon(vehicle_position(1), vehicle_position(2), area_x, area_y)
        warning('车辆超出规划区域！');
        break;
    end
    
    % 2. 多方向感知
    sensor_angles = linspace(0, 2*pi, num_sensors);
    sensor_points = vehicle_position + sensor_radius * [cos(sensor_angles); 
                                                       sin(sensor_angles)]';
    
    % 3. 评估各方向代价
    total_cost = zeros(num_sensors, 1);
    for i = 1:num_sensors
        goal_cost = goalfunction(sensor_points(i,:), goal_position);
        obstacle_cost = obstaclefunction(sensor_points(i,:), obstacles);
        total_cost(i) = 0.7*goal_cost + 0.3*obstacle_cost; % 加权和
    end
    
    % 4. 选择最优方向
    [~, best_idx] = min(total_cost);
    best_direction = sensor_points(best_idx,:) - vehicle_position;
    unit_direction = best_direction/norm(best_direction);
    
    % 5. 执行移动(加入噪声模拟不确定性)
    noise = 0.02*randn(1,2); % 5%的步长噪声
    vehicle_position = vehicle_position + step_size*unit_direction + noise;
    
    % 记录轨迹
    trajectory(iter,:) = vehicle_position;
end

3. 算法优化与工程实践

3.1 代价函数改进方案

基础版本的代价函数存在几个可优化点：

1. 动态权重调整：

matlab复制% 根据距离动态调整目标与障碍的权重比例
dist_to_goal = norm(vehicle_position - goal_position);
if dist_to_goal < 2  % 接近目标时更注重避障
    w_goal = 0.4; w_obs = 0.6;  
else                % 远离目标时更注重前进
    w_goal = 0.8; w_obs = 0.2;
end

2. 障碍函数平滑化：

matlab复制function obstacle_value = improved_obstaclefunction(pos, obstacles)
    safe_dist = 0.5;  % 安全距离阈值
    distances = vecnorm(pos - obstacles, 2, 2);
    % 使用指数衰减函数
    obstacle_value = sum(exp(-(distances-safe_dist)/0.2).*(distances<safe_dist));
end

3.2 实时性优化技巧

为提高算法实时性，可采用以下方法：

1. 感知方向动态调整：

matlab复制% 根据运动方向集中采样
main_angle = atan2(goal_position(2)-vehicle_position(2), 
                   goal_position(1)-vehicle_position(1));
angle_range = pi/2;  % 90°范围
sensor_angles = linspace(main_angle-angle_range/2, 
                         main_angle+angle_range/2, 
                         num_sensors/2);

2. 多分辨率搜索：

• 首轮粗搜索(18方向)
• 在最优方向附近精细搜索(18方向)
• 总计算量不变，但局部精度提高

3.3 实际工程问题排查

常见问题及解决方案：

4. 算法扩展与应用

4.1 引入车辆动力学约束

实际车辆有转向半径限制，需在决策时考虑：

matlab复制% 计算各方向与当前航向的夹角
current_heading = atan2(vehicle_position(2)-prev_position(2),
                        vehicle_position(1)-prev_position(1));
angle_diffs = abs(wrapToPi(sensor_angles - current_heading));

% 剔除超出最大转向角的方向
max_steer = pi/6;  % 最大30°转向
valid_idx = angle_diffs <= max_steer;
sensor_points = sensor_points(valid_idx,:);

4.2 多障碍物场景处理

对于密集障碍物环境，可采用分层处理策略：

1. 首先识别关键障碍物(在前进方向上)
2. 对关键障碍物使用精细代价计算
3. 对其他障碍物使用粗略估算
4. 最终加权综合决策

4.3 与全局规划器集成

本局部规划器可与全局路径规划结合：

1. 全局规划器生成粗略路径点
2. 将路径点作为局部规划器的临时目标
3. 逐个点导航直至最终目标

这种架构既保证全局最优性，又能处理动态障碍物。

在真实项目中实现时，还需要考虑传感器噪声处理、定位误差补偿、实时性保证等工程问题。这个Matlab程序虽然简化，但完整呈现了基于代价函数的轨迹规划核心思想，是理解更复杂规划算法的基础。