深蓝学院自主泊车第7次作业-轨迹规划

原创

已于 2025-03-28 00:44:10 修改 · 1.2k 阅读

于 2025-03-09 00:10:13 首次发布

1 题目

对于本次作业，您需要完成AVP系统的规划算法。您需要完成BFS/A*/Hybrid A*算法的功能，并显示最终的规划结果。请完成src/planner.cpp中的Bfs()、AStar()和HybridAStar()函数。

秦老师课上演示点位，搜索路径示例如下图所示。其中绿色路径表示bfs搜索得到的路径，红色路径表示AStar搜索得到的路径，黄色路径表示Hybrid AStar搜索得到的路径！

在这里插入图片描述

2 解答

2.1 bfs

使用队列完成bfs搜索，需要注意的是：
(1) 扩展结点时，上下左右移动的优于斜对角移动。本质是因为它们的边权不一样，前者是1.0，后者是 $\sqrt{2}$ 。
(2) 访问过的结点不再访问，用unordered_set<Vector2i, Vector2iHash> visited来记录。

C++代码如下，

vector<Vector2i> Planner::Bfs(const Vector2i &start, const Vector2i &goal,
                     const unordered_set<Vector2i, Vector2iHash>& obstacles) {
   
   
    //起点start,终点goal,障碍物obstacles
    //计算最短路,并把路径存储在res中,最终返回res

    std::cout << "=====start bfs planning=====" << std::endl;

    if (obstacles.find(goal) != obstacles.end()) {
   
    //特判终点落在障碍物上的情况
        return vector<Vector2i>(); 
    }

    unordered_set<Vector2i, Vector2iHash> visited;
    queue<Vector2i> q;
    q.push(start);
    visited.insert(start);
    unordered_map<Vector2i, Vector2i, Vector2iHash> fa; //记录父结点
    while (!q.empty()) {
   
   
        Vector2i t = q.front();
        q.pop();

        if (t == goal) {
   
   
            break; //提前退出 
        }

        //t能走到哪些结点 
        int x = t.x();
        int y = t.y();
        //注意对角线移动的代价不等于上下左右移动的代价 
        //bfs是等边权,只能采取4邻域
        //对角线的扩展可以放在数组dirs的后面，表示不优先考虑它。但这样其实与"bfs求解等权边最短路"相矛盾。
        //int dirs[4][2] = {
   
   {-1,0}, {1,0}, {0,-1}, {0,1}}; 
        int dirs[8][2] = {
   
   {
   
   -1,0}, {
   
   1,0}, {
   
   0,-1}, {
   
   0,1}, {
   
   -1,-1}, {
   
   -1,1}, {
   
   1,-1}, {
   
   1,1}}; 
        for (int k = 0; k < 8; ++k) {
   
   
            int nx = x + dirs[k][0];
            int ny = y + dirs[k][1];
            Vector2i node(nx,ny);
            if (obstacles.find(node) != obstacles.end()