路径规划算法Dijkstra和A*

最新推荐文章于 2024-04-30 11:40:51 发布

L Ryan

最新推荐文章于 2024-04-30 11:40:51 发布

阅读量850

点赞数 10

文章标签：算法 c++ 数据结构自动驾驶

本文链接：https://blog.csdn.net/Ryan_LeeL/article/details/135576583

版权

外部环境变化太快，又到了找工作的“期末考”了，复习一下路径规划算法 Dijkstra和A*。把两者的简版代码放一起便于对比。

Dijkstra算法

参考了以下文章详解，自己手敲了一遍C++代码加深印象。

参考链接：

狄克斯特拉(Dijkstra)算法详解-CSDN博客

#include <iostream>
#include <vector>
#include <queue>

using namespace std;

// 定义一个节点结构体
struct Node {
    int name;
    int cost;
    
    //bool operator<(const Node& other) const {
    //    return cost > other.cost; // 用于优先队列中节点的比较
    //}
    /*这里不用优先队列是因为需要更新队列中的节点开销，查找并修改队列中某元素比较麻烦。
    所以直接用数组存储并通过组合判断closedlist和openlist来确定花销最少的点*/
};

const int N = 6;
const int M = 6;

// https://blog.csdn.net/wopelo/article/details/125471811 乐谱换钢琴问题
int linecost[N][M] = {
    {-1, -1, -1, -1, -1, -1},
    {5,  -1, -1, -1, -1, -1},
    {0, -1, -1, -1, -1, -1},
    {-1, 15, 30, -1, -1, -1},
    {-1, 20, 35, -1, -1, -1},
    {-1, -1, -1, 20, 10, -1}
};

// 定义一个用于存储节点是否被访问过的数组
bool openlist[N];

// 定义一个用于存储起点到每个节点的花销
int costs[N];

// 定义一个用于存储每个节点的父节点的数组
Node parent[N];

// 定义一个每个节点是否已经处理完毕的数组（后继节点邻居都访问过）
bool closedlist[N];

vector<int> getNeighbor(Node node) {
    vector<int> res;
    int curname = node.name;
    for (int i = 0; i < M; i++) {
        if (linecost[i][curname] > -1) {
            res.emplace_back(i);
        }
    }
    return res;
}

// 从openlist里面找到花销最少的点
Node findLowerCostNode() {
    int lowestcost = INT_MAX;
    Node key = { -1, -1 };
    for (int i = 0; i < N; i++) {
        if (!closedlist[i] && openlist[i] && costs[i] <= lowestcost) {
            lowestcost = costs[i];
            key = { i, costs[i] };
        }
    }
    return key;
}

bool isOpenlistEmpty() {
    for (int i = 0; i < N; i++) {
        if (!closedlist[i] && openlist[i]) return false;
    }
    return true;
}

// Dijkstra算法
void Dijkstra() {
    // 初始化起点
    Node start = { 0, 0 };
    openlist[0] = true;
    costs[0] = 0;
    closedlist[0] = false;

    // 开始搜索
    while (!isOpenlistEmpty()) {
        Node cur = findLowerCostNode();
        vector<int> neighbors;
        neighbors = getNeighbor(cur);
        for (auto neighbor : neighbors) {
            int newcost = costs[cur.name] + linecost[neighbor][cur.name];
            if (!openlist[neighbor]) {
                openlist[neighbor] = true;
                costs[neighbor] = newcost;
                parent[neighbor] = cur;
            }
            else {
                if (newcost < costs[neighbor]) {
                    costs[neighbor] = newcost;
                    parent[neighbor] = cur;
                }
            }
        }
        closedlist[cur.name] = true;
    }
}

// 输出地图和路径
void print() {
    Node cur = { 5, 0, };
    while (cur.name != 0) {
        cout << cur.name << " <- ";
        cur = parent[cur.name];
    }
    cout << cur.name << endl;
}

int main() {
    Dijkstra();
    print();
    return 0;
}

A*算法

总代价f = g (历史代价) + h (引导)

