A*搜索算法简介

A*搜索算法是Greedy Best-First算法的改进，因为Greedy Best-First算法只考虑将预估距离（当前节点到目标点的预估距离）作为遴选开节点集的条件。即：
$p r i o r i t y = h e u r i s t i c (g o a l, n e x t)$
而A*算法考虑将起始点到当前节点的实际路径耗散也作为遴选的条件，因此有：
$p r i o r i t y = c o s t + h e u r i s t i c (g o a l, n e x t)$
一般又记为：
$f = g + h$
其中f为计算出来的遴选值，g为路径耗散，h为预估距离，也称为启发函数。

A*搜索算法的Python简化代码

这里强烈建议先看下这个网址里的内容，将算法的可视化做得十分形象：
Introduction to the A* Algorithm
其中给出的A*简化代码如下：

frontier = PriorityQueue()# 这是一个优先队列，get出priority值最小的元素并删除。
frontier.put(start, 0)# 初始化队列中start元素
came_from = dict()# 记录一个节点的父节点
cost_so_far = dict()# 记录一个节点的路径耗散
came_from[start] = None# 初始化start的父节点为None
cost_so_far[start] = 0# 初始化start节点的路径耗散为0

while not frontier.empty():# 只要frontier优先队列不为空
   current = frontier.get()# 从frontier中get一个priority值最小的子节点

   if current == goal:# 如果这个节点与goal重合则退出循环
      break
   
   for next in graph.neighbors(current):# 得到父节点的子节点next
      # 计算新的路径耗散
      new_cost = cost_so_far[current] + graph.cost(current, next)
      # 如果新的路径耗散小于之前记录的，则更新路径耗散表，如果路径耗散表没有该子节点的信息，则新加入该节点的路径耗散信息。
      if next not in cost_so_far or new_cost < cost_so_far[next]:
         cost_so_far[next] = new_cost # 更新g表
         priority = new_cost + heuristic(goal, next) # 获得新的优先值（因为g变了）
         frontier.put(next, priority) # 只要一个节点的路径耗散有更新，则其优先值同样需要更新。
         came_from[next] = current # 更新其父节点的信息。

我已将每一步的注释写在了上面的代码中，这是A*的算法框架，可以套用于任何问题。
完整Python代码建议参考github 1.4k stars项目：
zhm-real PathPlanning
这个项目包含了常用的所有规划算法的Python实现，并附有gif展示。
在这里插入图片描述
2D A*:

3D A*:

MATLAB实现

障碍物环境：
在这里插入图片描述
结果：

平滑后：

代码：

% A* algorithm实现
clc; clear; close all;
%% 参数读取与设置
obstacleMatrix = csvread("./data_csv/obstacleMatrix.csv");
RobstacleMatrix = csvread("./data_csv/RobstacleMatrix.csv")';
cylinderMatrix = csvread("./data_csv/cylinderMatrix.csv");
cylinderRMatrix = csvread("./data_csv/cylinderRMatrix.csv")';
cylinderHMatrix = csvread("./data_csv/cylinderHMatrix.csv")';
start = csvread("./data_csv/start.csv")';
goal = csvread("./data_csv/goal.csv")';
[numberOfSphere, ~] = size(obstacleMatrix);
[numberOfCylinder, ~] = size(cylinderMatrix);
Alldirec = [[1,0,0];[0,1,0];[0,0,1];[-1,0,0];[0,-1,0];[0,0,-1];...
            [1,1,0];[1,0,1];[0,1,1];[-1,-1,0];[-1,0,-1];[0,-1,-1];...
            [1,-1,0];[-1,1,0];[1,0,-1];[-1,0,1];[0,1,-1];[0,-1,1];...
            [1,1,1];[-1,-1,-1];[1,-1,-1];[-1,1,-1];[-1,-1,1];[1,1,-1];...
            [1,-1,1];[-1,1,1]];
threshold = 0.7;
stop = threshold*1.5;
g = [start, 0; goal, inf]; % 每一行前三个数为点坐标，第四个数为路径耗散
Path = [];
Parent = [];
Open = [start, g(findIndex(g,start),4) + getDist(start,goal)];
%% 绘制障碍环境
figure(1)
[n,~] = size(obstacleMatrix);
for i = 1:n   %绘制静态球障碍物
    [x,y,z] = sphere();
    surfc(RobstacleMatrix(i)*x+obstacleMatrix(i,1),...
        RobstacleMatrix(i)*y+obstacleMatrix(i,2),...
        RobstacleMatrix(i)*z+obstacleMatrix(i,3));
    hold on;
