路径规划算法BFS/Astar/HybridAstar简单实现

借鉴本文所述代码简单实现一下BFS，Astar和HybridAstar路径规划算法，用于辅助理解算法原理。
代码在这里，画图用到了matplotlibcpp库，需要先装一下，然后直接在文件目录下执行如下代码即可运行：

mkdir build
cd build
cmake ..
make
./HAstar

1. 场景

这里没考虑朝向

start_pose: 6, 10         // 起始点和朝向
end_pose: 90, 90          // 目标点和朝向
boundary: 0, 100, 0, 100  // 整个场景的边界（这里没有使用）
obstacle: 0, 0, 0, 100, 100, 100, 100, 0  // 整个场景的边界，四个顶点，顺时针组成一个四边形
obstacle: 12, 12, 12, 36, 30, 36, 30, 12  // 障碍物（下面都是），四个顶点，顺时针组成一个四边形
obstacle: 20, 50, 20, 80, 40, 80, 40, 50  
obstacle: 50, 6, 50, 60, 60, 60, 60, 6
obstacle: 60, 70, 60, 95, 85, 95, 85, 70
obstacle: 75, 20, 75, 50, 95, 50, 90, 20

网格大小取0.2（可以选其他值，可能会有不同的效果），原点处网格坐标为（0，0），将场景信息都转化成网格坐标，场景示意图如下图所示，其中红色圆点是起点，绿色圆点是终点：

2. BFS

std::vector<Vec2i> bfs(const Scenario& scenario,
                     const std::unordered_set<Vec2i, Vec2iHash>& obstacles) {

    // 每个节点的查找方向，左上右下
    std::vector<Vec2i> neighbors{Vec2i{-1,0}, Vec2i{0,1}, Vec2i{1,0}, Vec2i{0,-1}}; 
    // 每个节点的查找方向，8个方向
    //vector<Vec2i> neighbors{Vec2i{-1,1}, Vec2i{0, 1}, Vec2i{1, 1},
    //                           Vec2i{1,0}, Vec2i{1,-1}, Vec2i{0,-1}, 
    //                           Vec2i{-1,-1}, Vec2i{-1,0}};                   
    
    std::vector<Vec2i> path; // 路径结果
    std::queue<Vec2i> open_nodes; // 待check的节点
    std::unordered_map<std::string, Vec2i> pre_node; // 每个节点的父节点
    bool isvalid = true;

    Vec2i start{static_cast<int>(scenario.start_pos.x/RES_GRID), 
                static_cast<int>(scenario.start_pos.y/RES_GRID)};
    Vec2i goal{static_cast<int>(scenario.end_pos.x/RES_GRID), 
               static_cast<int>(scenario.end_pos.y/RES_GRID)};
    
    std::string goal_name = std::to_string(goal.x)+"_" + std::to_string(goal.y);
    std::string start_name = std::to_string(start.x)+"_" + std::to_string(start.y);

    open_nodes.push(start);
    pre_node[start_name] = start;

    while(!open_nodes.empty()){
        Vec2i curr_node = open_nodes.front();
        open_nodes.pop();

        // 判断是否到达终点
        if(curr_node.x == goal.x && curr_node.y == goal.y){
            pre_node[goal_name] = pre_node[std::to_string(curr_node.x)+"_" + std::to_string(curr_node.y)];
            break;
        }
        // 获取相邻的有效节点
        for(int i = 0; i < 4; i++){
            isvalid = true;
            Vec2i neighbor{curr_node.x+neighbors[i].x, curr_node.y+neighbors[i].y};
            // 检查节点是否已经被遍历过
            std::string n_name = std::to_string(neighbor.x) + "_" + std::to_string(neighbor.y);
            if(pre_node.find(n_name) != pre_node.end()){
                continue;
            }

            // 简单的碰撞检测，节点附近一定范围内不能有障碍点 
            for(int i = -4; i< 5; i++){
                for(int j = 4; j > -5; j--){
                    Vec2i pt{neighbor.x+i, neighbor.y+j};
                    if(obstacles.find(pt) != obstacles.end()){
                        isvalid = false;
                        break;
                    }
                }
            }
            if(!isvalid) continue;
            
            pre_node[n_name] = curr_node;
            open_nodes.push(neighbor);
        }
    }

    if(pre_node.find(goal_name) == pre_node.end()){
        std::cout<<"未找到有效路径！"<<std::endl;
    }
    else{
        std::cout<<"找到有效路径！"<<std::endl;
        Vec2i pt = goal;
        while(pt.x != start.x || pt.y != start.y){
            path.emplace_back(pt);
            pt = pre_node[std::to_string(pt.x) + "_" + std::to_string(pt.y)];   
        }
        path.emplace_back(start);
    }
    return path;
}

