A* 算法初试 matlab 版本/C++ 版本

最新推荐文章于 2021-05-24 13:18:58 发布

春暖夏凉

最新推荐文章于 2021-05-24 13:18:58 发布

阅读量301

点赞数

分类专栏：移动机器人文章标签： c++

本文链接：https://blog.csdn.net/weixin_42808534/article/details/106488893

版权

移动机器人专栏收录该内容

3 篇文章 0 订阅

订阅专栏

A* 算法基本概念

参考
A*算法是一种寻路算法，常常被用在游戏智能ai的自动寻路过程等等，它较之于图论的最短路算法而言，更加适用于节点巨大的情况下，但是该算法是一种启发式搜索算法，并不能保证总是找到最优路径。
参考这个人基本概念讲解很清楚来源

算法基本流程 A星算法伪码：

		a、将开始点记录为当前点P
		b、将当前点P放入封闭列表
		c、搜寻点P所有邻近点，假如某邻近点既没有在开放列表或封闭列表里面，则计算出该邻近点的F值，并设父节点为P，然后将其放入开放列表
		d、判断开放列表是否已经空了，如果没有说明在达到结束点前已经找完了所有可能的路径点，寻路失败，算法结束；否则继续。
		e、从开放列表拿出一个F值最小的点，作为寻路路径的下一步。
		f、判断该点是否为结束点，如果是，则寻路成功，算法结束；否则继续。
		g、将该点设为当前点P，跳回步骤c

非常像我们的BFS 搜索过程，可以说就是

matlab 验证程序参考来源此博主的代码应该比较正宗非常适合没有matlab 基础而又C++ 基础的人基本函数思想类似BFS 只是算法过程进行了可视化

一共分三个.m 文件
分别为 TestAstarandGij.m运行此代码就可以

 %
% TestScript for Assignment 1
%

%% Define a small map
map = false(10);

% Add an obstacle
map (1:5, 6) = true;

start_coords = [6, 2];
dest_coords  = [8, 10];

%%
close all;
 %[route, numExpanded] = DijkstraGrid (map, start_coords, dest_coords);
% Uncomment following line to run Astar
 [route, numExpanded] = AStarGrid (map, start_coords, dest_coords);

%HINT: With default start and destination coordinates defined above, numExpanded for Dijkstras should be 76, numExpanded for Astar should be 23.

A * 算法AStarGrid.m

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用一个map矩阵来表示每个点的状态
% 1 - white - clear cell
% 2 - black - obstacle
% 3 - red = visited 相当于CLOSED列表的作用
% 4 - blue  - on list 相当于OPEN列表的作用
% 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       %普通格设置为1
map(input_map)  = 2;   % Mark obstacle cells  %障碍物设置为2

% 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 坐标 方便求曼哈顿距离
[X, Y] = meshgrid (1:ncols, 1:nrows);

xd = dest_coords(1); % 目标点的 x值
yd = dest_coords(2); % 目标点的 y值

% 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);

% keep track of the number of nodes that are expanded
numExpanded = 0;

% Main Loop

