D*算法-动态路径/运动规划

目录

D*算法-动态路径/运动规划

1 前言

1.1 命名由来

1.2 使用场合

2 算法逻辑

2.1 主流程

2.2 核心函数

3 要点探讨

3.1 D* 主要特点

3.2 D* 和Dijkstra、A* 到底什么关系

4 实例

5 参考资料

5.1 原文

1 前言

1.1 命名由来

原话：The name of the algorithm, D*, was chosen because it resembles A*

解释：就是觉着像A*干脆起名D*，说是Dynamic A* 不太严谨

1.2 使用场合

适用于动态环境（/路网）中的路径/运动规划（motion planning)，与Dijkstra、A* 形成对比（两者适用于静态环境/路网）

解释：

• 动态是指在沿着最优路径前进时，突然出现了既定路径中断的情况需要进行重新规划。

• 路径中断在现实生活中可以理解为诸如塌方、巨石、车祸、阻塞等情况的发生，在图/Graph中是某条边Edge/弧Arc的代价/权重cost/weight突然变得非常大，使得当前路径成本陡增，需要重新规划。

• 当然该方法也可以应用在静态环境/路网中，向下兼容，实际就相当于反向（从终点G开始求解）Dijkstra算法。

• 该方法不是唯一的算法，比如在遇到障碍时再次调用A*（参考A* replanner）也是可以的，但是该方法计算成本低，效率高。

• 同时该方法对已知局部环境（类似游戏中的趟地图开荒）、未知环境情况下也有很好的实用性。

2 算法逻辑

2.1 主流程

2.1.1 情形模拟

• 假设有一机器人任务是从起点S到终点G，首先寻找一条最优路径{S, ……, Y, X, ……，G}

• 之后沿该路径行驶过程中在Y处发现X点塌方（无法到达），需要寻找新的路径{ Y, Z, ……，G}继续行驶。

2.1.2 符号约定

1、有向图：D* 算法以有向图作为前提，即在A、B节点间，“A到B” 与 “B到A” 表示两个弧，因此对应的权重不一定一样。

2、X、Y 、Z、S、G等：称为节点，也称为state，可理解为一个坐标position，其中S表示Start，G表示Goal。

3、{S，……，G}：表示一条路径，该路径以S为起点，G为终点，简记为{S}。

4、c(X, Y)：表示“Y->X”节点的成本/权重（cost），具体计算逻辑如下。

• 以Y为中心构成8邻域的九宫格

• 当不存在障碍物时，横向、竖向的连接/移动成本为1、斜向连接成本为1.4

• 当存在障碍物时，不管什么方向的连接/移动成本都设置为比较大的值，比如10000，以确保很难通过

• 虽然是有向图，但是在此文中可以设定双向移动成本一样即c(X, Y) == c(Y, X)

5、b(X) = Y： 用来记录反向指针（backpointer)，即X->Y，当处于X位置时可以根据b(X)获得下一个要到达的节点Y，以使得路径最优

6、t(X)：节点的访问标记位（tag），支持3个类型：NEW、OPEN、CLOSED

• NEW：节点从未访问过，最初的设置，可能存在部分节点永远处于改状态

• OPEN：节点正在访问中，存储在OPEN_LIST（使用优先队列PriorityQueue实现）中，也称为OPEN_QUEUE，以待访问扩展

• CLOSED：节点已经被访问过，从优先队列中移除，并完成了相邻节点的扩展，一般找到了到达重点G的最优路径

7、h(X)：用来存储路径代价，指从X到达终点G的路径（{X,……G}，简记为{X}）代价，不一定是全局最优。

8、k(X)：用来记录自X节点被加入到OPEN_LIST中后的最小h(X)值（具体计算方式由Insert函数决定）,也是优先队列OPEN_LIST的排序依据。

同时h(X)与k(X)的大小关系也决定了PROCESS_STATE()的执行逻辑，进行最优选择还是异常信息传播

2.1.3 过程拆解

• P1、构建一条从S到G的路径{S, ……, Y, X, ……，G}，采用反向构建路径的方法（以G为起点的Dijkstra算法），在PROCESS_STATE()函数的部分分支逻辑中实现

注：图中的每一个点在栅格化的图上称为state（简单理解为一个position），在传统图Graph中称为节点、顶点。