end

[n,~] = size(cylinderMatrix);
for i = 1:n   %绘制圆柱体障碍物
    [x,y,z] = cylinder(cylinderRMatrix(i));
    z(2,:) = cylinderHMatrix(i);
    surfc(x + cylinderMatrix(i,1),y + cylinderMatrix(i,2),...
        z,'FaceColor','interp');
    hold on;
end

bar1 = scatter3(start(1),start(2),start(3),80,"cyan",'filled','o');hold on
bar2 = scatter3(goal(1),goal(2),goal(3),80,"magenta",'filled',"o");
axis equal
set(gcf,'unit','centimeters','position',[30 10 20 15]);
%% 主循环
while ~isempty(Open)
    [xi, index] = findMin(Open);
    Open(index,:) = [];
    if getDist(xi, goal) < stop
        break;
    end
    children = getChildren(xi, Alldirec, threshold, obstacleMatrix, RobstacleMatrix,...
                           cylinderMatrix, cylinderRMatrix, cylinderHMatrix);
    scatter3(children(:,1),children(:,2),children(:,3),10,'filled','o');
    drawnow;
    [n,~] = size(children);
    for i = 1:n
        child = children(i,:);
        if findIndex(g, child) == 0   % child不在g
            g = [g; child, inf];
        end
        a = g(findIndex(g, xi),4) + getDist(xi,child);
        if a < g(findIndex(g, child),4)
            g(findIndex(g, child),4) = a;
            Parent = setParent(Parent, child,xi);
            Open = setOpen(Open, child, a, goal);
        end
    end  
end
lastPoint = xi;
%% 回溯轨迹
x = lastPoint;
Path = x;
[n,~] = size(Parent);
while any(x ~= start)
    for i = 1:n
        if Parent(i,1:3) == x
            Path = [Parent(i,4:6); Path];
            break;
        end
    end
    x = Parent(i,4:6);
end
plot3([Path(:,1);goal(1)],[Path(:,2);goal(2)],[Path(:,3);goal(3)],'LineWidth',3,'color','r');
%% 计算轨迹距离
pathLength = 0;
[n,~] = size(Path);
for i = 1:n-1
    pathLength = pathLength + getDist(Path(i,:),Path(i+1,:));
end
pathLength = pathLength + getDist(Path(end,:),goal);
fprintf('路径的长度为:%f',pathLength);
%% 函数
function children = getChildren(pos, Alldirec, step,circleCenter,circleR, cylinderCenter,cylinderR, cylinderH)
allchild = [];
[n,~] = size(Alldirec);
for i = 1:n
    direc = Alldirec(i,:);
    child = pos + direc * step;
    if ~checkCol(child, circleCenter,circleR, cylinderCenter,cylinderR, cylinderH)
        continue;
    end
    allchild = [allchild; child];
end
children = allchild;
end

function flag = checkCol(pos, circleCenter,circleR, cylinderCenter,cylinderR, cylinderH)
[numberOfSphere, ~] = size(circleCenter);
[numberOfCylinder, ~] = size(cylinderCenter);
flag = true;
for i = 1:numberOfSphere
    if getDist(pos, circleCenter(i,:)) <= circleR(i)
        flag = false;
        break;
    end
end
for i = 1:numberOfCylinder
    if getDist(pos(1:2), cylinderCenter(i,:)) <= cylinderR(i) && pos(3) <= cylinderH(i)
        flag = false;
        break;
    end
end
if pos(3) <= 0 flag = false; end
end

function Par = setParent(Parent, xj, xi)
[n,~] = size(Parent);
if n == 0
    Par = [xj, xi];
else
    for i = 1:n
        if Parent(i,1:3) == xj
            Parent(i,4:6) = xi;
            Par = Parent;
            break;
        end
        if i == n
            Par = [Parent; xj, xi];
        end
    end
end
end

function Ope = setOpen(Open, child, a, goal)
[n,~] = size(Open);
if n == 0
    Ope = [child, a + getDist(child, goal)];