g：每个节点到起点的距离 + extracost (这里故意加了个到达每个节点的额外开销模拟工程中的其他一些代价值，如自动泊车系统中的曲率或规划次数)

h：每个节点到终点的距离，主要有欧氏距离和曼哈顿距离，曼哈顿距离导致低估，欧式距离导致高估，所以工程上有时候是结合二者，给个权重。这里仅使用曼哈顿距离。

求从Map左上角到右下角的最短路径，其中，Map中的数值1表示障碍物，数值0表示无障碍物。

#include <iostream>
#include <vector>
#include <queue>
#include <cmath>

using namespace std;

#define INF INT_MAX
#define NRAND 100

// 定义一个节点结构体
struct Node {
    int x; // 节点的横坐标
    int y; // 节点的纵坐标
    double g; // 距离起点的距离
    double h; // 距离终点的估计距离
    bool operator<(const Node& other) const {
        return g + h > other.g + other.h; // 用于优先队列中节点的比较
    }
};

// 定义地图大小和起点终点坐标
const int N = 6;
const int M = 6;
int sx = 0, sy = 0; // 起点坐标
int tx = N - 1, ty = M - 1; // 终点坐标

// 定义地图
//int map[N][M] = {
//    {0, 0, 0, 0, 0},
//    {0, 1, 1, 0, 0},
//    {0, 0, 1, 0, 0},
//    {0, 0, 1, 1, 0},
//    {0, 0, 0, 0, 0}
//};
int map[N][M] = {
    {0, 0, 1, 0, 0, 0},
    {0, 1, 0, 0, 1, 0},
    {0, 0, 0, 1, 0, 0},
    {0, 1, 0, 1, 0, 0},
    {0, 0, 0, 1, 1, 0},
    {0, 0, 0, 0, 0, 0}
};

// 定义一个用于存储路径的二维数组
int path[N][M];

// 定义一个用于存储节点是否被访问过的二维数组
bool closedlist[N][M];

// 定义一个用于存储节点是否被访问过的二维数组
bool openlist[N][M];

// 定义一个用于存储起点到每个节点的距离的二维数组
double dist[N][M];

// 定义一个用于存储每个节点的二维数组
Node curnode[N][M];

// 定义一个用于存储每个节点的父节点的二维数组
Node parent[N][M];

// 定义一个extracost
double extracost[N][M];

// 计算两个节点之间的曼哈顿距离
double manhattan(Node a, Node b) {
    return fabs(a.x - b.x) + fabs(a.y - b.y);
}

// 计算两个节点之间的欧氏距离
int eulerDist(Node a, Node b) {
    return sqrt((a.x - b.x)^2 + (a.y - b.y)^2);
}

// initial
void initial() {
    for (int i = 0; i < N; i++) {
        for (int j = 0; j < M; j++) {
            dist[i][j] = INF;
        }
    }
    cout << "extracost : " << endl;
    for (int i = 0; i < N; i++) {
        for (int j = 0; j < M; j++) {
            if (i < 2) {
                extracost[i][j] = (double(rand() % (NRAND + 1)) / 100) * 1;
            }
            else {
                extracost[i][j] = 0;
            }
            
            cout << extracost[i][j] << " ";
        }
        cout << endl;
    }
    cout << endl;

}

