路徑規劃算法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;} // 當前節點已checkclose_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;
}