• P2、机器人沿着{S, ……, Y, X, ……，G}路径行驶，直到发现异常点（塌方，不可到达）后停下来，在Y处发现X塌方，停在Y。

• P3、修正塌方节点X与周边节点间（最不可少的是Y）的访问成本（代价/权重），在MODIFY_COST()函数中实现

• P4、然后将节点X阻塞的信息传播到临近周边节点（最重要的是传到Y）中，并同时进行新路径的规划，得到新的路径{ Y, Z, ……，G}，在PROCESS_STATE()函数的部分分支逻辑中实现

总结：

整个过程核心是PROCESS_STATE()和MODIFY_COST()函数的实现，其中前者负责了最优路径的规划（P1、P4：寻找代价最小的路径）和阻塞信息的传播（P4：将突然增加的访问成本增加到相关联的节点上，促使路径的更新）

2.1.4 主逻辑伪代码

//S is the start State
 //G is the goal State
 h(G) = 0;
 put_into_OPEN_QUEUE(G); //将G加入到OPEN_QUEUE/OPEN_LIST中
 do {
     k_min = PROCESS_STATE(); //主要是减少cost，寻找最优路径
 }while(k_min != -1 && S_not_removed_from_open_list);
 if(k_min == -1){
     G_unreachable;
     exit;
 }else{
     do{
         do{
             trace_optimal_path(); //沿着最优路径行驶{S, ……, G}，根据具体情形来实现
         }while(G_is_not_reached && map == environment); //map ！= environment 遇到了异常，塌陷、障碍物等
         if(G_is_reached){ //到达终点
             exit;
         }else{
             X = state_of_discrepancy_reached_trying_to_move_from_some_state_Y;
             MODIFY_COST(X, Y, new_c(X, Y));
             do{
                 k_min = PROCESS_STATE();   //减少cost 和 异常信息传播
             }while(k_min != -1 && k_min<h(Y)); //k_min>=h(Y)时表示已经找到了最优路径，不可能有更优
             if(k_min == -1){
                 exit;
             }
         }
     }while(1);
 }

2.2 核心函数

2.2.1 状态处理：PROCESS_STATE()

PROCESS_STATE()主要解决两大问题，1、计算最优路径（传播cost减少）；2、传播异常信息（传播cost增加）。以上两种情况分别用k(X)与h(X)的关系来进行控制。假设X是从优先队列OPEN_QUEUE中弹出来的最优节点

• 当节点X的k(X)==h(X)时，称X为LOWER State，此时进行路径的优化，试图减少周围节点的cost

• 当节点X的k(X)<h(X)时，称X为RAISE State，此时进行异常信息的传播，将cost的增加传给周围节点

具体伪代码如下：PROCESS_STATE()

注意：前序必要准备工作已经完成：

• t(·)=NEW //t(·)表示所有节点的t()值

• h(G)=0

• G被加入到了OPEN_QUEUE中

// PROCESS_STATE()
 X = MIN_STATE()
 if(X==NULL){
     return -1
 }
 k_old = GET_KMIN()；
 DELETE(X);
 if( k_old < h(X) ){
     for( each neighbor Y of X ){
         if( h(Y) <= k_old  and  h(X) > h(Y) + c(Y, X) ){ //尝试在现有路径条件下，先更新当前cost
               b(X) = Y;
               h(X) = h(Y) + c(Y, X); 
         }
     }
 }
 if( k_old == h(X) ){
     for( each neighbor Y of X ){
         if( t(Y) == NEW or
             ( b(Y) == X and h(Y) != h(X)+c(X,Y) ) or
             ( b(Y) != X and h(Y) > h(X)+c(X,Y) ) ){ //正常扩展
             b(Y) = X;
             INSERT( Y, h(X)+c(X,Y) );
         } 
     }
 }else{   //k_old < h(X) condition
     for( each neighbor Y of X){
         if( t(Y) == NEW or
             ( b(Y) == X and h(Y) != h(X)+c(X,Y) ) ){ //传递异常信息c(X,Y)带来的变化
             b(Y) == X;
             INSERT( Y, h(X)+c(X,Y) )
         }else{
             if( b(Y) != X and h(Y) > h(X) + c(X, Y) ){ //将X再次插入队列，从RAISE模式变成了LOWER模式来扩展修正Y
                 INSERT( X, h(X) ); 
             }else{
                 if( b(Y) != X and h(X) > h(Y)+c(Y,X) and 
                     t(Y) == CLOSED and h(Y) > k_old ){  //寻找次优解，新的路径
                     INSERT( Y, h(Y) );
                 }
             }
         }
     }
 }
 return GET_KMIN()

