环境感知与规划专题(一)——A*算法入门

209
0
2021年1月18日 09时16分

A*算法作为路径规划算法中应用最广泛实用的算法,由于A*算法的算法原理较为简单,网络上能够找到各种各样的学习资料,本文仅对其实现过程进行总结,同时,附上matlab仿真源码及结果图供参考学习。

 

算法实现过程

 

A*算法作为一种启发式的寻路算法被广泛应用于解决游戏中以及机器人、无人车、无人机的路径规划问题。该算法的步骤为:

 

1. 把起始节点添加到openList;

 

2. 重复如下步骤:

a) 寻找openList当中F值最低的节点,将其称为当前节点;

b) 将该节点从openList中删除,并加入closeList当中;

c) 对于当前节点相邻的8个节点

* 如果它不可通过或者已在closeList当中,忽略它,反之如下;

* 如果它不在openList当中,将其添加进去。把当前节点作为它的父节点。同时更新它的F,G和H值。

* 如果它已经在openList当中,通过判断沿当前节点到它的路径的G值是否更小,若更小,则将当前节点作为它的父节点,同时更新它的F和G值。否则,不做任何操作。

d) 当目标节点已添加到closeList时,路径已被找到;或者没有找到目标节点,此时openList已为空。这时候,路径不存在。以上两种情况结束循环。

 

3. 路径回归。从目标节点开始,沿着每一个节点的父节点移动至起始节点,形成的路径即为所求路径。

 

仿真源码

 

clear,clc

startPosition = [1 5];

goalPosition = [8 8];

findGoal = false;

map = [0 0 0 0 0 0 0 0 0 0;

      0 0 0 1 1 1 0 1 0 0;

      0 0 0 0 0 0 1 0 1 0;

      0 0 0 0 0 1 0 0 1 0;

      0 0 0 0 1 0 0 1 0 0;

      0 0 0 0 0 0 1 0 1 1;

      0 0 1 0 1 0 0 0 0 0;

      0 1 0 0 0 1 0 0 0 0;

      1 0 0 0 0 0 1 0 0 0;

      0 0 0 0 0 0 0 0 0 0;];

[mapRow, mapCol] = size(map);

closeList = struct('row', 0, 'col', 0, 'g', 0, 'h', 0, 'fartherNodeRow', 0, 'fartherNodeCol', 0);

closeListLength = 0;

openList = struct('row', 0, 'col', 0, 'g', 0, 'h', 0, 'fartherNodeRow', 0, 'fartherNodeCol', 0);

openListLength = 0;

direction = [0, -1; -1, -1; -1, 0; -1, 1; 0, 1; 1, 1; 1, 0; 1, -1;];

openList(1).row = startPosition(1);

openList(1).col = startPosition(2);

openListLength = openListLength + 1;

count = 0;

%找到目标或者开启列表已空时退出循环

while (openListLength > 0 || findGoal == true)

    count = count + 1;

%比较f值,更新当前格

    f = openList(1).g + openList(1).h;

    nodePosition = 1;

    for i = 1:openListLength

        if f > openList(i).g + openList(i).h

            f = openList(i).g + openList(i).h;

            nodePosition = i;

        end

    end



%将当前格加入关闭列表,并去掉开启列表中对应的节点

    closeListLength = closeListLength + 1;

    closeList(closeListLength) = openList(nodePosition);

    openList(nodePosition) = [];

    openListLength = openListLength - 1;

%    openListLength = 0;

    if closeList(closeListLength).row == goalPosition(1) && closeList(closeListLength).col == goalPosition(2) 

        findGoal = true;

        break;

    end



%检查相邻格并更新g、h值以及对应的父节点

    for i = 1: 8

        newPosition = [closeList(closeListLength).row, closeList(closeListLength).col] + direction(i, :);



%碰撞检测以及相邻点有效性检测

        if all(newPosition > 0) && newPosition(1) <= mapRow && newPosition(2) <= mapCol && map(newPosition(1), newPosition(2)) ~= 1

            if i == 2

                if map(newPosition(1) + 1, newPosition(2)) == 1 || map(newPosition(1), newPosition(2) + 1) == 1

                    continue;

                end

            end

            if i == 4

                if map(newPosition(1), newPosition(2) - 1) == 1 || map(newPosition(1) + 1, newPosition(2)) == 1

                    continue;

                end

            end

            if i == 6

                if map(newPosition(1), newPosition(2) - 1) == 1 || map(newPosition(1) - 1, newPosition(2)) == 1

                    continue;

                end

            end

            if i == 8

                if map(newPosition(1) - 1, newPosition(2)) == 1 || map(newPosition(1), newPosition(2) + 1) == 1

                    continue;

                end

            end           

            flagCloseList = false;