while true
    
    % Draw current map
    map(start_node) = 5;
    map(dest_node) = 6;
    
    % make drawMapEveryTime = true if you want to see how the 
    % nodes are expanded on the grid. 
    if (drawMapEveryTime)
        image(1.5, 1.5, map);
        grid on;
        axis image;
        drawnow;
    end
    
    % Find the node with the minimum f value,其中的current是index值，需要转换
    [min_f, current] = min(f(:));
    
    if ((current == dest_node) || isinf(min_f))
        break;
    end;
    
    % Update input_map
    map(current) = 3;
    f(current) = Inf; % remove this node from further consideration
    numExpanded=numExpanded+1;
    % Compute row, column coordinates of current node
    
    % 计算每个格子的索引
    [i, j] = ind2sub(size(f), current);
    
    % *********************************************************************
    % ALL YOUR CODE BETWEEN THESE LINES OF STARS
    % Visit all of the neighbors around the current node and update the
    % entries in the map, f, g and parent arrays
    %
    action=[-1 0; 1 0; 0 -1; 0 1];%上，下，左，右
    for a=1:4
        expand=[i,j]+action(a,:);
        expand1=expand(1,1);
        expand2=expand(1,2);
        %不超出边界，不穿越障碍，不在CLOSED列表里，也不是起点，则进行扩展
         %  ~= 为不等于的意思  
        if ( expand1>=1 && expand1<=10 && expand2>=1 && expand2<=10 && map(expand1,expand2)~=2 && map(expand1,expand2)~=3 && map(expand1,expand2)~=5)
            if ( g(expand1,expand2)> g(i,j)+1 )
                g(expand1,expand2)= g(i,j)+1;
                f(expand1,expand2)= g(expand1,expand2)+H(expand1,expand2);
                parent(expand1,expand2)=current;
               
                map(expand1,expand2)=4;
            end
        end
    end
    %*********************************************************************
    
    
end

%% Construct route from start to dest by following the parent links

% 判断目标点是不是为无穷大  如果是则证明没有找到目标点 
if (isinf(f(dest_node)))
    route = [];
else
    disp('parent 矩阵');
    disp(parent);
    route = [dest_node];
    
    while (parent(route(1)) ~= 0)
        disp(route(1));
        route = [parent(route(1)), route];
        % 打印索引到命窗口
        disp(route);
    end

    % Snippet of code used to visualize the map and the path
    for k = 2:length(route) - 1        
        map(route(k)) = 7;
        pause(0.1);
        image(1.5, 1.5, map);
        grid on;
        axis image;
    end
end

end

附带 dijksta验证程序

function [route,numExpanded] = DijkstraGrid (input_map, start_coords, dest_coords)
% Run Dijkstra's 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;

% Initialize distance array
distanceFromStart = Inf(nrows,ncols);

% For each grid cell this array holds the index of its parent
parent = zeros(nrows,ncols);

distanceFromStart(start_node) = 0;

% keep track of number of nodes expanded 
numExpanded = 0;

% Main Loop
while true
    
    % Draw current map
    map(start_node) = 5;
    map(dest_node) = 6;
    
    % make drawMapEveryTime = true if you want to see how the 
    % nodes are expanded on the grid. 
    if (drawMapEveryTime)
        image(1.5, 1.5, map);
        grid on;
        axis image;
        drawnow;
    end
    
    % Find the node with the minimum distance
    [min_dist, current] = min(distanceFromStart(:));
    
    if ((current == dest_node) || isinf(min_dist))
        break;
    end;
    
    % Update map
    map(current) = 3;         % mark current node as visited
    numExpanded=numExpanded+1;
    % Compute row, column coordinates of current node
    [i, j] = ind2sub(size(distanceFromStart), current);
    
   % ********************************************************************* 
    % YOUR CODE BETWEEN THESE LINES OF STARS
    
    % Visit each neighbor of the current node and update the map, distances
    % and parent tables appropriately.
    action=[-1 0; 1 0; 0 -1; 0 1];%上，下，左，右
    for a=1:4
        expand=[i,j]+action(a,:);
        expand1=expand(1,1);
        expand2=expand(1,2);
        %不超出边界，不穿越障碍，不在CLOSED列表里，则进行扩展
        if ( expand1>=1 && expand1<=10 && expand2>=1 && expand2<=10 && map(expand1,expand2)~=2 && map(expand1,expand2)~=3 && map(expand1,expand2)~=5 )
            if ( distanceFromStart(expand1,expand2)> distanceFromStart(i,j)+1 )
                distanceFromStart(expand1,expand2)= distanceFromStart(i,j)+1;
                parent(expand1,expand2)=current;
                map(expand1,expand2)=4;
            end
        end
    end
    distanceFromStart(current) = Inf; % remove this node from further consideration
    %*********************************************************************

end