一些辅助函数，都是基于OPEN_QUEUE进行的操作

// MIN_STATE()
 Return X if k(X) is minimum in the OPEN_QUEUE
 ​
 // GET_KMIN()
 Return the minimum value of k in the OPEN_QUEUE
 ​
 // DELETE(X)
 delete X state from the OPEN_QUEUE
 ​
 // INSERT(X, h_new) 
 // 隐含着将X节点加入到OPEN_QUOPEN(如果不存在的的话)
 if( t(X) == NEW ){
     k(X) = h_new;
 }else if( t(X) == OPEN ){
     k(X) = min( k(X), h_new );
 }else{ // t(X) == CLOSED
     k(X) = min( h(X), h_new ); //可使得X节点从RAISE进入LOWER模式
 }
 h(X) = h_new;
 t(X) = OPEN;
 SORT_OPEN_QUEUE_BY_k() //对OPEN_QUEUE按k进行排序

2.2.2 代价修改：MODIFY_COST()

代价的修改是在发现障碍物所处的state时进行的，不是机器人所处的位置

假设机器人在Y正准备前往X出前（即Y->X链接关系），发现了X处突然塌陷或者出现了障碍物则进行cost的修正

MODIFY_COST( X, Y, new_c(X,Y) ) 伪代码如下

 // MODIFY_COST( X, Y, new_c(X,Y) )
 c(X, Y) = new_c(X, Y); //做cost修正
 if( t(X) == CLOSED ){
     INSERT( X, h(X) ); //将X重新插入队列，X处于LOWER模式，进行扩展，进而引起Y的路径发生变动，Y变成RAISE模式
 }
 return GET_KMIN();

说明：

MODIFY_COST()函数只是完成了某一个弧的代价修正，实际一个节点上障碍物的出现可能导致周边的节点都受到影响，尤其是b(·)==X的节点，所以最好的办法是所有的关联节点都进行修正，并且是双边修正。

在主函数中X = state_of_discrepancy_reached_trying_to_move_from_some_state_Y; MODIFY_COST(X, Y, new_c(X, Y));只修复了一个节点的单边关系，虽然能够解决当前问题（修正h(Y)），但是X节点自身的h(X)值并未因塌陷/障碍物的出现而得到修正

要想对h(X)进行修正就需要b(X)的节点加入到OPEN_QUEUE中进行扩展以修正h(X)，也可把X的8领域+X全部加入到OPEN_QUEUE中以实现全覆盖。

3 要点探讨

3.1 D* 主要特点

• 反向规划初始路径，S到G路径的搜索一般用S作为起点采用Dijkstra算法，在D*中采用G作为起点进行搜索，形成反向路径树，所有节点的backpointer都最终指向了终点G（一般指向起点S），当遇到障碍需要再次进行路径规划时，此时的h(X)值就充当了启发函数，实现了A*算法的效果。

• 在一个函数（PROCESS_STATE()）中实现了两个任务过程（最优路径的搜索，障碍信息的传播），通过k(X)、h(X)的差异对比形成RAISE、LOWER两种状态，实现两个任务的切换控制。

3.2 D* 和Dijkstra、A* 到底什么关系

• D* 主要应用于动态环境下的路径优化，也能兼容静态环境，而Dijkstra、A* 只能用于静态环境下的路径优化

• D* 的实现过程中有和Dijkstra、A* 相似的思想，在初始路径规划的过程中，是一个反向Dijkstra算法，在遇障碍重新规划的过程中，可看做是一个启发式算法A*，只是起点是一些不固定的点，h()充当了启发函数的角色。

4 实例

待更新

5 参考资料

5.1 原文

Optimal and Efficient Path Planning for Partially-Known Environments

更多阅读

【算法】图解A* 搜索算法​​​​​​​