算法分析
車輛路徑規(guī)劃尋路算法有很多，apollo路徑規(guī)劃模塊使用的是啟發(fā)式搜索算法A*尋路算法律杠。
a*算法是一種在路網(wǎng)上中求解最短路徑的直接搜索尋路算法,原理是引入估價函數(shù)患蹂，加快搜索速度决记，提高了局部擇優(yōu)算法搜索的精度，成為當前較為流行的最短路算法桐汤。
估價函數(shù)用公式表示為：
f(n)=g(n)+h(n)
其中：
- f(n) 是從初始節(jié)點到目標節(jié)點的最佳路徑的估計代價垢夹；
- g(n) 是從初始節(jié)點到節(jié)點n的代價溢吻；
- h(n) 是從節(jié)點n到目標節(jié)點的估計代價。
要保證找到最短路徑（最優(yōu)解的）條件棚饵，關鍵在于估價函數(shù)f(n)的選让喝埂（或者說h(n)的選妊谕辍）噪漾。
很顯然，距離估計與實際值越接近且蓬，估價函數(shù)取得就越好欣硼，例如對于路網(wǎng)來說，可以取兩節(jié)點間曼哈頓距離做為距離估計恶阴，即f=g(n) + (abs(dx - nx) + abs(dy - ny))诈胜；這樣估價函數(shù)f(n)在g(n)一定的情況下豹障，會或多或少的受距離估計值h(n)的制約，節(jié)點距目標點近焦匈，h值小血公，f值相對就小，能保證最短路的搜索向終點的方向進行缓熟。
a*算法保持著兩個表累魔，open表和closed表，open表由未考察的節(jié)點組成够滑，而closed表由已考察的節(jié)點組成垦写，當算法已經(jīng)檢查過與某個節(jié)點相連的所有節(jié)點，計算出它們的f,g和h值彰触，并把它們放入open表梯投，以待考察，則稱這個節(jié)點為已考察的况毅。
算法過程
算法過程
- 令s為起始節(jié)點
- 計算s的f,g和h值
- 將s加入open表分蓖，此時s是open表里唯一的節(jié)點
- 令b＝open表中的最佳節(jié)點（最佳的意思是該節(jié)點的f值最小）
- 如果b是目標節(jié)點尔许，則退出咆疗，此時已找到一條路徑
- 如果open表為空，則退出母债，此時沒有找到路徑
- 令c等于一個與b相連的有效節(jié)點
- 計算c的f,g,h值
- 檢查c是在open表里還是在closed表里午磁，若在closed 表中，則檢查新路徑是否比原先更好（f值更姓泵恰）迅皇，若是則采用新路徑，否則把c添加入open表
- 對所有b的有效子孫節(jié)點重復第5步
- 重復第4步
源碼分析
現(xiàn)在分析一下route模塊里a*算法的實現(xiàn)衙熔。
- 節(jié)點定義在modules/routing/graph/topo_node.h文件,graph目錄下還有計算用到的邊和圖的定義
- 主要算法實現(xiàn)在modules/routing/strategy/a_star_strategy.cc文件
首先看定義的h(n)函數(shù)
double AStarStrategy::HeuristicCost(const TopoNode* src_node,
const TopoNode* dest_node) {
const auto& src_point = src_node->AnchorPoint();
const auto& dest_point = dest_node->AnchorPoint();
double distance = fabs(src_point.x() - dest_point.x()) +
fabs(src_point.y() - dest_point.y());
return distance;
}
這段代碼非常清晰登颓，h(n)=abs(dx-nx)+abs(dy-ny)。
bool Reconstruct(
const std::unordered_map<const TopoNode*, const TopoNode*>& came_from,
const TopoNode* dest_node, std::vector<NodeWithRange>* result_nodes)
這段函數(shù)實現(xiàn)了主要是算法第5步的功能红氯，然后內(nèi)部調用了AdjustLaneChange框咙，AdjustLaneChange函數(shù)又調用了AdjustLaneChangeBackward和AdjustLaneChangeForward函數(shù)
最后是供其他模塊調用的Search函數(shù)
bool AStarStrategy::Search(const TopoGraph* graph,
const SubTopoGraph* sub_graph,
const TopoNode* src_node, const TopoNode* dest_node,
std::vector<NodeWithRange>* const result_nodes)
其中open_set_detail就是open表，使用的是priority_queue優(yōu)先級隊列痢甘，push加入一個元素喇嘱，pop刪除第一個元素。
這是一個非常標準的A*尋路算法實現(xiàn)塞栅。
百度路徑尋路算法的不足
百度路徑尋路算法的不足：
- 使用了通用的a*算法者铜，沒有使用效果更好的其他尋路算法
- 只考慮了本車的情況，沒有考慮和周圍其他行人車輛的協(xié)同
- 沒有考慮碰撞檢測和避障算法的實現(xiàn)，避障算法這個其實是重中之重