else
    for i = 1:n
        if Open(i,1:3) == child
            Open(i,4) = a + getDist(child, goal);
            Ope = Open;
        end
        if i == n
            Ope = [Open; child, a + getDist(child, goal)];
        end
    end
end
end

function h = heuristic(pos, goal)
h = max([abs(goal(1) - pos(1)),abs(goal(2) - pos(2)),abs(goal(3) - pos(3))]);
end

function index = findIndex(g, pos)
[n,~] = size(g);
index = 0;    % 表示没有找到索引
for i = 1:n
    if g(i,1:3) == pos
        index = i;   % 索引为i
        break;
    end
end
end

function d = getDist(x,y)
d = sqrt(sum((x - y).^2));
end

function [pos, index] = findMin(Open)
[~,index] = min(Open(:,4));
pos = Open(index,1:3);
end

obstacleMatrix为球障碍物的圆心坐标矩阵（n*3矩阵），RobstacleMatrix为对应半径向量（n*1向量），cylinderMatrix为圆柱体中心坐标（n*2矩阵，没有第三个维度，从z=0开始绘制圆柱体），cylinderRMatrix为对应圆柱体半径（n*1向量），cylinderHMatrix为对应圆柱体的高（n*1向量）。

实现上的trick：
Python实现A*要比matlab实现起来容易，归根于其多样的数据结构，尤其是字典，在python中元组作为字典的键，优先值作为字典的值使得其实现异常容易。虽然matlab也有map这种映射数据结构，但是不支持以向量作为“键”，故放弃了这种思路，直接使用矩阵来实现。以路径耗散表g为例，采用n*4矩阵实现，其中每一行前三个值为节点的坐标，第四个值为节点的路径耗散值。这样实现起来需要自定义查找函数，但是思路比较简单，易于实现。

启发函数

欧氏距离
$h=\sqrt {(x_1-x_2)^2+(y_1-y_2)^2}$
曼哈顿距离
$h=|x_1-x_2|+|y_1-y_2|$
others
$h=max\{|x_1-x_2|,|y_1-y_2|\}$
本文采用的是第一种，我也写了第三种，可以直接修改函数名（将getDist改为heuristic）即可调用。

回溯轨迹

无论是RRT还是本文的A*，都需要回溯轨迹，在本文中采用的是一个Parent矩阵来实现，Parent矩阵是一个n*6的矩阵，每一行前三个值为子节点的坐标，后三个值为对应父节点的坐标。利用上面给出的代码中的这一段完成回溯：

x = lastPoint;
Path = x;
[n,~] = size(Parent);
while any(x ~= start)
    for i = 1:n
        if Parent(i,1:3) == x
            Path = [Parent(i,4:6); Path];
            break;
        end
    end
    x = Parent(i,4:6);
end
plot3([Path(:,1);goal(1)],[Path(:,2);goal(2)],[Path(:,3);goal(3)],'LineWidth',3,'color','r');

这里的any函数可以理解为检查数组中有没有非零元素，若有则返回1，否则为0，当x等于start时，x~=start结果为[0,0,0]，any的结果为0，其余所有情况为1，继续执行循环。循环内则是通过Parent寻找每一个节点的父节点，将其储存在Path矩阵中。

RRT算法对比

除了A*算法，RRT算法也做了一个demo，在搜索速度上，由于其基于采样的机制，快于A*，但是距离方面显然不太优秀。代码不附了，可以交流:
在这里插入图片描述

A*算法在三维路径规划中的一些不足

A*算法在三维路径规划中的使用必须对三维空间进行离散化，抽象出无数节点，对于一个父节点，其子节点是立方体除中心外的点：
在这里插入图片描述
当父节点与子节点之间的距离设置得较小时，由于节点之间的关联性很强（父节点同时可以作为其子节点的子节点），导致算法搜索缓慢。

对于三维路径规划A*算法属于边探索边优化，在执行程序前，并没有得到一个带有边长的节点图，其每个节点与起始点之间的路径耗散需要在程序执行过程中更新得到。也就是说从start开始到n节点之间有很多路径，不同的路径对应不同的路径耗散，A*需要找到该节点的最短路径耗散作为f=g+h中的g，因此节点会被多次探索，导致算法执行较慢。

从上图可以看到，一个父节点有26个子节点，节点数量庞大也导致A*在三维规划中存在一定问题。

A*搜索算法在三维路径规划（避障）中的MATLAB实现与Python实现

A star algorithm