前文45.在ROS中實(shí)現(xiàn)global planner（1）
實(shí)現(xiàn)了一個(gè)global planner的模板闪萄，并且可以工作，本文將實(shí)現(xiàn)astar算法，為后續(xù)完成一個(gè)astar global planner做準(zhǔn)備
1. AStar簡介
1.1 AStar
Astar算法是一種圖形搜索算法,常用于尋路颖榜。Astar算法原理網(wǎng)上可以找到很多庐氮，簡單的說就是，從起點(diǎn)開始垢乙，向外發(fā)散渣慕，再去其中每個(gè)點(diǎn)到終端的估計(jì)距離最短的，繼續(xù)循環(huán)上次步驟裂逐，直到到達(dá)目標(biāo)點(diǎn)歹鱼。
1.2 啟發(fā)函數(shù)
估算距離(f)=距離起點(diǎn)距離(G)+距離終點(diǎn)的距離(H)
顯然G是已知的替劈，
- 第一次從起點(diǎn)開始绿语，G當(dāng)然是0，
- 向外發(fā)散也就是上下左右睡陪，距離起點(diǎn)當(dāng)然是1,也就是其父節(jié)點(diǎn)的G+1
H 是距離目標(biāo)點(diǎn)的距離掺涛，我們就是要規(guī)劃路徑庭敦，怎么找到距離目標(biāo)有多遠(yuǎn)，其實(shí)這個(gè)距離是估計(jì)理想距離薪缆，當(dāng)沒有障礙物的時(shí)的距離秧廉，也就是直線距離
這里的直線距離又有兩種方式表示
- 曼哈頓距離
- 歐式距離
顯然網(wǎng)格計(jì)算適合使用曼哈頓距離的，其計(jì)算消耗要小很多
2. 實(shí)現(xiàn)過程
2.1 數(shù)據(jù)結(jié)構(gòu)
上面簡單提到實(shí)現(xiàn)過程拣帽，下面我們先定義數(shù)據(jù)結(jié)構(gòu)疼电， 我們需要保存當(dāng)前已經(jīng)搜索的節(jié)點(diǎn)，同時(shí)需要找到最小的f值减拭，然后在該節(jié)點(diǎn)進(jìn)行繼續(xù)搜索和添加
- 節(jié)點(diǎn)定義
class Grid {
public:
Point parent_point_;
Point point_;
float g_;
float h_; // f = g + h
};
節(jié)點(diǎn)定義比較簡單蔽豺，也就是當(dāng)前點(diǎn)坐標(biāo)，父節(jié)點(diǎn)坐標(biāo)拧粪，g茫虽，h值
- open list
需要保存當(dāng)前已經(jīng)搜索點(diǎn)的列表，由于下次搜索有需要搜有f最小值既们，我們定義一個(gè)有限隊(duì)列濒析，這樣我們?nèi)op就可以得到最小f的節(jié)點(diǎn)
struct greater {
bool operator()(const Grid& g1, const Grid& g2) const {
float f1 = g1.h_ + g1.g_;
float f2 = g2.h_ + g2.g_;
return f1 > f2 || (f1 == f2 && g1.g_ < g2.g_);
}
};
std::priority_queue<Grid, std::vector<Grid>, greater> open_list_;
2.2 鄰域
鄰域定義較簡單，定義為相對(duì)該點(diǎn)的偏移即可
std::vector<Point> neighbors_;
// 四鄰域
neighbors_.emplace_back(-1, 0);
neighbors_.emplace_back(1, 0);
neighbors_.emplace_back(0, -1);
neighbors_.emplace_back(0, 1);
// 八領(lǐng)域再加上下面
neighbors_.emplace_back(-1, -1);
neighbors_.emplace_back(1, 1);
neighbors_.emplace_back(1, -1);
neighbors_.emplace_back(-1, 1);
2.3 搜索實(shí)現(xiàn)
2.3.1 搜索過程
簡單概括就是搜索過程就是不斷最小的f值的節(jié)點(diǎn)的鄰域啥纸，直到到達(dá)終點(diǎn)
偽代碼如下
open_list.push(start);
while(!open_list_.empty()) {
// 取最前面的也就是最小的f節(jié)點(diǎn)
Grid grid = open_list.top();
open_list.pop();
// 直到當(dāng)前搜索點(diǎn) 為終點(diǎn)号杏，終止循環(huán)
if (grid.point == end.point) {
return true;
}
// 循環(huán)這個(gè)節(jié)點(diǎn)的鄰居V節(jié)點(diǎn)， 分別計(jì)算g h斯棒， 同時(shí)把這些節(jié)點(diǎn)添加到open_list
for (neighbor:neighbors) {
Grid current;
current.g_ = grid.g_ + 1
current.h_ = calc_h(grid, neighbor, end); // 計(jì)算鄰域的h
current.parent_point_ = grid.point; // 更新父節(jié)點(diǎn)
if (!(current in open_list)) {
// 如果該點(diǎn)已經(jīng)不在open list中則添加
open_list.push(current);
else {
// 如果該點(diǎn)已經(jīng)存在open list中 則根據(jù)V計(jì)算結(jié)果確認(rèn)是否需要更新
float f = current.g_ + current.h_;
open_list[current.point].g_ = current.g_ ;
open_list[current.point].h_ = current.h_ ;
open_list[current.point].parent_point_ = grid.point; // 更新父節(jié)點(diǎn)
}
}
}
2.3.2 得到路徑
grid結(jié)構(gòu)可以看出來盾致，其實(shí)相當(dāng)于一個(gè)鏈表結(jié)構(gòu)，找到路徑后荣暮，只需要從end循環(huán)即可得到路徑
bool GetPathFromGrid(const Point& start_point, const Point& end_point, std::vector<Point>& path) {
path.clear();
path.push_back(end_point);
int start_index;
bool ret = Point2Index(start_point, start_index);
if (!ret) {
return false;
}
int index;
Point point = end_point;
ret = Point2Index(point, index);
if (!ret) {
return false;
}
while (index != start_index) {
point = all_grid_[index].parent_point_;
path.push_back(point);
Point2Index(point, index);
}
return true;
}
3. 測試驗(yàn)證
3.1 輸入
為了方便我們直接讀取png圖庭惜，這樣我們直接編輯圖就可以直接用于測試,
// 使用opencv直接讀取png圖片
cv::Mat mat = cv::imread("../map/map_demo.png", cv::IMREAD_GRAYSCALE);
// 為了保持習(xí)慣 我們反轉(zhuǎn)下， 值255認(rèn)為障礙物(讀取的圖片255是白色)
cv::threshold(mat, mat, 128, 255, CV_THRESH_BINARY_INV);
3.2 顯示
為了方便我們觀察過程穗酥，我們?cè)O(shè)計(jì)一個(gè)函數(shù)用于顯示規(guī)劃和過程护赊，為了簡便我們使用opencv窗口
void Display(const cv::Mat& map_data, // 傳入grid map
cv::Point begin, // 起點(diǎn)
cv::Point end, // 終點(diǎn)
const std::vector<cv::Point>& path, // 輸出的路徑
const std::vector<cv::Point>& close_list // 已經(jīng)完成搜索的點(diǎn)
);
4. 測試
-
輸入地圖
image.png -
測試結(jié)果
61a441eaef0c4759aa68f90467b164d0.gif
