DStar
算法
网上的D*算法大多是用python编的，而且有的还有问题，讲解感觉也不是很透彻，本文解释了核心算法的步骤并以C++为框架搭建了DStar算法，目前在测试用例中未发现问题，如哪位读者发现任何测试用例有问题请及时给予评论，谢谢！
DStar算法即Dynamic A Star，它是一种动态搜索路径的算法，也就是在我们预先找到Start 到 Target之间的路径后，如果在Start通过后向指针向Target运动的过程中，当前点X发现下一个点Y为障碍点，则此时通过以往保存过的路径信息，相比于replan尽可能快的重新找到X到Target的新的路径！
在论文中有用的字母定义
1.每一个Node分别有三个State，即New：表示还未加入到openlist，Open：表示在openlist中，Closed：从openlist中移除
2.Dstar算法是后向搜素，也就是从Target点到Start点利用Dijkstra算法保存最短路径信息，因此H表示当前点到Target（G）点的距离
3.b（x）表示x的后向指针
4.K表示该点所有状态下最小的H值！！！（核心）
5.X表示在openlist找到的k值最小点
6.当K=H表示当前为lower状态(也就是没有障碍堵塞),反之K
算法核心
-
引入了k，而且相比于Dijkstra算法是每次找openlist中H最小的点，在DStar算法中是找k最小的点，这是为什么呢？见2
-
当我们知道了上文Y是一个障碍点，那么我们最想做的就是在X周围找到一条不受X和Y影响的，为lower的路径！因此我们就需要在openlist找到X点，只有这样我们才能在X周围尽可能找到能使X点变为lower的点，而又因为Y作为障碍点，X和Y之间的cost变为无穷大，因此如果不引入K我们只有在最后才能在openlist找到X点，从而大幅降低了效率！
-
我们需要先把障碍点放进openlist！因为需要把障碍点的raise状态扩散给所有指向障碍点的点
-
把一个状态为New的X周围的Y节点(new_H)插入openlist中，Y点的k为new_H，把一个状态为open的Y插入openlist其实也就是指的是可以从当前的最优点优化原来到Y的距离，因此只需要对应修改Y的H和k即可，此时k=min(k_old,new_H)，当把一个状态为closed的Y插入openlist中时，此时有两种可能：一种可能就是Y是lower的，这时候就是我们把raise状态的Y点放了进去然后在周围又找到一个次优点，用来更新这个raise点的位置！！对应于L23-L25 ，另一种可能就是Y是raise状态，其实也就对应了上一种可能的下一次process-state的调用，即使得我们的raise点Y的状态变为lower，找到了一条次优的路径，对应于L13，对是L13我没写错，因此k=min(old_H,new_H)，因此这时候raise点就变为lower了
详解算法流程
先说一下如果当前点位(3,2)时碰到的下一个点(4,3)为障碍点时，以该图为例应该怎么做，即沿着蓝线走到(3,2)发现(4,3)变为障碍的流程。
具体的整个PPT图片可以详见链接
https://zhuanlan.zhihu.com/p/366462473
-
（4，3）的H变为很大的一个数，程序中为cost_max，这时就要把（4，3）加入到openlist中，此时Y的状态变为raise，接下来判断Y的周围的点有没有next指针指向（4，3），也就是执行L15-L18，其余均不执行，如果有那么Y的周围这些next指针指向Y的点的H也变为cost_max，k不变，即被扩散为raise状态，该图中即（3，2）点变为raise加入openlist中其余（4，3）的周围点不加入进去
-
下一步找到了（3，2）点并d出，先执行L4-L7，看看周围能不能找到一个k值小的最优点绕出去，该例中没有，随后重复1的过程执行L15-L18即扩散raise状态给周围点，该图为（2，2）、（2，1）、（3，1）点的H变为cost_max，之后执行L23-L25看看能不能找一个次优点（4，1）加入到openlist用来下一次process-state函数调用时执行L8-L13步骤，将（3，2）点变为lower状态，即算法核心中的4！！
-
下一步就是d出（4，3），更新了（3，2）点的状态从raise变为lower，同时（3，1）点的h也被更新，但由于（3，1）点为open状态，因此他的k不变！！
-
下一步d出（2，2），同样的重复1的过程执行L15-L18即扩散raise状态给周围点（1，1）、（2，1）、（3，1）的H变为cost_max并加入openlist中
-
再接下来d出（3，1），对于（2，2）点执行了L19-L21，表示周围有点可以通过该点降低H值，那么就需要把（3，1）重新加入openlist当中用来下一步执行L8-L13优化（2，2），此时再加入也使得（3，1）的h和k相同，即（3，2）变为了lower
-
再往后的步骤就类似了，直到d出去的k值大于等于当点（3，2）点的H，那么算法停止，也就是说不可能再找到一个更小的k值的点去有可能优化（3，2）了那么程序也就自然而然地结束了。
其实这里也蕴含了一个隐喻，即将一个raise点变为closed状态后再加入openlist时，该点也就变成了lower状态！这其实很合理因为一个点变成raise状态很大可能时H变为cost_max，那么再给他加进去就说明要么L8-L13找到了一条别的路能够把它的H降下来，要么L19-L21别的点能通过他降低H（说明该点的H已经不是原来cost_max，一定降低过，不然别的点怎么可能通过他降低H呢？）又因为每步其实都是找的最小的k，也就是每步都是最优解，因此自然而然的该点也就变成了Lower点！！！！！
C++代码
在实际编程过程中，openlist用了一个优先级队列维护，从而把每次找最小k值的时间复杂度降到O（logn）（因为每次刷新堆为O（logn）），每个节点以链表的数据结构展示，即DNode类，DStar类为主体，它包含了一个存放着map中每一个点的指针的vector，用来保存每一个点的最短路径，同时也可以用来判断一个坐标位置的点是不是New的。
DStar.h
#pragma once
#include
#include
#include
#include
#include
namespace DStar{
enum State
{
New,
Open,
Closed
};
struct DNode{
explicit DNode(int X, int Y, DNode *Next = nullptr) : x(X), y(Y),next(Next) {}
DNode() = default;
void setObstacle()
{
is_obstacle = true;
}
int x = 0;
int y = 0;
int H = 0; // distance to target
int K = 0; //所有时刻最小的H！
State state = New;
DNode *next = nullptr;
bool is_obstacle = false;
};
struct MyCompare
{
bool operator()(DNode *lhs,DNode *rhs) const{
return lhs->K > rhs->K;
}
};
class DStar{
public:
DStar(const std::vector> map) : Map(map), shortestPaths(map.size(), std::vector(map.front().size(),nullptr)){}
void find_first_path(const std::pair &start, const std::pair &target, const std::vector> &obstacles);
~DStar()
{
for (int i = 0; i < shortestPaths.size();++i)
{
for (int j = 0; j < shortestPaths.front().size();++j)
{
if(shortestPaths[i][j])
delete shortestPaths[i][j];
}
}
}
private:
int cost(DNode *lhs, DNode *rhs) const;
void insert(DNode *node,int Cost);
//核心函数
int process_state();
int modify(DNode *node);
void setObstacles(const std::vector> &obstacles);
private:
const static int _cost_low = 10;
const static int _cost_high = 14;
const static int _cost_max = 65536;
std::priority_queue,MyCompare> openlist;
std::vector> Map;
std::vector> shortestPaths; //保存每个节点到终点的最短路径信息
};
int DStar::cost(DNode *lhs, DNode *rhs) const
{
if (lhs->is_obstacle || rhs->is_obstacle)
return _cost_max;
if (lhs->x != rhs->x && lhs->y != rhs->y)
return _cost_high;
else
return _cost_low;
}
void DStar::insert(DNode *node,int new_H)
{
new_H = (new_H >= _cost_max) ? _cost_max : new_H;
if (node->state == New)
{
node->H = new_H;
node->K = new_H;
node->state = Open;
shortestPaths[node->x][node->y] = node;
openlist.push(node);
}
else if (node->state == Open) //节点已经在openlist中 那么只需要改变对应openlist位置的节点的H和K即可
{
node->H = new_H;
node->K = std::min(node->K, new_H);
}
else
{
node->K = std::min(node->H, new_H);
node->H = new_H;
node->state = Open;
openlist.push(node);
}
}
int DStar::process_state()
{
DNode *cur_min = openlist.top();
if (!cur_min)
return -1;
openlist.pop();
cur_min->state = Closed;
int K_old = cur_min->K;
if (K_old < cur_min->H)
{
for (int i = cur_min->x - 1; i <= cur_min->x + 1; ++i)
{
for (int j = cur_min->y - 1; j <= cur_min->y + 1; ++j)
{
if (i >= 0 && j >= 0 && i < Map.size() && j < Map.front().size() && shortestPaths[i][j])
{
if (i == cur_min->x && j == cur_min->y)
continue;
if (shortestPaths[i][j]->H <= K_old && cur_min->H > shortestPaths[i][j]->H + cost(cur_min, shortestPaths[i][j]))
{
cur_min->H = shortestPaths[i][j]->H + cost(cur_min, shortestPaths[i][j]);
cur_min->next = shortestPaths[i][j];
}
}
}
}
}
if (K_old == cur_min->H)
{
for (int i = cur_min->x - 1; i <= cur_min->x + 1; ++i)
{
for (int j = cur_min->y - 1; j <= cur_min->y + 1; ++j)
{
if (i >= 0 && j >= 0 && i < Map.size() && j < Map.front().size())
{
if (i == cur_min->x && j == cur_min->y)
continue;
if (!shortestPaths[i][j] && !Map[i][j]) //表示当前节点为New
{
DNode *neighbor = new DNode(i, j, cur_min);
insert(neighbor, cur_min->H + cost(neighbor, cur_min));
}
else if (shortestPaths[i][j] && !shortestPaths[i][j]->is_obstacle)
{
if ((shortestPaths[i][j]->next == cur_min && shortestPaths[i][j]->H != cur_min->H + cost(shortestPaths[i][j], cur_min))
|| (shortestPaths[i][j]->next != cur_min && shortestPaths[i][j]->H > cur_min->H + cost(shortestPaths[i][j], cur_min)))
{
insert(shortestPaths[i][j],cur_min->H + cost(shortestPaths[i][j], cur_min));
shortestPaths[i][j]->next = cur_min;
}
}
}
}
}
}
else
{
//传递raise状态
for (int i = cur_min->x - 1; i <= cur_min->x + 1; ++i)
{
for (int j = cur_min->y - 1; j <= cur_min->y + 1; ++j)
{
if (i == cur_min->x && j == cur_min->y)
continue;
if (i >= 0 && j >= 0 && i < Map.size() && j < Map.front().size())
{
if (!shortestPaths[i][j] && !Map[i][j]) //表示当前节点为New
{
DNode *neighbor = new DNode(i, j, cur_min);
insert(neighbor, cur_min->H + cost(neighbor, cur_min));
}
else if(shortestPaths[i][j] && !shortestPaths[i][j]->is_obstacle)
{
if (shortestPaths[i][j]->next == cur_min && shortestPaths[i][j]->H != cur_min->H + cost(cur_min, shortestPaths[i][j]))
{
insert(shortestPaths[i][j], cur_min->H + cost(cur_min, shortestPaths[i][j]));
}
else
{
if (shortestPaths[i][j]->next != cur_min)
{
int temp = (shortestPaths[i][j]->H + cost(shortestPaths[i][j], cur_min) >= _cost_max) ? _cost_max : shortestPaths[i][j]->H + cost(shortestPaths[i][j], cur_min);
if(cur_min->H < temp)
insert(cur_min, cur_min->H);
else if(shortestPaths[i][j]->next != cur_min && cur_min->H > shortestPaths[i][j]->H + cost(shortestPaths[i][j], cur_min)
&& shortestPaths[i][j]->state==Closed && shortestPaths[i][j]->K>K_old)
{
insert(shortestPaths[i][j], shortestPaths[i][j]->H);
}
}
}
}
}
}
}
}
return openlist.top()->K;
}
void DStar::setObstacles(const std::vector> &obstacles)
{
if (obstacles.empty())
return;
for (const auto &rhs : obstacles)
{
if(shortestPaths[rhs.first][rhs.second])
{
shortestPaths[rhs.first][rhs.second]->is_obstacle = true;
shortestPaths[rhs.first][rhs.second]->H = _cost_max;
}
Map[rhs.first][rhs.second] = '#';
}
}
int DStar::modify(DNode *node)
{
DNode *next = node->next;
openlist.push(next);
int k_old;
do{
k_old = process_state();
if (k_old == -1)
break;
} while (node->H == _cost_max || k_old < node->H);
return k_old;
}
void DStar::find_first_path(const std::pair &start, const std::pair &target,
const std::vector> &obstacles)
{
DNode *end = new DNode(target.first, target.second);
shortestPaths[target.first][target.second] = end;
openlist.push(end);
while (true)
{
process_state();
if (shortestPaths[start.first][start.second] && shortestPaths[start.first][start.second]->state == Closed)
break;
}
setObstacles(obstacles);
DNode *begin = shortestPaths[start.first][start.second];
int i;
while (begin)
{
Map[begin->x][begin->y] = '*';
if (begin->next && begin->next->is_obstacle)
{
i = modify(begin);
if (i == -1)
{
break;
}
}
begin = begin->next;
}
if (i == -1)
{
std::cout << "No New Path To Target" << std::endl;
return;
}
else
{
for (const auto &i : Map)
{
for (const auto &j : i)
{
if (j > 1)
std::cout << char(j)<<" ";
else
std::cout << j<<" ";
}
std::cout << std::endl;
}
}
}
}
测试用例部分
#include
#include"DStar.h"
int main()
{
std::vector> map2(25, std::vector(25));
for (int j = 3; j < 8; ++j)
map2[6][j] = 1;
for (int j = 4; j < 7; ++j)
map2[7][j] = 1;
clock_t start = clock();
DStar::DStar d(map2);
d.find_first_path({3, 2}, {23, 16}, {{12, 4}, {12, 5}, {12, 6}, {12, 7}, {12, 8}, {12, 9}, {12, 10}, {12, 11}, {11, 7}, {13, 3}, {10, 6}, {14, 5}, {6, 2}});
clock_t ends = clock();
std::cout <<"Running Time : "<<(double)(ends - start)/ CLOCKS_PER_SEC *1000<<"ms"<< std::endl;
return 0;
}
未设置障碍时候
设置障碍后：
微信扫一扫
支付宝扫一扫
评论列表（0条）