运行结果如下图所示：

3. Astar

std::vector<Vec2i> AStar(const Scenario& scenario,
                    const std::unordered_set<Vec2i, Vec2iHash>& obstacles) {

    // 每个节点的查找方向，8个方向
    std::vector<Vec2i> neighbors{Vec2i{-1,1}, Vec2i{0, 1}, Vec2i{1, 1},
                               Vec2i{1,0}, Vec2i{1,-1}, Vec2i{0,-1}, 
                               Vec2i{-1,-1}, Vec2i{-1,0}}; 
    std::vector<Vec2i> path; // 路径结果                               
    
    // 节点队列，保存节点名和节点总代价（路径代价+启发函数代价）
    std::priority_queue<std::pair<std::string, double>, 
        std::vector<std::pair<std::string, double>>, cmp> open_nodes_cost;
    // <节点名，<节点坐标，节点路径代价>>
    std::unordered_map<std::string, std::pair<Vec2i, double>> open_nodes;
    // <节点名，<节点坐标，节点路径代价>>
    std::unordered_map<std::string, Vec2i> close_nodes;
    // 保存节点的父节点
    std::unordered_map<std::string, Vec2i> pre_node; 
    bool isvalid = true;

    Vec2i start{static_cast<int>(scenario.start_pos.x/RES_GRID), 
                static_cast<int>(scenario.start_pos.y/RES_GRID)};
    Vec2i goal{static_cast<int>(scenario.end_pos.x/RES_GRID), 
               static_cast<int>(scenario.end_pos.y/RES_GRID)};

    double start_cost = std::sqrt((goal.x - start.x)*(goal.x - start.x) + 
                        (goal.y - start.y)*(goal.y - start.y));
    std::string goal_name = std::to_string(goal.x) + "_" + std::to_string(goal.y);
    std::string start_name = std::to_string(start.x) + "_" + std::to_string(start.y);

    open_nodes_cost.emplace(start_name, start_cost);
    open_nodes.emplace(start_name, std::pair<Vec2i, double>(start, 0));
    pre_node.emplace(start_name, start);

    while(!open_nodes_cost.empty()){
        const std::string curr_name = open_nodes_cost.top().first;
        open_nodes_cost.pop();
        Vec2i curr_node = open_nodes[curr_name].first;
        double curr_path_cost = open_nodes[curr_name].second;

        // 判断是否到达终点
        if(curr_node.x == goal.x && curr_node.y == goal.y){
            pre_node[goal_name] = pre_node[curr_name];
            break;
        } 

        // 当前节点已check
        close_nodes.emplace(curr_name, curr_node);
        
        // 遍历相邻节点
        for(int i = 0; i < 8; i++){
            isvalid = true;
            Vec2i neighbor{curr_node.x+neighbors[i].x, curr_node.y+neighbors[i].y};
            std::string neighbor_name = std::to_string(neighbor.x) + "_" + std::to_string(neighbor.y);
            
            // 简单的碰撞检测，节点附近一定范围内不能有障碍点 
            for(int i = -4; i< 5; i++){
                for(int j = 4; j > -5; j--){
                    Vec2i pt{neighbor.x+i, neighbor.y+j};
                    if(obstacles.find(pt) != obstacles.end()){
                        isvalid = false;
                        break;
                    }
                }
            }
            if(!isvalid) continue;

            // 如果该点已经check过
            if(close_nodes.find(neighbor_name) != close_nodes.end()){
                continue;
            }
            // 计算节点代价
            double neighbor_path_cost = curr_path_cost + 
                            std::sqrt(neighbors[i].x*neighbors[i].x+neighbors[i].y*neighbors[i].y);
            // 启发函数直接用欧式距离
            double H_cost = std::sqrt((goal.x - neighbor.x)*(goal.x - neighbor.x) + 
                            (goal.y - neighbor.y)*(goal.y - neighbor.y));

            if(open_nodes.find(neighbor_name) == open_nodes.end()){
                open_nodes.emplace(neighbor_name, std::pair<Vec2i, double>(neighbor, neighbor_path_cost));
                open_nodes_cost.emplace(neighbor_name, H_cost+neighbor_path_cost);
                pre_node[neighbor_name] = curr_node;
            }
        }
    }

    if(pre_node.find(goal_name) == pre_node.end()){
        std::cout<<"未找到有效路径！"<<std::endl;
    }
    else{
        std::cout<<"找到有效路径！"<<std::endl;
        Vec2i pt = goal;
        while(pt.x != start.x || pt.y != start.y){
            path.emplace_back(pt);
            pt = pre_node[std::to_string(pt.x) + "_" + std::to_string(pt.y)];   
        }
        path.emplace_back(start);
    }
    return path;
}

