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

1. A*算法原理

A*算法是基于網(wǎng)格的搜索算法。在算法中，路徑的搜索主要考慮了兩種代價(jià)，一個(gè)從起點(diǎn)到當(dāng)前點(diǎn)路徑長(zhǎng)度代價(jià)g(n)，另一個(gè)從當(dāng)前點(diǎn)到目標(biāo)點(diǎn)的預(yù)測(cè)代價(jià)h(n)，二者之和構(gòu)成柵格上目標(biāo)搜索方向的判斷的總代價(jià)f(n)。

因此，A*算法可以通過如下函數(shù)構(gòu)建搜索方向上的柵格代價(jià)

其中，f(n)代表柵格節(jié)點(diǎn)n的總代價(jià)值，g(n) 代表柵格節(jié)點(diǎn)n距離起點(diǎn)的代價(jià)，h(n)是節(jié)點(diǎn)n距離終點(diǎn)的預(yù)測(cè)代價(jià)，也就是所謂的啟發(fā)函數(shù)。

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

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

2. HybridAstar算法原理

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

代價(jià)函數(shù)g(n)同時(shí)考慮了路徑長(zhǎng)，轉(zhuǎn)向、倒車等附加成本；啟發(fā)函數(shù)考慮了最優(yōu)的無碰撞路徑，采用的是reedshepp曲線生成，滿足了車輛的運(yùn)動(dòng)學(xué)約束；采用無碰撞的reedshepps曲線長(zhǎng)度和運(yùn)動(dòng)學(xué)約束作為最終的啟發(fā)函數(shù)代價(jià)項(xiàng)h(n)。以下是其實(shí)現(xiàn)的偽代碼：

2.1 核心代碼分析

下面以Apollo代碼作為切入點(diǎn)來分析HybridAstar算法，主要分為四個(gè)部分：

柵格化規(guī)劃區(qū)域內(nèi)環(huán)境地圖，采用djkstra或者A*算法生成當(dāng)前位置到停車目標(biāo)位置的柵格代價(jià)，也就是所謂的最優(yōu)路徑的柵格代價(jià)地圖。

在車輛的世界坐標(biāo)系下以一定的曲率采樣間隔和步長(zhǎng)，進(jìn)行車輛的位姿采樣，形成滿足運(yùn)動(dòng)學(xué)約束的節(jié)點(diǎn)樹。

計(jì)算采樣的節(jié)點(diǎn)代價(jià)（會(huì)將前面計(jì)算的柵格代價(jià)也考慮進(jìn)來），并存放到openlist中，從openlist中取出最優(yōu)代價(jià)存放到closelist中，openlist和closelist的操作過程和前面講到的A*算法原理一樣。

在采樣節(jié)點(diǎn)快要到達(dá)目標(biāo)點(diǎn)時(shí)，反復(fù)采用reedshepps曲線將當(dāng)前節(jié)點(diǎn)和目標(biāo)點(diǎn)進(jìn)行無碰撞連接，一旦reedshepps曲線連接成功，則標(biāo)志著路徑生成完成。

?2.1.1 柵格代價(jià)地圖生成

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

采用djkstra產(chǎn)生的柵格代價(jià)地圖

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*算法產(chǎn)生的柵格代價(jià)地圖：

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通過調(diào)用CheckDpMap(const double sx, const double sy)獲取柵格代價(jià)。

2.1.2 HybirdAstar路徑搜索

該部分代碼在coarse_trajectorygenerator/http://hybrid_a_star.cc中。節(jié)點(diǎn)采樣采用了半徑約束，是在世界坐標(biāo)系下進(jìn)行的，節(jié)點(diǎn)搜索借鑒的是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路徑規(guī)劃主函數(shù) {// 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) //感知活動(dòng)的障礙物邊界信息，以直線段的形式存儲(chǔ){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_)) //檢查起點(diǎn)是否發(fā)生碰撞，是否有效{AINFO << "start_node in collision with obstacles";return false;}if (!ValidityCheck(end_node_)) //檢查終點(diǎn)是否發(fā)生碰撞，是否有效{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_); //柵格路徑搜索，主要用于產(chǎn)生柵格路徑勢(shì)場(chǎng)，也就是柵格最短路徑的代價(jià)值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柵格搜索的柵格代價(jià)值（可以理解最優(yōu)路徑花費(fèi)）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)) //是否可以生成到目標(biāo)點(diǎn)的無碰撞的reedshepp曲線，加速搜索，直接連接規(guī)劃終點(diǎn)。{break;}end_time = Clock::NowInSeconds();rs_time += end_time - start_time;}close_set_.insert(std::make_pair(current_node->GetIndex(), current_node)); //將最優(yōu)代價(jià)值節(jié)點(diǎn)放入到closelist中。Box2d node_box = GetNodeExpandRange(current_node->GetX(),current_node->GetY(),current_node->GetPhi()); //節(jié)點(diǎn)footprintobstacles_linesegments_vec_local_.clear();for(const auto& obstacle : obstacles_linesegments_vec_) //感知獲得的障礙物邊界信息以直線段的形式存儲(chǔ){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); //以不同曲率產(chǎn)生下一個(gè)相鄰節(jié)點(diǎn)// 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()) //檢查下一個(gè)采樣節(jié)點(diǎn)是否在closelist中{continue;}// collision checkif (!ValidityCheck(next_node)) //檢查下一個(gè)采樣節(jié)點(diǎn)是否發(fā)生碰撞{continue;}if (open_set_.find(next_node->GetIndex()) == open_set_.end()) //下一個(gè)節(jié)點(diǎn)也不在openlist中{explored_node_num++;start_time = Clock::NowInSeconds();CalculateNodeCost(current_node, next_node); //計(jì)算當(dāng)前采樣節(jié)點(diǎn)的代價(jià)值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()); //將當(dāng)前節(jié)點(diǎn)放入到closelist中,, //將當(dāng)前節(jié)點(diǎn)放入到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)) //將前面獲得的搜尋節(jié)點(diǎn)信息按順序提取為特定格式的路徑數(shù)據(jù){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的代價(jià)值計(jì)算

