HybridAstar是一種帶有半徑約束的路徑平滑規劃算法，算法思想來自A*算法，但A*是沒有考慮平滑和半徑約束的路徑規劃算法，且基于柵格地圖的網格搜索算法，它們的目標代價函數是為了形成一條路徑最短的無碰撞路徑。而HybirdAstar是在二者的基礎上加入半徑約束，進行路徑不同曲率方向采樣，同時采用reedsheep曲線，進行目標點的銜接，來加速路徑的生成效率。

1. A*算法原理

A*算法是基于網格的搜索算法。在算法中，路徑的搜索主要考慮了兩種代價，一個從起點到當前點路徑長度代價g(n)，另一個從當前點到目標點的預測代價h(n)，二者之和構成柵格上目標搜索方向的判斷的總代價f(n)。

因此，A*算法可以通過如下函數構建搜索方向上的柵格代價

其中，f(n)代表柵格節點n的總代價值，g(n) 代表柵格節點n距離起點的代價，h(n)是節點n距離終點的預測代價，也就是所謂的啟發函數。

A*算法是通過openlist和closelist形成柵格的可行搜索解和封閉搜索解，算法實現如下：

初始化兩個空的列表openlist和closelist，將起點加入openlist中，并設置代價值為0。 while(1) {if(openlist != null){從openlist中選取代價值最小的節點n.1. 如果節點n為終點，則從終點開始逐步追蹤parent節點，一直達到起點，返回找到的結果路徑，跳出循環，算法結束，break;2. 如果節點n不是終點，2.1 將節點n從openlist中刪除，加入到closelist中.2.2 遍歷節點n的8個相鄰無碰撞節點2.2.1 如果相鄰節點在closelist或者openlist中，則跳過該節點，計算下一個節點。2.2.2 如果相鄰節點不在openlist中，則設置該節點父節點為n，通過f(n)計算該節點的代價值，并將該節點放入openlist中。}elsebreak; //不能找到一條最優路徑 }

2. HybridAstar算法原理

HybridAstar算法主要分為兩部分，一部分是最短路徑的柵格代價值生成，有兩種方式，一種是采用A*，另一種采用djkstra，但目標都一樣，形成最短路徑上的柵格代價值；另一部分是曲率采樣，也就是所謂路徑平滑，和A*的思想一樣采用節點的方式產生樹結構，在樹結構中引入openlist和closelist兩個列表，以此進行樹的剪枝和回退操作。根據代價函數，將采樣節點放到openlist中，同時根據最優代價從openlist中取出節點，并存放到closelist中，其操作過程和前面提到的A*算法過程一致。

代價函數g(n)同時考慮了路徑長，轉向、倒車等附加成本；啟發函數考慮了最優的無碰撞路徑，采用的是reedshepp曲線生成，滿足了車輛的運動學約束；采用無碰撞的reedshepps曲線長度和運動學約束作為最終的啟發函數代價項h(n)。以下是其實現的偽代碼：

2.1 核心代碼分析

下面以Apollo代碼作為切入點來分析HybridAstar算法，主要分為四個部分：

柵格化規劃區域內環境地圖，采用djkstra或者A*算法生成當前位置到停車目標位置的柵格代價，也就是所謂的最優路徑的柵格代價地圖。

在車輛的世界坐標系下以一定的曲率采樣間隔和步長，進行車輛的位姿采樣，形成滿足運動學約束的節點樹。

計算采樣的節點代價（會將前面計算的柵格代價也考慮進來），并存放到openlist中，從openlist中取出最優代價存放到closelist中，openlist和closelist的操作過程和前面講到的A*算法原理一樣。

在采樣節點快要到達目標點時，反復采用reedshepps曲線將當前節點和目標點進行無碰撞連接，一旦reedshepps曲線連接成功，則標志著路徑生成完成。

?2.1.1 柵格代價地圖生成

在Apollo代碼中，有兩種方式來生成柵格代價地圖，一種是djkstra算法，另一種是A*算法，源代碼在coarse_trajectorygenerator/http://grid_search.cc中，對應的函數分別為GenerateDpMap和GenerateAStarPath，Apollo提供的源碼使用的是GenerateDpMap產生最短路徑的柵格代價。

采用djkstra產生的柵格代價地圖

bool GridSearch::GenerateDpMap(const double ex, const double ey, const std::vector<double>& XYbounds,const std::vector<std::vector<common::math::LineSegment2d>>&obstacles_linesegments_vec) {std::priority_queue<std::pair<std::string, double>,std::vector<std::pair<std::string, double>>, cmp>open_pq;std::unordered_map<std::string, std::shared_ptr<Node2d>> open_set;dp_map_ = decltype(dp_map_)();XYbounds_ = XYbounds;// XYbounds with xmin, xmax, ymin, ymaxmax_grid_y_ = std::round((XYbounds_[3] - XYbounds_[2]) / xy_grid_resolution_);max_grid_x_ = std::round((XYbounds_[1] - XYbounds_[0]) / xy_grid_resolution_);std::shared_ptr<Node2d> end_node =std::make_shared<Node2d>(ex, ey, xy_grid_resolution_, XYbounds_); // obstacles_linesegments_vec_ = obstacles_linesegments_vec;open_set.insert(std::make_pair(end_node->GetIndex(), end_node));open_pq.push(std::make_pair(end_node->GetIndex(), end_node->GetCost()));// Grid a star beginssize_t explored_node_num = 0;while (!open_pq.empty()){ // std::cout<<"open_pq: "<<open_pq.size()<<std::endl;const std::string current_id = open_pq.top().first;open_pq.pop();std::shared_ptr<Node2d> current_node = open_set[current_id];dp_map_.insert(std::make_pair(current_node->GetIndex(), current_node));Box2d node_box = GetNodeExpandRange(current_node->GetGridX(),current_node->GetGridY());obstacles_linesegments_vec_.clear();for(const auto& obstacle : obstacles_linesegments_vec){for(const auto& linesegment : obstacle)if(node_box.HasOverlap(linesegment)){obstacles_linesegments_vec_.emplace_back(obstacle);break;}}std::vector<std::shared_ptr<Node2d>> next_nodes =std::move(GenerateNextNodes(current_node));for (auto& next_node : next_nodes){if (!CheckConstraints(next_node)){continue;}if (dp_map_.find(next_node->GetIndex()) != dp_map_.end()){continue;}if (open_set.find(next_node->GetIndex()) == open_set.end()){++explored_node_num;next_node->SetPreNode(current_node);open_set.insert(std::make_pair(next_node->GetIndex(), next_node));open_pq.push(std::make_pair(next_node->GetIndex(), next_node->GetCost()));}else{if (open_set[next_node->GetIndex()]->GetCost() > next_node->GetCost()){open_set[next_node->GetIndex()]->SetCost(next_node->GetCost());open_set[next_node->GetIndex()]->SetPreNode(current_node);}}}}ADEBUG << "explored node num is " << explored_node_num; // std::cout<<"explored node num is " <<explored_node_num<<"[grid_search.cc]"<<std::endl;return true; }

采用A*算法產生的柵格代價地圖：

bool GridSearch::GenerateAStarPath(const double sx, const double sy, const double ex, const double ey,const std::vector<double>& XYbounds,const std::vector<std::vector<common::math::LineSegment2d>>&obstacles_linesegments_vec,GridAStartResult* result) {std::priority_queue<std::pair<std::string, double>,std::vector<std::pair<std::string, double>>, cmp>open_pq;std::unordered_map<std::string, std::shared_ptr<Node2d>> open_set;std::unordered_map<std::string, std::shared_ptr<Node2d>> close_set;XYbounds_ = XYbounds;std::shared_ptr<Node2d> start_node =std::make_shared<Node2d>(sx, sy, xy_grid_resolution_, XYbounds_);std::shared_ptr<Node2d> end_node =std::make_shared<Node2d>(ex, ey, xy_grid_resolution_, XYbounds_);std::shared_ptr<Node2d> final_node_ = nullptr;obstacles_linesegments_vec_ = obstacles_linesegments_vec;open_set.insert(std::make_pair(start_node->GetIndex(), start_node));open_pq.push(std::make_pair(start_node->GetIndex(), start_node->GetCost()));// Grid a star beginssize_t explored_node_num = 0;while (!open_pq.empty()) {std::string current_id = open_pq.top().first;open_pq.pop();std::shared_ptr<Node2d> current_node = open_set[current_id];// Check destinationif (*(current_node) == *(end_node)) {final_node_ = current_node;break;}close_set.emplace(current_node->GetIndex(), current_node);std::vector<std::shared_ptr<Node2d>> next_nodes =std::move(GenerateNextNodes(current_node));for (auto& next_node : next_nodes) {if (!CheckConstraints(next_node)) {continue;}if (close_set.find(next_node->GetIndex()) != close_set.end()) {continue;}if (open_set.find(next_node->GetIndex()) == open_set.end()) {++explored_node_num;next_node->SetHeuristic(EuclidDistance(next_node->GetGridX(), next_node->GetGridY(),end_node->GetGridX(), end_node->GetGridY()));next_node->SetPreNode(current_node);open_set.insert(std::make_pair(next_node->GetIndex(), next_node));open_pq.push(std::make_pair(next_node->GetIndex(), next_node->GetCost()));}}}if (final_node_ == nullptr) {AERROR << "Grid A searching return null ptr(open_set ran out)";return false;}LoadGridAStarResult(result);ADEBUG << "explored node num is " << explored_node_num;return true; }

最終在HybirdAstar通過調用CheckDpMap(const double sx, const double sy)獲取柵格代價。

2.1.2 HybirdAstar路徑搜索

該部分代碼在coarse_trajectorygenerator/http://hybrid_a_star.cc中。節點采樣采用了半徑約束，是在世界坐標系下進行的，節點搜索借鑒的是A*的思想。

bool HybridAStar::Plan(double sx, double sy, double sphi,double sv, double ex, double ey, double ephi,double ev,const std::vector<double>& XYbounds,const std::vector<std::vector<common::math::Vec2d>>& obstacles_vertices_vec,HybridAStartResult* result) //HybridAstar路徑規劃主函數 {// clear containersopen_set_.clear(); //前面A*中提到的openlistclose_set_.clear(); //前面A*中提到的closelistopen_pq_ = decltype(open_pq_)();final_node_ = nullptr;obstacles_linesegments_vec_.clear();for (const auto& obstacle_vertices : obstacles_vertices_vec) //感知活動的障礙物邊界信息，以直線段的形式存儲{size_t vertices_num = obstacle_vertices.size();std::vector<common::math::LineSegment2d> obstacle_linesegments;for (size_t i = 0; i < vertices_num - 1; ++i){common::math::LineSegment2d line_segment = common::math::LineSegment2d(obstacle_vertices[i], obstacle_vertices[i + 1]);obstacle_linesegments.emplace_back(line_segment); // printf("linesegment:%f,%f-%f,%f\n",line_segment.start().x(),line_segment.start().y(),line_segment.end().x(),line_segment.end().y());}obstacles_linesegments_vec_.emplace_back(obstacle_linesegments);} // obstacles_linesegments_vec_ = std::move(obstacles_linesegments_vec);obstacles_linesegments_vec_local_ = obstacles_linesegments_vec_;// load XYboundsXYbounds_ = XYbounds;// load nodes and obstaclesstart_node_.reset(new Node3d({sx}, {sy}, {sphi}, XYbounds_, planner_open_space_config_));end_node_.reset(new Node3d({ex}, {ey}, {ephi}, XYbounds_, planner_open_space_config_));if (!ValidityCheck(start_node_)) //檢查起點是否發生碰撞，是否有效{AINFO << "start_node in collision with obstacles";return false;}if (!ValidityCheck(end_node_)) //檢查終點是否發生碰撞，是否有效{AINFO << "end_node in collision with obstacles";return false;}double map_start = Clock::NowInSeconds();grid_a_star_heuristic_generator_->GenerateDpMap(ex, ey, XYbounds_,obstacles_linesegments_vec_); //柵格路徑搜索，主要用于產生柵格路徑勢場，也就是柵格最短路徑的代價值double map_time = Clock::NowInSeconds() - map_start;ADEBUG << "map time " << map_time;// load open set, pqopen_set_.insert(std::make_pair(start_node_->GetIndex(), start_node_));open_pq_.push(std::make_pair(start_node_->GetIndex(), start_node_->GetCost()));// Hybrid A* beginssize_t explored_node_num = 0;double astar_start_time = Clock::NowInSeconds();double heuristic_time = 0.0;double rs_time = 0.0;double start_time = 0.0;double end_time = 0.0;unsigned int node_cnt = 0;while (!open_pq_.empty()){//timeout checkif(Clock::NowInSeconds() - astar_start_time > 3.0){std::cout <<"timeout in 3 s,return" << std::endl;return false;}// take out the lowest cost neighboring nodeconst std::string current_id = open_pq_.top().first;open_pq_.pop();std::shared_ptr<Node3d> current_node = open_set_[current_id];// check if an analystic curve could be connected from current// configuration to the end configuration without collision. if so, search// ends.:Plan(node_cnt++;double current_heuristic = grid_a_star_heuristic_generator_->CheckDpMap(current_node->GetX(),current_node->GetY()); //grid_search中djkstra柵格搜索的柵格代價值（可以理解最優路徑花費）unsigned int N= static_cast<unsigned int>(0.1*current_heuristic); //用于加速hybridastar搜索速度，if(node_cnt >= N+1){node_cnt =0;start_time = Clock::NowInSeconds();if (AnalyticExpansion(current_node)) //是否可以生成到目標點的無碰撞的reedshepp曲線，加速搜索，直接連接規劃終點。{break;}end_time = Clock::NowInSeconds();rs_time += end_time - start_time;}close_set_.insert(std::make_pair(current_node->GetIndex(), current_node)); //將最優代價值節點放入到closelist中。Box2d node_box = GetNodeExpandRange(current_node->GetX(),current_node->GetY(),current_node->GetPhi()); //節點footprintobstacles_linesegments_vec_local_.clear();for(const auto& obstacle : obstacles_linesegments_vec_) //感知獲得的障礙物邊界信息以直線段的形式存儲{for(const auto& linesegment : obstacle)if(node_box.HasOverlap(linesegment)){obstacles_linesegments_vec_local_.emplace_back(obstacle);break;}}for (size_t i = 0; i < next_node_num_; ++i){std::shared_ptr<Node3d> next_node = Next_node_generator(current_node, i); //以不同曲率產生下一個相鄰節點// boundary check failure handleif (next_node == nullptr){continue;}// check if the node is already in the close setif (close_set_.find(next_node->GetIndex()) != close_set_.end()) //檢查下一個采樣節點是否在closelist中{continue;}// collision checkif (!ValidityCheck(next_node)) //檢查下一個采樣節點是否發生碰撞{continue;}if (open_set_.find(next_node->GetIndex()) == open_set_.end()) //下一個節點也不在openlist中{explored_node_num++;start_time = Clock::NowInSeconds();CalculateNodeCost(current_node, next_node); //計算當前采樣節點的代價值end_time = Clock::NowInSeconds();heuristic_time += end_time - start_time;open_set_.emplace(next_node->GetIndex(), next_node);open_pq_.emplace(next_node->GetIndex(), next_node->GetCost()); //將當前節點放入到closelist中,, //將當前節點放入到closelist中, cost = traj_cost_ + heuristic_cost_}}}if (final_node_ == nullptr){ADEBUG << "Hybrid A searching return null ptr(open_set ran out)";return false;}if (!GetResult(result,sv,ev)) //將前面獲得的搜尋節點信息按順序提取為特定格式的路徑數據{ADEBUG << "GetResult failed";return false;}double astar_end_time = Clock::NowInSeconds() - astar_start_time;double total_plan = Clock::NowInSeconds()-map_start;ADEBUG << "explored node num is " << explored_node_num;ADEBUG << "heuristic time is " << heuristic_time;ADEBUG << "reed shepp time is " << rs_time;ADEBUG << "hybrid astar total time is "<< astar_end_time;std::cout <<"map time is " << map_time << std::endl;std::cout <<"explored node num is " << explored_node_num << std::endl;std::cout <<"heuristic time is " << heuristic_time << std::endl;std::cout <<"reed shepp time is " << rs_time << std::endl;std::cout <<"hybrid astar run time is " << astar_end_time << std::endl;std::cout <<"total plan time is " << total_plan << std::endl;return true; }

2.1.3 HybirdAstar的代價值計算

該部分代碼在coarse_trajectorygenerator/http://hybrid_a_star.cc中。代碼實現如下：

//路徑搜索的總花費計算 void HybridAStar::CalculateNodeCost(std::shared_ptr<Node3d> current_node,std::shared_ptr<Node3d> next_node) {next_node->SetTrajCost(current_node->GetTrajCost() +TrajCost(current_node, next_node)); //節點搜索的代價值// evaluate heuristic costdouble optimal_path_cost = 0.0;optimal_path_cost += HoloObstacleHeuristic(next_node); //柵格搜索的路徑代價值，最短柵格路徑next_node->SetHeuCost(optimal_path_cost); //將柵格搜索的最短路徑花費合并到節點搜索的花費中 } //HybirdAstar路徑搜索花費 double HybridAStar::TrajCost(std::shared_ptr<Node3d> current_node,std::shared_ptr<Node3d> next_node) {// evaluate cost on the trajectory and add current costdouble piecewise_cost = 0.0;if (next_node->GetDirec()){piecewise_cost += static_cast<double>(next_node->GetStepSize() - 1) *step_size_ * traj_forward_penalty_; //路徑長度正向花費}else{piecewise_cost += static_cast<double>(next_node->GetStepSize() - 1) *step_size_ * traj_back_penalty_; //路徑反向長度花費 倒車路徑}if (current_node->GetDirec() != next_node->GetDirec()){piecewise_cost += traj_gear_switch_penalty_; //反向花費}piecewise_cost += traj_steer_penalty_ * std::abs(next_node->GetSteer()); //節點轉向角花費piecewise_cost += traj_steer_change_penalty_ *std::abs(next_node->GetSteer() - current_node->GetSteer()); //節點轉向角變化率花費return piecewise_cost; } //柵格搜索的最短路徑花費 double HybridAStar::HoloObstacleHeuristic(std::shared_ptr<Node3d> next_node) {return grid_a_star_heuristic_generator_->CheckDpMap(next_node->GetX(),next_node->GetY()); //柵格搜索的最短路徑花費 } 代價函數的計算項： cost = traj_cost_ + heuristic_cost_; traj_cost_ = kfp* step_cnt * step_size + kbp*dir + ksvp*steer + ksap*dsteer ; cost = kfp* step_cnt * step_size + kbp*dir + ksvp*steer + ksap*dsteer + heuristic_cost_; 其中， kfp代表hybridstar路徑搜索的路徑長度代價項權重。
kbp代表hybridstar路徑搜索倒車時的代價項權重。
ksvp代表hybridastar路徑搜索轉向角的代價權重。
ksap代表hybridastar路徑搜索的轉向角變化率的權重。
heuristic_cost_代表grid_search中的最短柵格路徑代價值。

2.1.4 ReedSheep曲線

ReedShepp曲線由Reeds和Shepp二人在1990年的論文《

Optimal paths for a car that goes both forwards and backwards?sector3.imm.uran.ru/shepp/Reeds_Shepp_trunk.pdf

》中提出，ReedShepp曲線由幾段半徑固定的圓弧和一段直線段拼接組成，而且圓弧的半徑就是汽車的最小轉彎半徑。前原理類似于Dubin曲線，不過在Dubin曲線的基礎上考慮了倒車的情景，因此在無人駕駛領域的引用非常普遍。

ReedShepps曲線從起點到終點路徑是通過車輛最小轉彎半徑的圓弧和直線拼接組成，曲線類型有48種，所有類型的組合如下表所示：

表中的form代表按圓弧和直線的組合分類，可以分為9大類，"C"代表了圓弧，"S" 代表了直線，"|"表示車輛運動朝向由正向轉為反向或者由反向轉為正向。

在Baseword下又可以分為很多的基元類型，由，，，，，這六種元素組成，其中表示車輛左轉前進；表示車輛左轉后退；表示車輛右轉前進；表示車輛右轉后退；表示車輛直行前進；表示車輛直行后退。更加詳細內容，請參考論文《Optimal paths for a car that goes both forwards and backwards》，上圖圖表亦來自論文。

ReedShepp曲線部分的代碼在coarse_trajectorygenerator/http://reeds_shepp_path.cc中。

bool ReedShepp::ShortestRSP(const std::shared_ptr<Node3d> start_node,const std::shared_ptr<Node3d> end_node,std::shared_ptr<ReedSheppPath> optimal_path) {std::vector<ReedSheppPath> all_possible_paths;if (!GenerateRSPs(start_node, end_node, &all_possible_paths)) //產生所有類型的reedshepp曲線{ADEBUG << "Fail to generate different combination of Reed Shepp ""paths";return false;}double optimal_path_length = std::numeric_limits<double>::infinity();size_t optimal_path_index = 0;size_t paths_size = all_possible_paths.size();for (size_t i = 0; i < paths_size; ++i){if (all_possible_paths.at(i).total_length > 0 &&all_possible_paths.at(i).total_length < optimal_path_length) {optimal_path_index = i;optimal_path_length = all_possible_paths.at(i).total_length; //尋找最短的reedshepp曲線參數}}if (!GenerateLocalConfigurations(start_node, end_node,&(all_possible_paths[optimal_path_index]))) //根據曲線參數插值reedshepp曲線{ADEBUG << "Fail to generate local configurations(x, y, phi) in SetRSP";return false;}//檢驗是否精確插值到目標點if (std::abs(all_possible_paths[optimal_path_index].x.back() -end_node->GetX()) > 1e-3 ||std::abs(all_possible_paths[optimal_path_index].y.back() -end_node->GetY()) > 1e-3 ||std::abs(all_possible_paths[optimal_path_index].phi.back() -end_node->GetPhi()) > 1e-3){ADEBUG << "RSP end position not right";for (size_t i = 0;i < all_possible_paths[optimal_path_index].segs_types.size(); ++i){ADEBUG << "types are "<< all_possible_paths[optimal_path_index].segs_types[i];}ADEBUG << "x, y, phi are: "<< all_possible_paths[optimal_path_index].x.back() << ", "<< all_possible_paths[optimal_path_index].y.back() << ", "<< all_possible_paths[optimal_path_index].phi.back();ADEBUG << "end x, y, phi are: " << end_node->GetX() << ", "<< end_node->GetY() << ", " << end_node->GetPhi();return false;}(*optimal_path).x = all_possible_paths[optimal_path_index].x;(*optimal_path).y = all_possible_paths[optimal_path_index].y;(*optimal_path).phi = all_possible_paths[optimal_path_index].phi;(*optimal_path).gear = all_possible_paths[optimal_path_index].gear;(*optimal_path).total_length =all_possible_paths[optimal_path_index].total_length;(*optimal_path).segs_types =all_possible_paths[optimal_path_index].segs_types;(*optimal_path).segs_lengths =all_possible_paths[optimal_path_index].segs_lengths;return true; }

reedshepp曲線起點start_node到終點end_node最短路徑ShortestRSP一定是上表類型中的一個，且其曲線路徑是所有reedshepp曲線中最短的。

總結

以上是生活随笔為你收集整理的Hybird Astar算法原理的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。