近期一直使用lgsvl測試autoware的各種功能，前期使用autoware1.14+OpenPlanner_v1，測試期間發(fā)現(xiàn)OpenPlanner_v1可以正常生成local trajactories，但local trajactory的evalutor和selector部分有問題，大部分時(shí)刻沒有做出正確的選擇，且檢測到障礙物的trajactory沒有正常被Block掉。因此升級使用Autoware1.15 + OpenPlanner2.5。

Autoware1.15 + OpenPlanner2.5地址：

GitHub - hatem-darweesh/autoware.ai.openplanner: The workspace for directly downloading and installing Autoware.AI versions with the latest OpenPlanner 2.5 updatesThe workspace for directly downloading and installing Autoware.AI versions with the latest OpenPlanner 2.5 updates - GitHub - hatem-darweesh/autoware.ai.openplanner: The workspace for directly downloading and installing Autoware.AI versions with the latest OpenPlanner 2.5 updateshttps://github.com/hatem-darweesh/autoware.ai.openplanner

---

本篇先研究global planner里的laneChange功能，分為以下幾個(gè)部分：

Global planner里的laneChange

? ? ? ? 1. 對vector map的要求

? ? ? ? 2. Vector map的loader如何提供laneChange的功能

? ? ? ? 3. Global planner中l(wèi)aneChange的代碼實(shí)現(xiàn)

?

1.對vector map的要求

LaneChange顧名思義就是車輛行駛過程中，從一條lane change到另一條lane，那么勢必要求地圖中本身存在并行的車道，我們可以從vector map的lane.csv中，每個(gè)lane的LCnt看出該lane的并行l(wèi)ane數(shù)量。

將vector map load進(jìn)tier4，選擇某條lane，也能看出它的并行l(wèi)ane數(shù)量，并在可視化的vector map里得到確認(rèn)。?

? ? ? ?

2.?Vector map的loader如何提供laneChange的功能? ? ? ? ?

Vector map部分的代碼是如何處理map數(shù)據(jù)，實(shí)現(xiàn)laneChange的呢？Open planner會創(chuàng)建VectorMapLoader，調(diào)用ConstructRoadNetworkFromROSMessageV2函數(shù)構(gòu)建RoadNetwork，其中有一個(gè)關(guān)鍵參數(shù)m_params.bEnableLaneChange，也就是之前我們在Runtime Manager中勾選的Lane Changing。其中跟變道有關(guān)的函數(shù)為FindAdjacentLanesV2。具體解析參考這個(gè)博客：

OpenPlanner變道遇到的問題及解決_此心安處是吾鄉(xiāng)-Aaron的博客-CSDN博客_openplanner111https://blog.csdn.net/weixin_39940512/article/details/117451918

3. 車輛運(yùn)行中l(wèi)aneChange的代碼實(shí)現(xiàn)

首先看op_global_planner_core.cpp里的MainLoop()，其中實(shí)現(xiàn)global plan功能的函數(shù)GenerateGlobalPlan：

