船舶路径规划系列算法——A*算法

乘风云帆

于 2024-09-27 20:36:44 发布

阅读量240

点赞数 10

分类专栏：路径规划算法文章标签：算法动态规划

本文链接：https://blog.csdn.net/weixin_60630397/article/details/142601602

版权

路径规划算法专栏收录该内容

2 篇文章 0 订阅

订阅专栏

A*算法是一种广泛应用于路径规划的启发式算法，它基于启发式搜索和最优搜索策略，能够找到从起点到终点的最短路径。

1.A*算法原理

A*算法的核心思想是通过综合考虑从起点到当前节点的实际代价和从当前节点到目标节点的估计代价，从而在搜索过程中优先选择更有潜力接近目标节点的路径。具体来说， A*算法会根据评价函数为每个节点计算一个代价，计算方法如下。

f(n)= g(n)+ h(n)

g(n)：从起点到当前节点的实际代价，通常是路径长度或消耗。

h(n)：从当前节点到终点的估计代价（启发式函数），通常采用欧几里得距离或曼哈顿距离等。

f(n)：当前节点的总评价值，表示从起点经过当前节点再到目标节点的总代价的估计值。

2.A*算法步骤

步骤1：栅格化地图，设置障碍物及起始点

将地图划分为规则的网格（栅格），并标识每个网格是否为障碍物。设置起点和终点的位置

步骤2：创建开放列表和关闭列表

初始化开放列表（openlist）和关闭列表（closedlist）。将起点节点加入openlist，并将其g值设为0，父节点设为null

注：开放列表和关闭列表中不仅储存节点位置，还储存了g值、h值和f值

步骤3：选取openlist中代价最小的节点

从openlist中选择总代价f最小的节点，记为Pmin

步骤4：检查是否到达终点

判断Pmin是否为终点

如果是终点，则构建路径（从终点逐步向前追溯父节点，直到起点），输出路径并结束算法
如果不是终点，转到步骤5

步骤 5：更新开放列表和关闭列表

将 Pmin从openlist中移除，并加入closedlist

步骤 6：处理Pmin的邻居节点

步骤 6.1：筛选邻居节点

遍历 Pmin的所有邻居节点，逐个进行检查

如果邻居节点在网格范围外，或者是障碍物，或者已经存在于closedlist中，则跳过该邻居节点
否则，继续执行步骤 6.2

步骤 6.2：判断邻居节点是否在开放列表中

如果邻居节点不在openlist中，执行步骤 6.3
如果邻居节点在openlist中，执行步骤 6.4

步骤 6.3：添加新的邻居节点到开放列表

计算邻居节点的g值（等于Pmin的g值加上与邻居节点之间的代价）
计算邻居节点的h值（可使用曼哈顿距离或欧几里得距离作为启发式函数）
将邻居节点的父节点设置为Pmin，将邻居节点加入开放列表。

步骤 6.4：检查并更新邻居节点的代价

比较邻居节点当前存储g值（记为g1）与通过Pmin路径计算得到的g值（记为g2）
如果 g2 < g1，说明经过Pmin的路径更优，更新邻居节点的 g 值为 g2，更新邻居节点的父节点为Pmin

所有邻居节点检查完毕后，返回步骤3，循环执行，直到找到终点或开放列表为空（表示无可行路径）

3.实例验证

初始化网格，黑色为障碍物，绿色为起点，红色为终点，将起点放入openlist中（openlist使用蓝色填充，closedlist使用红色填充）

初始化

第1次迭代

第2次迭代

第3次迭代

第4次迭代

第5次迭代

第6次迭代

第7次迭代

第8次迭代

第9次迭代（得到路径）

4.主要代码

QList<QPoint> AStar::findPath(MainWindow* window)
{
    Node* startNode = new Node{startPoint, nullptr, 0, heuristic(startPoint, endPoint)};
    openList.append(startNode);// 将起点加入开放列表
    
    while (!openList.isEmpty())
    {
        // 获取f值最小的节点
        Node* currentNode = getLowestFCostNode(openList);
        // 如果到达终点，构建路径并返回
        if (currentNode->position == endPoint)
        {
            return reconstructPath(currentNode);
        }

        openList.removeOne(currentNode);
        closedList.append(currentNode);

        for (const QPoint& direction : directions)
        {
            QPoint neighborPos = currentNode->position + direction;
            // 检查邻居节点是否在网格内，是否为障碍物，或者是否已经在关闭列表中
            if (!isInBounds(neighborPos) || grid[neighborPos.y()][neighborPos.x()] == 1)
            {
                continue;
            }
            if (isInList(closedList, neighborPos))
            {
                continue;
            }
            int tentativeGCost = currentNode->gCost + 1; // 计算邻居节点的g值
            // 如果邻居节点不在开放列表中，创建它
            Node* neighborNode = findNodeInList(openList, neighborPos);
            if (neighborNode == nullptr)
            {
                neighborNode = new Node{neighborPos, currentNode, tentativeGCost, heuristic(neighborPos, endPoint)};
                openList.append(neighborNode);
            }

            else if (tentativeGCost < neighborNode->gCost)
            {
                neighborNode->gCost = tentativeGCost;
                neighborNode->parent = currentNode;//这一步很关键 保留父节点
            }
        }
    }

    return QList<QPoint>();
}