基于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*路径规划算法functionpath=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)=[];% 到达终点ifisequal(current.pos,goal)path=reconstruct_path(current);return;end% 扩展邻居节点neighbors=get_neighbors(current.pos,rows,cols);fori=1:length(neighbors)neighbor=neighbors(i,:);% 检查障碍物和封闭列表ifmap(neighbor(1),neighbor(2))==1||closedList(neighbor(1),neighbor(2))continue;end% 计算新G值tentative_g=current.g+1;iftentative_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);% 更新开放列表ifisempty(openList)||~is_node_in_list(openList,neighbor)openList(end+1)=neighbor_node;elseidx_update=find_node(openList,neighbor);openList(idx_update)=neighbor_node;endendendclosedList(current.pos(1),current.pos(2))=true;endpath=[];% 无路径end%% 3. 路径平滑处理functionsmooth_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. 辅助函数functionh=manhattan_distance(p1,p2)h=abs(p1(1)-p2(1))+abs(p1(2)-p2(2));endfunctionneighbors=get_neighbors(pos,rows,cols)directions=[0,1;1,0;0,-1;-1,0;1,1;1,-1;-1,1;-1,-1];neighbors=[];fori=1:size(directions,1)n=pos+directions(i,:);ifn(1)>0&&n(1)<=rows&&n(2)>0&&n(2)<=cols neighbors=[neighbors;n];endendendfunctionpath=reconstruct_path(node)path=[];while~isempty(node)path=[node.pos;path];node=node.parent;endend

二、完整实现流程

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([111;000]);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. 动态障碍物处理

% 动态更新地图functionupdated_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;% 动态障碍物半径3end

2. 多目标路径规划

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