// A*算法
void Astar() {
    // 定义一个优先队列用于存储待访问的节点
    priority_queue<Node> q;

    // 初始化起点
    Node start = { sx, sy, 0.0, manhattan({sx, sy}, {tx, ty}) };
    // Node start = { sx, sy, 0, eulerDist({sx, sy}, {tx, ty}) };
    q.push(start);
    dist[sx][sy] = 0.0;
    closedlist[sx][sy] = true;
    openlist[sx][sy] = true;
    curnode[sx][sy] = start;


    // 开始搜索
    while (!q.empty()) {
        Node cur = q.top();
        q.pop();
        // 如果当前节点是终点，则搜索结束
        if (cur.x == tx && cur.y == ty) {
            break;
        }
        // 遍历当前节点九宫格的八个相邻节点
        for (int i = -1; i <= 1; i++) {
            for (int j = -1; j <= 1; j++) {
                if (i == 0 && j == 0) continue;
                int nx = cur.x + i;
                int ny = cur.y + j;
                // 如果相邻节点在地图内且没有被访问过且不是障碍物
                if (nx >= 0 && nx < N && ny >= 0 && ny < M && !closedlist[nx][ny] && map[nx][ny] == 0) {
                    // 计算相邻节点到起点的距离
                    double ndist = cur.g + ((i == 0 || j == 0) ? 1.0 : sqrt(2)) + extracost[nx][ny];
                    // int ndist = cur.g + ((i == 0 || j == 0) ? 1 : sqrt(2));
                    // 计算相邻节点到终点的距离
                    double nh = manhattan({ nx, ny }, { tx, ty });
                    // int nh = eulerDist({ nx, ny }, { tx, ty });
                    // 将未探索过的相邻节点加入openlist优先队列
                    if (!openlist[nx][ny]) {
                        dist[nx][ny] = ndist;
                        curnode[nx][ny] = { nx, ny, ndist, nh };
                        parent[nx][ny] = cur;
                        q.push({ nx, ny, ndist, nh });
                        openlist[nx][ny] = true;  
                    }
                    // 若已经由其他节点探索过，比较OldParent和CurParent那个的总代价f更小
                    else {
                        if (ndist + nh < curnode[nx][ny].g + curnode[nx][ny].h) {
                            dist[nx][ny] = ndist;
                            curnode[nx][ny] = { nx, ny, ndist, nh };
                            parent[nx][ny] = cur;
                            cout << "The cost by cur parent is less than old parent!" << endl;
                        }
                        
                    }   
                }
            }
        }
        // 标记当前节点已经被访问过，加入ClosedList
        closedlist[cur.x][cur.y] = true;
    }

    // 回溯路径
    Node cur = { tx, ty, 0.0, 0.0 };
    int k = 1;
    while (cur.x != sx || cur.y != sy) {
        path[cur.x][cur.y] = k++;
        cur = parent[cur.x][cur.y];
    }
    path[sx][sy] = k;
}

// 输出地图和路径
void print() {
    for (int i = 0; i < N; i++) {
        for (int j = 0; j < M; j++) {
            if (map[i][j] == 1) {
                cout << "X ";
            }
            else if (path[i][j] != 0) {
                cout << "* ";
            }
            else {
                cout << ". ";
            }
        }
        cout << endl;
    }
    cout << endl;
    // print path
    for (int i = 0; i < N; i++) {
        for (int j = 0; j < M; j++) {
            cout << path[i][j] << " ";
        }
        cout << endl;
    }
    cout << endl;
    for (int i = 0; i < N; i++) {
        for (int j = 0; j < M; j++) {
            cout << curnode[i][j].g + curnode[i][j].h << " ";
        }
        cout << endl;
    }
}

int main() {
    initial();
    Astar();
    print();
    return 0;
}

个人理解

相同之处：Dijkstra和A*算法很像，都是求解起点到终点的最短路径，都是每一步遍历一遍当前节点的邻居节点，记录下第一次遍历到的邻居节点的cost，或者是该邻居节点已经被遍历过（inOpenList），则比较该邻居节点之前记录的cost跟当前节点作为父节点的cost，如果当前节点作为父节点的cost更小的话就更新该邻居节点的cost值和父节点。当前节点的邻居节点都被访问过了，则放进ClosedList里面，不再重复检查。二者都可以采用优先队列存储OpenList里面的节点，每次把cost最小的值弹出，并访问其所有邻居节点。

不同之处：A*的总代价f包含两个部分，历史代价g和引导h，f = g + h。使得A*能够一直朝着引导h的方向拓展路径点。而Dijkstra算法从起点开始，每次保持与起点相同的距离一层一层地向外遍历，可以理解为Dijkstra的引导h = 0，始终保持与起点最近距离的方向拓展。