基于MATLAB实现A*路径规划并包含地图膨胀和路径平滑功能

一、核心代码实现

%% 1. 地图初始化与膨胀处理
function [expanded_map] = map_inflation(original_map, inflation_radius)
    % 使用形态学膨胀处理障碍物
    se = strel('square', inflation_radius*2);
    expanded_map = imdilate(original_map, se);
end

%% 2. A*路径规划算法
function path = AStar(start, goal, map)
    % 参数设置
    [rows, cols] = size(map);
    openList = struct('pos',{},{ 'g', {}, 'h', {}, 'f', {}, 'parent', {}});
    closedList = false(rows, cols);
    
    % 初始化起点
    openList(1).pos = start;
    openList(1).g = 0;
    openList(1).h = manhattan_distance(start, goal);
    openList(1).f = openList(1).g + openList(1).h;
    
    while ~isempty(openList)
        % 选择F值最小节点
        [~, idx] = min([openList.f]);
        current = openList(idx);
        openList(idx) = [];
        
        % 到达终点
        if isequal(current.pos, goal)
            path = reconstruct_path(current);
            return;
        end
        
        % 扩展邻居节点
        neighbors = get_neighbors(current.pos, rows, cols);
        for i = 1:length(neighbors)
            neighbor = neighbors(i,:);
            
            % 检查障碍物和封闭列表
            if map(neighbor(1), neighbor(2)) == 1 || closedList(neighbor(1), neighbor(2))
                continue;
            end
            
            % 计算新G值
            tentative_g = current.g + 1;
            if tentative_g < openList.g || isempty(openList)
                neighbor_node = struct('pos', neighbor, ...
                                      'g', tentative_g, ...
                                      'h', manhattan_distance(neighbor, goal), ...
                                      'f', tentative_g + openList.h, ...
                                      'parent', current);
                
                % 更新开放列表
                if isempty(openList) || ~is_node_in_list(openList, neighbor)
                    openList(end+1) = neighbor_node;
                else
                    idx_update = find_node(openList, neighbor);
                    openList(idx_update) = neighbor_node;
                end
            end
        end
        closedList(current.pos(1), current.pos(2)) = true;
    end
    path = []; % 无路径
end

%% 3. 路径平滑处理
function smooth_path = path_smoothing(raw_path, smooth_factor)
    % 三次B样条平滑
    t = linspace(0,1,length(raw_path)-3);
    control_points = raw_path(1:4:end,:);
    spline_matrix = build_bspline_matrix(control_points, 3);
    
    % 优化求解
    options = optimoptions('lsqnonlin','Display','off');
    smooth_t = lsqnonlin(@(x) bspline_error(x, spline_matrix, control_points), ...
                        ones(size(control_points)), [], [], options);
    
    % 生成平滑路径
    smooth_path = bspline_eval(control_points, smooth_t);
end

%% 4. 辅助函数
function h = manhattan_distance(p1, p2)
    h = abs(p1(1)-p2(1)) + abs(p1(2)-p2(2));
end

function neighbors = get_neighbors(pos, rows, cols)
    directions = [0,1;1,0;0,-1;-1,0;1,1;1,-1;-1,1;-1,-1];
    neighbors = [];
    for i = 1:size(directions,1)
        n = pos + directions(i,:);
        if n(1)>0 && n(1)<=rows && n(2)>0 && n(2)<=cols
            neighbors = [neighbors; n];
        end
    end
end

function path = reconstruct_path(node)
    path = [];
    while ~isempty(node)
        path = [node.pos; path];
        node = node.parent;
    end
end

---

二、完整实现流程

1. 地图生成与膨胀

% 创建原始地图
map_size = [50,50];
original_map = zeros(map_size);
original_map(10:40,25) = 1; % 中间障碍物

% 地图膨胀处理
inflation_radius = 3;
expanded_map = map_inflation(original_map, inflation_radius);

% 可视化
figure;
imagesc(expanded_map);
colormap([1 1 1; 0 0 0]);
axis equal;
title('膨胀后地图');

2. 路径规划与平滑

% 定义起点和终点
start = [5,5];
goal = [46,46];

% 执行A*算法
raw_path = AStar(start, goal, expanded_map);

% 路径平滑处理
smooth_factor = 0.8;
smoothed_path = path_smoothing(raw_path, smooth_factor);

% 可视化结果
figure;
hold on;
imagesc(expanded_map);
plot(raw_path(:,2), raw_path(:,1),'r-o','LineWidth',1.5);
plot(smoothed_path(:,2), smoothed_path(:,1),'b--s','LineWidth',2);
axis equal;
legend('原始路径','平滑路径');
title('A*路径规划与平滑效果');

---

三、关键参数优化

参数	作用范围	推荐值	影响效果
inflation_radius	障碍物膨胀范围	2-5	安全距离控制
smooth_factor	平滑强度	0.5-0.9	路径平滑度与保真度平衡
搜索步长	离散化精度	1-2	计算效率与路径精度

---

四、高级功能扩展

1. 动态障碍物处理

% 动态更新地图
function updated_map = dynamic_obstacle(map, moving_obstacle)
    updated_map = map;
    [x,y] = meshgrid(1:size(map,2),1:size(map,1));
    dist = sqrt((x-moving_obstacle(1)).^2 + (y-moving_obstacle(2)).^2);
    updated_map(dist < 3) = 1; % 动态障碍物半径3
end

2. 多目标路径规划

% 多目标路径规划
function paths = multi_target_planning(starts, goals, map)
    num_targets = length(goals);
    paths = cell(num_targets,1);
    for i = 1:num_targets
        paths{i} = AStar(starts(i,:), goals(i,:), map);
    end
end

参考代码 A*路径规划matlab代码，加上地图膨胀和路径平滑 www.youwenfan.com/contentcnq/64213.html

五、性能优化建议

1. 空间索引优化：使用KD-Tree加速邻居节点搜索

2. 并行计算：利用parfor加速多路径规划

3. 内存管理：采用稀疏矩阵存储开放/关闭列表

4. 启发式函数：根据场景选择欧氏距离或对角线距离

---

六、应用场景示例

1. 仓储机器人导航：处理货架间的狭窄通道

2. 自动驾驶：城市道路中的障碍物规避

3. 无人机航路规划：避开禁飞区并保持平滑轨迹

4. 工业机械臂：复杂工作空间内的运动规划