%关闭列表检测,若已在关闭列表则忽略           

            for j = 1 : closeListLength

                if closeList(j).row == newPosition(1) && closeList(j).col == newPosition(2)

                    flagCloseList = true;

                    break;

                end

            end



            if flagCloseList

                continue;

            end

%若通过检测且不在开启列表中,则将其加入开启列表,并更新父节点       

            flagOpenList = false;

            openListPosition = 0;

            for j = 1 : openListLength

                if openList(j).row == newPosition(1) && openList(j).col == newPosition(2)

                    flagOpenList = true;

                    openListPosition = j;

                    break;

                end

            end



            if flagOpenList == false

                openListLength = openListLength + 1;

                openList(openListLength).row = newPosition(1);

                openList(openListLength).col = newPosition(2);

                if i == 2 || i == 4 || i == 6 || i == 8

                    openList(openListLength).g = closeList(closeListLength).g + 1.4; 

                else

                    openList(openListLength).g = closeList(closeListLength).g + 1; 

                end



                openList(openListLength).h = abs(goalPosition(1) - openList(openListLength).row) + abs(goalPosition(2) - openList(openListLength).col);

                openList(openListLength).fartherNodeRow = closeList(closeListLength).row;

                openList(openListLength).fartherNodeCol = closeList(closeListLength).col;

            else

%若通过检测且已在开启列表中,根据相邻格在路径上的新G值判断是否需要更新相邻格的父节点并重新计算G与H值

                if openList(openListPosition).g > (closeList(closeListLength).g + 1)

                    openList(openListPosition).g = closeList(closeListLength).g + 1;

                    openList(openListPosition).fartherNodeRow = closeList(closeListLength).row;

                    openList(openListPosition).fartherNodeCol = closeList(closeListLength).col;

                end

            end                 

        end

    end

end

%起点终点着色

map(startPosition(1), startPosition(2)) = 128;

map(goalPosition(1), goalPosition(2)) = 70;

%路径回归

pathList = struct('row', 0, 'col', 0, 'g', 0, 'h', 0, 'fartherNodeRow', 0, 'fartherNodeCol', 0);

pathListLength = 0;

pathPosition = 0;

i = closeListLength;

while pathPosition ~= 1

    for j = 1 : closeListLength

        if(closeList(i).fartherNodeRow == closeList(j).row && closeList(i).fartherNodeCol == closeList(j).col)

            pathListLength = pathListLength + 1;

            pathList(pathListLength) = closeList(j);

            pathPosition = j;

            i = j;

            break;

        end 

    end

end

pathListLength = pathListLength + 1;

for i = 1 : pathListLength - 1

    pathList(pathListLength - i + 1) =  pathList(pathListLength - i);

end

pathList(1) = closeList(closeListLength);

%地图着色

for i = 1 : mapRow

    for j = 1 : mapCol

        if map(i,j) == 1

            map(i,j) = 250;

        end

        if map(i,j) == 0

            map(i,j) = 0;

        end

    end

end

%关闭列表着色

for i = 2:closeListLength - 1

    map(closeList(i).row, closeList(i).col) = 80;

    imagesc(map);

    pause(0.5);

end

%最终路径着色

for i = 1 : pathListLength

    map(pathList(pathListLength - i + 1).row, pathList(pathListLength - i + 1).col) = 160;

    imagesc(map);

    pause(0.5);

end

 

仿真结果

 

如图1所示,绿色方格为起始节点,蓝色方格为目标节点,黄色方格为障碍物。

 

微信图片_20210114134433

 

如图2所示,淡蓝色方格为算法迭代过程中,所有被添加至closeList中的节点,绿色为算法最终生成的最优路径。

 

微信图片_20210114134447

 

作者简介: 一个被Coding耽误的无人机算法工程师,控制、导航略懂一二,热衷技术,喜欢乒乓、音乐、电影,欢迎交流。

知乎:@遥远的乌托邦

GitHub: github.com/DistantUtopi

微信公众号:@遥远的乌托邦

发表评论

后才能评论