該部分代碼在coarse_trajectorygenerator/http://hybrid_a_star.cc中。代碼實(shí)現(xiàn)如下：

//路徑搜索的總花費(fèi)計(jì)算 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)); //節(jié)點(diǎn)搜索的代價(jià)值// evaluate heuristic costdouble optimal_path_cost = 0.0;optimal_path_cost += HoloObstacleHeuristic(next_node); //柵格搜索的路徑代價(jià)值，最短?hào)鸥衤窂絥ext_node->SetHeuCost(optimal_path_cost); //將柵格搜索的最短路徑花費(fèi)合并到節(jié)點(diǎn)搜索的花費(fèi)中 } //HybirdAstar路徑搜索花費(fèi) 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_; //路徑長(zhǎng)度正向花費(fèi)}else{piecewise_cost += static_cast<double>(next_node->GetStepSize() - 1) *step_size_ * traj_back_penalty_; //路徑反向長(zhǎng)度花費(fèi) 倒車路徑}if (current_node->GetDirec() != next_node->GetDirec()){piecewise_cost += traj_gear_switch_penalty_; //反向花費(fèi)}piecewise_cost += traj_steer_penalty_ * std::abs(next_node->GetSteer()); //節(jié)點(diǎn)轉(zhuǎn)向角花費(fèi)piecewise_cost += traj_steer_change_penalty_ *std::abs(next_node->GetSteer() - current_node->GetSteer()); //節(jié)點(diǎn)轉(zhuǎn)向角變化率花費(fèi)return piecewise_cost; } //柵格搜索的最短路徑花費(fèi) double HybridAStar::HoloObstacleHeuristic(std::shared_ptr<Node3d> next_node) {return grid_a_star_heuristic_generator_->CheckDpMap(next_node->GetX(),next_node->GetY()); //柵格搜索的最短路徑花費(fèi) } 代價(jià)函數(shù)的計(jì)算項(xiàng)： 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路徑搜索的路徑長(zhǎng)度代價(jià)項(xiàng)權(quán)重。
kbp代表hybridstar路徑搜索倒車時(shí)的代價(jià)項(xiàng)權(quán)重。
ksvp代表hybridastar路徑搜索轉(zhuǎn)向角的代價(jià)權(quán)重。
ksap代表hybridastar路徑搜索的轉(zhuǎn)向角變化率的權(quán)重。
heuristic_cost_代表grid_search中的最短?hào)鸥衤窂酱鷥r(jià)值。

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曲線由幾段半徑固定的圓弧和一段直線段拼接組成，而且圓弧的半徑就是汽車的最小轉(zhuǎn)彎半徑。前原理類似于Dubin曲線，不過在Dubin曲線的基礎(chǔ)上考慮了倒車的情景，因此在無人駕駛領(lǐng)域的引用非常普遍。

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

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

在Baseword下又可以分為很多的基元類型，由，，，，，這六種元素組成，其中表示車輛左轉(zhuǎn)前進(jìn)；表示車輛左轉(zhuǎn)后退；表示車輛右轉(zhuǎn)前進(jìn)；表示車輛右轉(zhuǎn)后退；表示車輛直行前進(jìn)；表示車輛直行后退。更加詳細(xì)內(nèi)容，請(qǐng)參考論文《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)) //產(chǎn)生所有類型的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曲線參數(shù)}}if (!GenerateLocalConfigurations(start_node, end_node,&(all_possible_paths[optimal_path_index]))) //根據(jù)曲線參數(shù)插值reedshepp曲線{ADEBUG << "Fail to generate local configurations(x, y, phi) in SetRSP";return false;}//檢驗(yàn)是否精確插值到目標(biāo)點(diǎn)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曲線起點(diǎn)start_node到終點(diǎn)end_node最短路徑ShortestRSP一定是上表類型中的一個(gè)，且其曲線路徑是所有reedshepp曲線中最短的。

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。