在看了NTU的AI课程之后,试着用C++来实现了A*搜索算法。它的思想就是Avoid expanding paths that are already expensive.
。标准的A*搜索算法描述如下:
给出带权无向图,初始顶点,目标顶点,以及evaulation function。Evalutaion function包括
- g(n):为到达顶点n,当前已有的花费(cost so far to reach n)
- h(n):一个 从任意顶点 到 目标顶点 的估计花费(estimated cost to goal from n)
- f(n):从 初始顶点 到 目标顶点 的估计花费(estimated total cost from the starting node to goal through n)
f(n) = h(n) + g(n)
例如有三个地点A, B, C,它们之间的图如下
A —— B —— C
A —— B权重为30,B —— C权重为40,
A —— C的直线距离为50,但是并没有A——C这条边(即不能直接从A开始,只经过一条边就到达C)
现在,给出初始顶点为A,目标顶点为C,h(n)如下
h(A) = 50
h(B) = 40
h(C) = 0
在一开始,我们将A放入名为froniter的优先队列中,froniter按照f(n)的值升序排序。
那么我们有f(A) = h(A) + g(A)。h(A)是由用户直接给出的,等于50。g(A)是我们为了到达C,已有的花费,在这个例子中,可以理解为已经走过的路程,因为我们是一开始就在A,还没有开始走,所以g(A)为0。于是f(A) = h(A) + g(A) = 50 + 0 = 50
froniter中的数据如下:
current | from | g(current) | h(current) |
---|---|---|---|
A | A | 0 | 50 |
然后我们开始循环,只要froniter不为空,就取出队首的元素。然后从队列中删除它。并且通过一个一维数组visit记录我们是从哪个node到达当前node的。然后我们做goal test,即判断 当前顶点 是否就是我们的 目标顶点,如果是,则退出循环。如果不是,则展开当前顶点。展开当前顶点的操作为:把所有 邻接当前顶点 的 顶点n放入froniter,h(n)由用户给出,g(n)则为g(current_node) + 当前顶点 到 顶点n 的权重
。
整个循环的伪代码如下(或者跳到真实实现)
while (!froniter.empty()): (current, from, g, _) = froniter.top(); froniter.pop(); visit[current] = from; if (current == goal) break; else: for_each node in ajacent_node(current): froniter.push((node, current, g + weight(from: current, to: node), h(node)));
在这个例子中,froniter的队首为A,我们将有:
visit[A] = A;
A不是目标顶点,展开A;
与A邻接的顶点有B;
将B放入froniter
此时froniter中的数据如下:
current | from | g(current) | h(current) |
---|---|---|---|
B | A | 30 | 40 |
然后因为没有更多的邻接顶点,开始下一轮循环。
froniter的队首为B,
visit[B] = A;
B不是目标顶点,展开B;
与A邻接的顶点有A, C;
将A, C放入froniter
此时froniter中的数据如下:
current | from | g(current) | h(current) |
---|---|---|---|
C | B | 70 | 0 |
A | B | 60 | 50 |
再次开始下一轮循环,此时
froniter的队首为C,
visit[C] = B;
C是目标顶点,退出循环
那么在退出循环之后,我们只需要逆向访问visit数组,一路回到初始点即可。
from = goal; stack.push(from); while (from != initial): from = visit[from]; stack.push(from);
然后根据需要,既可以直接返回stack,也可以再把stack依次取出,放入queue或者数组。
对NTU AI课程, Week 3, Lecture 2中,计算从古亭到台北车站的最优路径的解
![A* Search Algorithm](/wp-content/uploads/2016/08/screenshot_astar.png)
整个A* Search Algorithm如下,(A-star)
#include <stdlib.h> #include <functional> #include <limits> #include <queue> #include <set> #include <stack> #include <tuple> #include <vector> /** * @brief A* Search Algorithm * * @param nodes Number of nodes * @param graph 2-dimension ajacent matrix * @param initial Starting node * @param goal Target node * @param estimated Heuristic function * * @note If estimated function is not provided, A* search is equivalent to uniformed-cost search */ template <typename Weight> std::vector<size_t> Astar(size_t nodes, const Weight ** graph, const size_t initial, const size_t goal, const std::function<Weight(const size_t current)>& estimated = [](const size_t current) -> Weight { return 0; }) { // return an empty std::vector on invaild input if (nodes == 0 || graph == nullptr) return std::vector<size_t>(); // return <initial, goal> if initial equals to goal if (initial == goal) return std::vector<size_t>(initial, goal); // state: current node, via, g(n), h(n) // g(n): cost so far to reach n // h(n): estimated cost to goal from n using state = std::tuple<size_t, size_t, Weight, Weight>; // compare function for std::priority_queue auto cmp = [](const state& a, const state& b) { // (g(a) + h(a)) > (g(b) + h(b)) // a.k.a sort by estimated total cost from initial node to goal through n return ((std::get<2>(a) + std::get<3>(a)) > (std::get<2>(b) + std::get<3>(b))); }; // frontier, increasing order std::priority_queue<state, std::vector<state>, decltype(cmp)> frontier(cmp); // visited node std::set<size_t> visited; // froniter set std::set<size_t> froniter_set; // record trace size_t * visit = (size_t *)malloc(sizeof(size_t) * nodes); // start from initial node frontier.push({initial, initial, 0, estimated(initial)}); froniter_set.emplace(initial); // a flag variable bool found = false; // frontier will be empty if goal is not in our search space while (!frontier.empty()) { // this is least weighted node auto current = frontier.top(); // node number size_t current_node = std::get<0>(current); frontier.pop(); froniter_set.erase(current_node); // record that this node has been visited visited.emplace(current_node); // we go to this node via last size_t from = std::get<1>(current); visit[current_node] = from; // goal test if ((found = (current_node == goal))) { break; } else { // iterate possible node for (size_t to = 0; to < nodes; to++) { // don't stay here // and if node to is reachable from current node if (to != current_node && graph[current_node][to] != std::numeric_limits<Weight>::max() && (visited.find(to) == visited.end())) { // don't go back if (froniter_set.find(to) == froniter_set.end()) { // push this state to frontier frontier.push({to, current_node, std::get<2>(current) + graph[current_node][to], estimated(to)}); froniter_set.emplace(to); } } } } } std::vector<size_t> trace; if (found) { // build trace std::stack<size_t> trace_stack; size_t from = goal; trace_stack.push(from); while (from != initial) { from = visit[from]; trace_stack.push(from); } // move trace into std::vector while (!trace_stack.empty()) { trace.emplace_back(trace_stack.top()); trace_stack.pop(); } } free((void *)visit); return trace; }