%% Construct route from start to dest by following the parent links
if (isinf(distanceFromStart(dest_node)))
    route = [];
else
    route = [dest_node];
    
    while (parent(route(1)) ~= 0)
        route = [parent(route(1)), route];
    end
    
        % Snippet of code used to visualize the map and the path
    for k = 2:length(route) - 1        
        map(route(k)) = 7;
        pause(0.1);
        image(1.5, 1.5, map);
        grid on;
        axis image;
    end
end

end

C++ 版本

代码参考 https://blog.csdn.net/u012234115/article/details/47152137
Astar.cpp

#include <math.h>
#include "Astar.h"
#include <iostream>


void Astar::InitAstar(std::vector<std::vector<int>>& _maze)
{
	maze = _maze;
}

int Astar::calcG(Point *temp_start,Point *point)
{
	int extraG = (abs(point->x-temp_start->x)+abs(point->y- point->y))==1?kCosntStraight:kConstSkew;
	int parentG = point->parent == NULL ? 0 : point->parent->G;

	return parentG + extraG;
}

int Astar::calcH(Point *point,Point *end)
{
	// 利用欧式距离计算这个H
	return sqrt((double)(end->x - point->x) * (double)(end->x - point->x) + (double)(end->y - point->y) * (double)(end->y - point->y));
}


int Astar::calcF(Point* point)
{
	return point->G + point->H;
}

Point* Astar::getLeastFpoint()
{
	if (!openList.empty())
	{
		auto resPoint = openList.front();
		for (auto& point : openList)
		{
			// 找f值最小的结点
			if (point->F < resPoint->F)
				resPoint = point;
		}
		return resPoint;
	}

	return NULL;
}
Point* Astar::findPath(Point& startPoint, Point& endPoint, bool isIgnoreCorner)
{
	openList.push_back(new Point(startPoint.x, startPoint.y));

	while (!openList.empty())
	{
		// 找到值最小的点
		auto curPoint = getLeastFpoint();
		openList.remove(curPoint);
		// 加入closelist 
		closeList.push_back(curPoint);

		//1,找到当前周围八个格中可以通过的格子
		auto surroundPoints = getSurroundPoints(curPoint, isIgnoreCorner);
		for (auto& target : surroundPoints)
		{
			//2,对某一个格子，如果它不在开启列表中，加入到开启列表，设置当前格为其父节点，计算F G H 
			if (!isInList(openList, target))
			{
				target->parent = curPoint;
				target->G = calcG(curPoint, target);
				target->H = calcH(target, &endPoint);
				target->F = calcF(target);
				openList.push_back(target);
			}
			//3，对某一个格子，它在开启列表中，计算G值, 如果比原来的大, 就什么都不做, 否则设置它的父节点为当前点,并更新G和F
			else
			{
				int tempG = calcG(curPoint, target);
				if (tempG < target->G)
				{
					target->parent = curPoint;
					target->G = tempG;
					target->F = calcF(target);
				}
			}

			Point* resPoint = isInList(openList, &endPoint);
			{
				if (resPoint)
					return resPoint;  //返回列表里的节点指针，不要用原来传入的endpoint指针，因为发生了深拷贝
			}
		}
	}
	
	return NULL;
}
std::vector<Point*> Astar::getSurroundPoints(const Point* point, bool isIgnoreCorner) const
{
	std::vector<Point*> surroundPoints;
	for (int x = point->x - 1; x <= point->x + 1; x++)
	{
		for (int y = point->y - 1; y <= point->y + 1; y++)
		{
			if (isCanreach(point, new Point(x, y), isIgnoreCorner))
			{
				surroundPoints.push_back(new Point(x,y));
			}
		}
	}

	return surroundPoints;
}