与BFS结果一起显示，如下图所示，红色路径是BFS结果，紫色路径结果是Astar：

4. HybridStar

std::vector<Vec2i> HybridAStar(const Scenario& scenario,
                                const unordered_set<Vec2i, Vec2iHash>& obstacles,
                                double wheelbase, double step_size, double max_steer) {
    
    std::shared_ptr<Node> last_node = nullptr;  
    std::vector<Vec2i> path; // 路径结果                             
    
    // 节点队列，保存节点名和节点总代价（路径代价+启发函数代价）
    std::priority_queue<std::pair<std::string, double>, 
        std::vector<std::pair<std::string, double>>, cmp> open_nodes_cost;
    // <节点名，节点状态>
    std::unordered_map<std::string, std::shared_ptr<Node>> open_nodes;
    // <节点名，节点状态>
    std::unordered_map<std::string, std::shared_ptr<Node>> close_nodes;

    Node start(scenario.start_pos.x, scenario.start_pos.y, 0, 0, 0, nullptr);
    Node goal(scenario.end_pos.x, scenario.end_pos.y, 0, 0, 0, nullptr);

    Vec2i goal_index{static_cast<int>(goal.x / RES_GRID), 
                     static_cast<int>(goal.y / RES_GRID)};

    double start_cost = std::sqrt((goal.x - start.x)*(goal.x - start.x) + 
                                  (goal.y - start.y)*(goal.y - start.y)); // 真实坐标距离

    Vec2i start_index{static_cast<int>(start.x / RES_GRID),
                      static_cast<int>(start.y / RES_GRID)};
    std::string start_name = std::to_string(start_index.x) +"_" + 
                             std::to_string(start_index.y) + "_" + std::to_string(0);
    
    std::shared_ptr<Node> start_Node = std::make_shared<Node>();
    start_Node->x = start.x;
    start_Node->y = start.y;
    start_Node->theta = start.theta;
    start_Node->g = start.g;
    start_Node->f = start.f;
    start_Node->parent = nullptr;

    open_nodes_cost.emplace(start_name, start_cost);
    open_nodes.emplace(start_name, start_Node);

    while(!open_nodes_cost.empty()){

        const std::string curr_name = open_nodes_cost.top().first;
        open_nodes_cost.pop();
        auto curr_node = open_nodes[curr_name];
        
        // 判断当前节点是否到达终点
        Vec2i curr_index{static_cast<int>(curr_node->x / RES_GRID), 
                         static_cast<int>(curr_node->y / RES_GRID)};
        if(curr_index.x == goal_index.x && curr_index.y == goal_index.y){
            last_node = curr_node;
            break;
        }

        close_nodes[curr_name] = curr_node;

        // 往下扩展，-45°到45°采样5次且只考虑前进
        for(int i = 0; i < 5; i++){
            std::shared_ptr<Node> next_Node = std::make_shared<Node>();
            double next_x = 0;
            double next_y = 0;
            double next_theta = 0;

            double steer = -M_PI/4 + i * (M_PI/8);  // 转向角
            if(i != 2){ // 转弯
                double radius = wheelbase / tan(steer); // 转弯半径
                double delt_theta = step_size / radius; // 航向角偏移
                next_theta = NormalizeAngle(curr_node->theta - delt_theta); // 转向角正时右转，航向角向负偏移

                if(radius < 0){ // 左转
                    next_x = curr_node->x + abs(radius)*(cos(curr_node->theta + abs(delt_theta))-cos(curr_node->theta));
                    next_y = curr_node->y + abs(radius)*(sin(curr_node->theta + abs(delt_theta))-sin(curr_node->theta));
                }
                else{ // 右转
                    next_x = curr_node->x + abs(radius)*(-cos(curr_node->theta - abs(delt_theta))+cos(curr_node->theta));
                    next_y = curr_node->y + abs(radius)*(sin(-curr_node->theta + abs(delt_theta))+sin(curr_node->theta));
                }
            }
            else{ // 直行
                
                next_x = curr_node->x - std::sin(curr_node->theta) * step_size;
                next_y = curr_node->y + std::cos(curr_node->theta) * step_size;
                next_theta = curr_node->theta;
            }

            next_Node->x = next_x;
            next_Node->y = next_y;
            next_Node->theta = next_theta;
            next_Node->g = curr_node->g + step_size; 
            next_Node->f = next_Node->g + std::sqrt((next_Node->x-goal.x)*(next_Node->x-goal.x) +
                                                    (next_Node->y-goal.y)*(next_Node->y-goal.y));
            next_Node->parent = curr_node;

            Vec2i next_node{static_cast<int>(next_Node->x / RES_GRID),
                            static_cast<int>(next_Node->y / RES_GRID)};
            std::string next_name = std::to_string(next_node.x) + "_" + 
                                    std::to_string(next_node.y)+"_"+to_string(i);

            if (close_nodes.find(next_name) != close_nodes.end()) {
                continue;
            }

            // 简单的碰撞检测，节点附近一定范围内不能有障碍点 
            bool isvalid_node = true;
            for(int i = -4; i< 5; i++){
                for(int j = 4; j > -5; j--){
                    Vec2i pt{next_node.x+i, next_node.y+j};
                    if(obstacles.find(pt) != obstacles.end()){
                        isvalid_node = false;
                        break;
                    }
                }
            }
            if(!isvalid_node) continue;

            if(open_nodes.find(next_name) == open_nodes.end()){
                open_nodes.emplace(next_name, next_Node);
                open_nodes_cost.emplace(next_name, next_Node->f);
            }
        }
    }

    if(last_node == nullptr){
        std::cout<<"未找到有效路径！"<<std::endl;
    }
    else{
        std::cout<<"找到有效路径！"<<std::endl;
        std::shared_ptr<Node> temp_node = last_node;
        while(temp_node->parent != nullptr){
            path.emplace_back(Vec2i{static_cast<int>(temp_node->x / RES_GRID), 
                                    static_cast<int>(temp_node->y / RES_GRID)});
            temp_node = temp_node->parent;
        }
    }

    return path;
}

与BFS和Astar的结果一起显示，如下图所示，红色路径是BFS结果，紫色路径结果是Astar，棕色路径是HybridAstar，看起来不是很平滑，感兴趣的可以自己调调看：