【路径规划】用于路径规划的星形算法（Matlab实现）

然哥依旧

于 2024-09-18 08:54:06 发布

阅读量92

点赞数 2

文章标签：算法 matlab 开发语言

本文链接：https://blog.csdn.net/m0_64583023/article/details/142323467

版权

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码实现

💥1 概述

星形算法（A* algorithm）是一种在路径规划中广泛应用的高效搜索算法。一、基本原理星形算法结合了启发式搜索和最佳优先搜索的优点。它通过维护一个开放列表和一个封闭列表来进行路径搜索。开放列表中存储待探索的节点，封闭列表中存储已探索过的节点。算法从起始节点开始，逐步向外扩展搜索，每次从开放列表中选择具有最小代价估计值的节点进行探索。二、代价估计星形算法的关键在于代价估计函数。该函数通常由两部分组成：从起始节点到当前节点的实际代价和从当前节点到目标节点的估计代价（启发式函数）。启发式函数的准确性对算法的效率有很大影响，一个好的启发式函数能够引导算法更快地找到最优路径。总之，用于路径规划的星形算法凭借其高效性、灵活性和通用性，成为了一种非常重要的路径规划工具。

📚2 运行结果

部分函数代码：

function [route,numExpanded] = AStarGrid (input_map, start_coords, dest_coords)
% Run A* algorithm on a grid.
% Inputs : 
%   input_map : a logical array where the freespace cells are false or 0 and
%   the obstacles are true or 1
%   start_coords and dest_coords : Coordinates of the start and end cell
%   respectively, the first entry is the row and the second the column.
% Output :
%    route : An array containing the linear indices of the cells along the
%    shortest route from start to dest or an empty array if there is no
%    route. This is a single dimensional vector
%    numExpanded: Remember to also return the total number of nodes
%    expanded during your search. Do not count the goal node as an expanded node. 

% set up color map for display
% 1 - white - clear cell
% 2 - black - obstacle
% 3 - red = visited
% 4 - blue  - on list
% 5 - green - start
% 6 - yellow - destination

cmap = [1 1 1; ...
    0 0 0; ...
    1 0 0; ...
    0 0 1; ...
    0 1 0; ...
    1 1 0; ...
    0.5 0.5 0.5];

colormap(cmap);

% variable to control if the map is being visualized on every
% iteration
drawMapEveryTime = true;

[nrows, ncols] = size(input_map);

% map - a table that keeps track of the state of each grid cell
map = zeros(nrows,ncols);

map(~input_map) = 1;   % Mark free cells
map(input_map)  = 2;   % Mark obstacle cells

% Generate linear indices of start and dest nodes
start_node = sub2ind(size(map), start_coords(1), start_coords(2));
dest_node  = sub2ind(size(map), dest_coords(1),  dest_coords(2));

map(start_node) = 5;
map(dest_node)  = 6;

% meshgrid will `replicate grid vectors' nrows and ncols to produce
% a full grid
% type `help meshgrid' in the Matlab command prompt for more information
parent = zeros(nrows,ncols);

% 
[X, Y] = meshgrid (1:ncols, 1:nrows);

xd = dest_coords(1);
yd = dest_coords(2);

% Evaluate Heuristic function, H, for each grid cell
% Manhattan distance
H = abs(X - xd) + abs(Y - yd);
H = H';
% Initialize cost arrays
f = Inf(nrows,ncols);
g = Inf(nrows,ncols);

g(start_node) = 0;
f(start_node) = H(start_node);