//判断某点是否可以用于下一步判断
bool Astar::isCanreach(const Point* point, const Point* target, bool isIgnoreCorner) const
{
	if (target->x<0 || target->x>maze.size() - 1
		|| target->y<0 || target->y>maze[0].size() - 1
		|| maze[target->x][target->y] == 1   // 1 代表是障碍物
		|| target->x == point->x && target->y == point->y
		|| isInList(closeList, target)
		)
		return false;
	else {
		if (abs(point->y - target->y) + abs(point->x-point->x) == 1)
			return true;
		else
		{
			//斜对角线
			if (maze[point->x][point->y] == 0 && maze[target->x][target->y] == 0)
				return true;
			else
				return isIgnoreCorner;
		}
	}
}

//判断开启 / 关闭列表中是否包含某点
Point* Astar::isInList(const std::list<Point*>& list, const Point* point) const
{
	for (auto p : list) {
		if (p->x == point->x && p->y == point->y)
			return p;
	}

	return NULL;
}


std::list<Point*> Astar::GetPath(Point& startPoint, Point& endPoint, bool isIgnoreCorner)
{
	Point* result = findPath(startPoint, endPoint, isIgnoreCorner);
	std::list<Point*> path;

	while (result)
	{
		path.push_back(result);
		result = result->parent;
	}

	openList.clear();
	closeList.clear();
	 
	return path;
}

Astar.h

#pragma once
#include<vector>
#include<list>

// 直接移动一格消耗
const int kCosntStraight = 10;

// 斜移移动消耗
const int kConstSkew = 14;

struct Point
{
	//  点坐标
	int x, y;
	int F, G, H;  //F=g+h
	Point* parent;
	Point(int _x, int _y)
		// 变量初始化
		:x(_x), y(_y), F(0), G(0), H(0),parent(NULL)
	{
	}
};
class Astar
{
public:
	void InitAstar(std::vector<std::vector<int>>& _maze);
	std::list<Point*> GetPath(Point& startPoint, Point& endPoint, bool isIgnoreCorner);

private:
	Point* findPath(Point& startPoint, Point& endPoint, bool isIgnoreCorner);

	std::vector<Point*> getSurroundPoints(const Point* point, bool isIgnoreCorner) const;
	bool isCanreach(const Point* point, const Point* target, bool isIgnoreCorner)const; //判断某点是否可以用于下一步判断
	Point* isInList(const std::list<Point*>& list, const Point* point) const ; //判断开启/关闭列表中是否包含某点
	Point* getLeastFpoint(); //从开启列表中返回F值最小的节点
	//计算FGH值
	int calcG(Point* temp_start, Point* point);
	int calcH(Point* point, Point* end);
	int calcF(Point* point);
private:
	std::vector<std::vector<int>> maze;
	std::list<Point*> openList;  //开启列表
	std::list<Point*> closeList; //关闭列表
};

testmain.c

#include <iostream>
#include "Astar.h"




/******************************************
*日期：2020/06/2
*来源https://blog.csdn.net/u012234115/article/details/47152137
*作者：
*
******************************************/
using namespace std;

int main()
{
	//初始化地图，用二维矩阵代表地图，1表示障碍物，0表示可通
	vector<vector<int>> maze = {
		{1,1,1,1,1,1,1,1,1,1,1,1},
		{1,0,0,1,1,0,1,0,0,0,0,1},
		{1,0,0,1,1,0,0,0,0,0,0,1},
		{1,0,0,0,0,0,1,0,0,1,1,1},
		{1,1,1,0,0,0,0,0,1,1,0,1},
		{1,1,0,1,0,0,0,0,0,0,0,1},
		{1,0,1,0,0,0,0,1,0,0,0,1},
		{1,1,1,1,1,1,1,1,1,1,1,1}
	};
	Astar astar;
	astar.InitAstar(maze);

	//设置起点和终点
	Point start(1, 1);
	Point end(6, 10);

	 // A*算法寻路

	list<Point*> path = astar.GetPath(start,end,false);

	// 反转起点到终点打印
	path.reverse();
	// 打印
	for (auto& p : path)
	{
		cout << "(" << p->x << "," << p->y << ")" << endl;
	}

	return 0;
}