if(m_iCurrentGoalIndex >= 0 && (m_bReplanSignal || bMakeNewPlan || m_GeneratedTotalPaths.size() == 0)){PlannerHNS::WayPoint goalPoint = m_GoalsPos.at(m_iCurrentGoalIndex);bool bNewPlan = GenerateGlobalPlan(m_CurrentPose, goalPoint, m_GeneratedTotalPaths);

具體實(shí)現(xiàn)：

bool GlobalPlanner::GenerateGlobalPlan(PlannerHNS::WayPoint& startPoint, PlannerHNS::WayPoint& goalPoint, std::vector<std::vector<PlannerHNS::WayPoint> >& generatedTotalPaths) {std::vector<int> predefinedLanesIds;double ret = 0;//The distance that is needed to brake from current speed to zero with acceleration of -1 m/s*sdouble planning_distance = pow((m_CurrentPose.v), 2);if(planning_distance < MIN_EXTRA_PLAN_DISTANCE){planning_distance = MIN_EXTRA_PLAN_DISTANCE;}// ... ignore if(m_bEnableAnimation)...else{if(m_bStoppingState){generatedTotalPaths.clear();ret = m_PlannerH.PlanUsingDPRandom(startPoint, 20, m_Map, generatedTotalPaths);}else if(m_bExploreMode){std::cout << "Generating Random Trajectories .. " << std::endl;generatedTotalPaths.clear();ret = m_PlannerH.PlanUsingDPRandom(m_CurrentPose, 250, m_Map, generatedTotalPaths);}else //do this{ret = m_PlannerH.PlanUsingDP(startPoint, goalPoint, MAX_GLOBAL_PLAN_SEARCH_DISTANCE, planning_distance, m_params.bEnableLaneChange, predefinedLanesIds, m_Map, generatedTotalPaths);}}if(ret == 0){std::cout << "Can't Generate Global Path for Start (" << startPoint.pos.ToString()<< ") and Goal (" << goalPoint.pos.ToString() << ") in GenerateGlobalPlan()" << std::endl;return false;}

會調(diào)用PlanUsingDP函數(shù)，具體實(shí)現(xiàn)在PlannerH.cpp里：

double PlannerH::PlanUsingDP(const WayPoint& start,const WayPoint& goalPos,const double& maxSearchDistance,const double& planning_distance,const bool bEnableLaneChange,const std::vector<int>& globalPath,RoadNetwork& map,std::vector<std::vector<WayPoint> >& paths, vector<WayPoint*>* all_cell_to_delete) //paths's size is 0 at the beginning {PlannerHNS::WayPoint* pStart = PlannerHNS::MappingHelpers::GetClosestWaypointFromMap(start, map);PlannerHNS::WayPoint* pGoal = PlannerHNS::MappingHelpers::GetClosestWaypointFromMap(goalPos, map);if(!pStart || !pGoal){GPSPoint sp = start.pos;GPSPoint gp = goalPos.pos;cout << endl << "Error: PlannerH -> Can't Find Global Waypoint Nodes in the Map, " << endl;cout << " Start: " << sp.ToString() << " Goal: " << gp.ToString() << "" << endl;return 0;}if(!pStart->pLane || !pGoal->pLane){cout << endl << "Error: PlannerH -> Null Lane," << endl << " Start Lane: " << pStart->pLane << ", Goal Lane: " << pGoal->pLane << endl;return 0;}RelativeInfo start_info, goal_info;PlanningHelpers::GetRelativeInfo(pStart->pLane->points, start, start_info);PlanningHelpers::GetRelativeInfo(pGoal->pLane->points, goalPos, goal_info);vector<WayPoint> goal_path;if(fabs(start_info.perp_distance) > START_POINT_MAX_DISTANCE){GPSPoint sp = start.pos;cout << endl << "Error: PlannerH -> Start Distance to Lane is: " << start_info.perp_distance<< ", Pose: " << sp.ToString() << ", LanePose:" << start_info.perp_point.pos.ToString()<< ", LaneID: " << pStart->pLane->id << " -> Check origin and vector map. " << endl;return 0;}if(fabs(goal_info.perp_distance) > GOAL_POINT_MAX_DISTANCE) //8{if(fabs(goal_info.perp_distance) > 20){GPSPoint gp = goalPos.pos;cout << endl << "Error: PlannerH -> Goal Distance to Lane is: " << goal_info.perp_distance<< ", Pose: " << gp.ToString() << ", LanePose:" << goal_info.perp_point.pos.ToString()<< ", LaneID: " << pGoal->pLane->id << " -> Check origin and vector map. " << endl;return 0;}else{WayPoint wp = *pGoal;wp.pos.x = (goalPos.pos.x+pGoal->pos.x)/2.0;wp.pos.y = (goalPos.pos.y+pGoal->pos.y)/2.0;goal_path.push_back(wp);goal_path.push_back(goalPos);}}vector<WayPoint*> local_cell_to_delete;WayPoint* pLaneCell = 0;char bPlan = 'A';if(all_cell_to_delete)pLaneCell = PlanningHelpers::BuildPlanningSearchTreeV2(pStart, *pGoal, globalPath, maxSearchDistance,bEnableLaneChange, *all_cell_to_delete);elsepLaneCell = PlanningHelpers::BuildPlanningSearchTreeV2(pStart, *pGoal, globalPath, maxSearchDistance,bEnableLaneChange, local_cell_to_delete);if(!pLaneCell){bPlan = 'B';cout << endl << "PlannerH -> Plan (A) Failed, Trying Plan (B)." << endl;if(all_cell_to_delete)pLaneCell = PlanningHelpers::BuildPlanningSearchTreeStraight(pStart, planning_distance, *all_cell_to_delete);elsepLaneCell = PlanningHelpers::BuildPlanningSearchTreeStraight(pStart, planning_distance, local_cell_to_delete);if(!pLaneCell){bPlan = 'Z';cout << endl << "PlannerH -> Plan (B) Failed, Sorry we Don't have plan (C) This is the END." << endl;return 0;}}vector<WayPoint> path;vector<vector<WayPoint> > tempCurrentForwardPathss;PlanningHelpers::TraversePathTreeBackwards(pLaneCell, pStart, globalPath, path, tempCurrentForwardPathss);if(path.size()==0) return 0;/*** Next line is added on 27 September 2020, when planning with map with sparse waypoints, skipping the start waypoint is a problem,* so I inserted after generating the initial plan*/path.insert(path.begin(), *pStart);paths.clear();if(bPlan == 'A'){PlanningHelpers::ExtractPlanAlernatives(path, planning_distance, paths, LANE_CHANGE_SMOOTH_FACTOR_DISTANCE);}else if (bPlan == 'B'){paths.push_back(path);}cout << endl <<"Info: PlannerH -> Plan (" << bPlan << ") Path With Size (" << (int)path.size() << "), MultiPaths No(" << paths.size() << ") Extraction Time : " << endl;if(path.size()<2){cout << endl << "Err: PlannerH -> Invalid Path, Car Should Stop." << endl;if(pLaneCell && !all_cell_to_delete)DeleteWaypoints(local_cell_to_delete);return 0 ;}if(pLaneCell && !all_cell_to_delete)DeleteWaypoints(local_cell_to_delete);for (int i = 0; i < path.size(); ++i) {std::cout << "path.at(" << i << ") 's distance is: " << path.at(i).distanceCost << ", laneId is: " << path.at(i).laneId << ", timeCost is: " << path.at(i).timeCost << std::endl;}double totalPlanningDistance = path.at(path.size()-1).distanceCost;return 1; //original is return totalPlanningDistance }

此函數(shù)首先獲取起點(diǎn)和終點(diǎn)的相關(guān)信息：

在由用戶輸入的start和goalPos（一般在Rviz上畫出來）調(diào)用GetClosestWaypointFromMap，獲得vector map中的最近的位置pStart和pGoal后，再調(diào)用GetRelativeInfo獲取pStart和pGoal的relativeInfo start_info和goal_info。

再調(diào)用BuildPlanningSearchTreeV2函數(shù)，尋找是否存在由pStart到達(dá)pGoal的最短路徑，具體實(shí)現(xiàn)在PlanningHelpers.cpp中：

WayPoint* PlanningHelpers::BuildPlanningSearchTreeV2(WayPoint* pStart,const WayPoint& goalPos,const vector<int>& globalPath,const double& DistanceLimit,const bool& bEnableLaneChange,vector<WayPoint*>& all_cells_to_delete) {if(!pStart) return NULL;vector<pair<WayPoint*, WayPoint*> >nextLeafToTrace;WayPoint* pZero = 0;WayPoint* wp = new WayPoint();*wp = *pStart;nextLeafToTrace.push_back(make_pair(pZero, wp));all_cells_to_delete.push_back(wp);double distance = 0;double before_change_distance = 0;WayPoint* pGoalCell = 0;double nCounter = 0;while(nextLeafToTrace.size()>0){nCounter++;unsigned int min_cost_index = 0;double min_cost = DBL_MAX;for(unsigned int i=0; i < nextLeafToTrace.size(); i++){if(nextLeafToTrace.at(i).second->cost < min_cost){min_cost = nextLeafToTrace.at(i).second->cost;min_cost_index = i;}}WayPoint* pH = nextLeafToTrace.at(min_cost_index).second;assert(pH != 0);nextLeafToTrace.erase(nextLeafToTrace.begin()+min_cost_index);double distance_to_goal = distance2points(pH->pos, goalPos.pos);double angle_to_goal = UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(UtilityHNS::UtilityH::FixNegativeAngle(pH->pos.a), UtilityHNS::UtilityH::FixNegativeAngle(goalPos.pos.a));if( distance_to_goal <= 0.1 && angle_to_goal < M_PI_4){cout << "Goal Found, LaneID: " << pH->laneId <<", Distance : " << distance_to_goal << ", Angle: " << angle_to_goal*RAD2DEG << endl;pGoalCell = pH;break;}else{if(pH->pLeft && !CheckLaneExits(all_cells_to_delete, pH->pLeft->pLane) && !CheckNodeExits(all_cells_to_delete, pH->pLeft) && bEnableLaneChange && before_change_distance > LANE_CHANGE_MIN_DISTANCE){wp = new WayPoint();*wp = *pH->pLeft;double d = hypot(wp->pos.y - pH->pos.y, wp->pos.x - pH->pos.x);distance += d;before_change_distance = -LANE_CHANGE_MIN_DISTANCE*2;for(unsigned int a = 0; a < wp->actionCost.size(); a++){//if(wp->actionCost.at(a).first == LEFT_TURN_ACTION)d += wp->actionCost.at(a).second;}wp->cost = pH->cost + d;wp->pRight = pH;wp->pLeft = 0;nextLeafToTrace.push_back(make_pair(pH, wp));all_cells_to_delete.push_back(wp);}if(pH->pRight && !CheckLaneExits(all_cells_to_delete, pH->pRight->pLane) && !CheckNodeExits(all_cells_to_delete, pH->pRight) && bEnableLaneChange && before_change_distance > LANE_CHANGE_MIN_DISTANCE){wp = new WayPoint();*wp = *pH->pRight;double d = hypot(wp->pos.y - pH->pos.y, wp->pos.x - pH->pos.x);distance += d;before_change_distance = -LANE_CHANGE_MIN_DISTANCE*2;for(unsigned int a = 0; a < wp->actionCost.size(); a++){//if(wp->actionCost.at(a).first == RIGHT_TURN_ACTION)d += wp->actionCost.at(a).second;}wp->cost = pH->cost + d;wp->pLeft = pH;wp->pRight = 0;nextLeafToTrace.push_back(make_pair(pH, wp));all_cells_to_delete.push_back(wp);}for(unsigned int i =0; i< pH->pFronts.size(); i++){if(CheckLaneIdExits(globalPath, pH->pLane) && pH->pFronts.at(i) && !CheckNodeExits(all_cells_to_delete, pH->pFronts.at(i))){wp = new WayPoint();*wp = *pH->pFronts.at(i);double d = hypot(wp->pos.y - pH->pos.y, wp->pos.x - pH->pos.x);distance += d;before_change_distance += d;for(unsigned int a = 0; a < wp->actionCost.size(); a++){//if(wp->actionCost.at(a).first == FORWARD_ACTION)d += wp->actionCost.at(a).second;}wp->cost = pH->cost + d;wp->pBacks.push_back(pH);nextLeafToTrace.push_back(make_pair(pH, wp));all_cells_to_delete.push_back(wp);}}}if(distance > DistanceLimit && globalPath.size()==0){//if(!pGoalCell)cout << "Goal Not Found, LaneID: " << pH->laneId <<", Distance : " << distance << endl;pGoalCell = pH;break;}//pGoalCell = pH;}while(nextLeafToTrace.size()!=0)nextLeafToTrace.pop_back();//closed_nodes.clear();return pGoalCell; }

這個(gè)函數(shù)的大致邏輯為：

使用動態(tài)規(guī)劃DP算法尋找是否存在由pStart到達(dá)pGoal的最短路徑。會從pStart開始，不斷遍歷它能夠到達(dá)的周邊的下一個(gè)waypoint，（判斷如果enbaleLaneChange，會找它的pLeft和pRight的wayPoint，這在前面構(gòu)建vector map時(shí)會創(chuàng)建；如果沒有enbaleLaneChange，則找它pFronts里的所有waypoints），加入到nextLeafToTrace，然后找到nextLeafToTrace里面WayPoint->cost最小的那個(gè)waypoint，把它設(shè)為選擇的當(dāng)前waypoint（代碼里為pH），然后繼續(xù)遍歷nextLeafToTrace。本質(zhì)上為貪心算法，只要保證在所有可到達(dá)的waypoint里，選擇cost最小的那個(gè)waypoint，就保證里local optimal solution。這時(shí)保證有一條path with min_cost能夠到達(dá)goal，并返回最終找到的那個(gè)waypoint（代碼里為pH）。

筆者加的log:

?

回到PlanUsingDP函數(shù)，如果BuildPlanningSearchTreeV2失敗，則會調(diào)用BuildPlanningSearchTreeStraight再次尋找路徑，它和BuildPlanningSearchTreeV2的區(qū)別在于它不會考慮laneChange造成的pLeft和pRight的wayPoint，只考慮pFronts里的waypoints。

接下來調(diào)用TraversePathTreeBackwards函數(shù)，它是由BuildPlanningSearchTreeV2返回的能到達(dá)的最接近goal的waypoint（pHead)，遍歷回到pStart。首先pHead會按直路遍歷回pStart，如果存在laneChange導(dǎo)致的pLeft或者pRight，就會先調(diào)到pLeft或者pRight，然后遍歷回到pStart。找到的全部路徑會存放進(jìn)PlanUsingDP函數(shù)里的vector<WayPoint> path里。

void PlanningHelpers::TraversePathTreeBackwards(WayPoint* pHead, WayPoint* pStartWP,const vector<int>& globalPathIds,vector<WayPoint>& localPath, std::vector<std::vector<WayPoint> >& localPaths) {if(pHead != NULL && pHead->id != pStartWP->id){if(pHead->pBacks.size()>0){localPaths.push_back(localPath);TraversePathTreeBackwards(GetMinCostCell(pHead->pBacks, globalPathIds),pStartWP, globalPathIds, localPath, localPaths);pHead->bDir = FORWARD_DIR;localPath.push_back(*pHead);}else if(pHead->pLeft && pHead->cost > 0){//vector<Vector2D> forward_path;//TravesePathTreeForwards(pHead->pLeft, forward_path, FORWARD_RIGHT);//localPaths.push_back(forward_path);cout << "Global Lane Change Right " << endl;TraversePathTreeBackwards(pHead->pLeft,pStartWP, globalPathIds, localPath, localPaths);pHead->bDir = FORWARD_RIGHT_DIR;localPath.push_back(*pHead);}else if(pHead->pRight && pHead->cost > 0){//vector<Vector2D> forward_path;//TravesePathTreeForwards(pHead->pRight, forward_path, FORWARD_LEFT);//localPaths.push_back(forward_path);cout << "Global Lane Change Left " << endl;TraversePathTreeBackwards(pHead->pRight,pStartWP, globalPathIds, localPath, localPaths);pHead->bDir = FORWARD_LEFT_DIR;localPath.push_back(*pHead);} // else // cout << "Err: PlannerZ -> NULL Back Pointer " << pHead;}elseassert(pHead); }

筆者實(shí)際測試結(jié)果和log：

以上就是global planner里的laneChange功能分析，以及它是如何影響global planner工作的。下篇會繼續(xù)分析local planner里的laneChange功能。

總結(jié)

以上是生活随笔為你收集整理的Autoware1.15 + OpenPlanner2.5 下的laneChange解析（1）的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

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