D* Lite算法核心概念
D* Lite是一种增量式的路径规划算法，适用于动态环境，能够高效地重新规划路径，而无需每次都从头开始计算。下表汇总了其实现中的关键数据结构与核心函数：
| 组件类型 | 名称 | 说明 |
|---|---|---|
| 关键数据结构 | 优先队列 (U) | 存储待处理的节点，按键值(key)排序。键值通常包含[k1, k2]，其中k1 = min(g(s), rhs(s)) + h(s_start, s) + km，k2 = min(g(s), rhs(s))。 |
| g值 | 记录节点s到目标点的历史最短代价估计。 | |
| rhs值 | 基于邻居节点g值计算的最小代价，满足rhs(s) = min_{s'∈Succ(s)} (c(s, s') + g(s'))。若g(s) == rhs(s)，则称节点s是一致的。 | |
| 核心函数 | CalculateKey(s) | 计算节点s的键值，用于确定在优先队列中的优先级。 |
| Initialize() | 初始化算法，设置g和rhs的初始值（通常为无穷大），并将目标节点的rhs设为0后加入队列。 | |
| UpdateVertex(u) | 当节点u的rhs值或其邻居信息发生变化时，更新其rhs值及其在队列中的状态。 | |
| ComputeShortestPath() | 核心计算过程，持续扩展节点直到起始节点达到一致且队列顶部的键值不小于起始节点的键值。 |
在D* Lite中，前驱（Pred）和后继（Succ）指的是图的邻接关系。Succ(u)是所有从u出发有边直接到达的节点（即u的后继），而Pred(u)是所有有边直接指向u的节点（即u的前驱）。由于算法是从目标点向起始点反向搜索的，所以在代码中我们通常操作的是节点的前驱集合Pred。
算法实现步骤与C++代码框架
以下是一个简化的D* Lite算法C++实现框架，基于网格地图环境。实际的实现会更复杂，但这能帮你理清主线逻辑。
#include<vector>#include<queue>#include<unordered_map>#include<limits>// 定义节点类型，例如用网格坐标表示structNode{intx,y;// 重载运算符以便用于比较或作为哈希键值booloperator==(constNode&other)const{returnx==other.x&&y==other.y;}};// 为Node特化std::hashnamespacestd{template<>structhash<Node>{size_toperator()(constNode&n)const{returnhash<int>()(n.x)^(hash<int>()(n.y)<<1);}};}classDStarLite{private:std::unordered_map<Node,double>g_;// g值表std::unordered_map<Node,double>rhs_;// rhs值表// 优先队列：元素为 (键值k1, 键值k2, 节点)usingQueueElement=std::tuple<double,double,Node>;std::priority_queue<QueueElement,std::vector<QueueElement>,std::greater<QueueElement>>U_;doublekm_;// 用于键值计算的偏移量Node s_start_,s_goal_,s_last_;// ... 其他成员变量，如地图信息 ...// 计算启发式函数h(s, s')doubleheuristic(constNode&a,constNode&b){// 例如，使用曼哈顿距离或欧几里得距离returnstd::abs(a.x-b.x)+std::abs(a.y-b.y);}// 计算键值std::pair<double,double>CalculateKey(constNode&s){doubleg_val=g_.count(s)?g_[s]:std::numeric_limits<double>::infinity();doublerhs_val=rhs_.count(s)?rhs_[s]:std::numeric_limits<double>::infinity();doublek1=std::min(g_val,rhs_val)+heuristic(s_start_,s)+km_;doublek2=std::min(g_val,rhs_val);return{k1,k2};}// 初始化算法voidInitialize(){U_=std::priority_queue<QueueElement,std::vector<QueueElement>,std::greater<QueueElement>>();km_=0.0;// 初始化所有节点的g和rhs为无穷大g_.clear();rhs_.clear();// 设置目标节点的rhs为0rhs_[s_goal_]=0.0;U_.push({CalculateKey(s_goal_),s_goal_});}// 更新顶点uvoidUpdateVertex(constNode&u){if(u==s_goal_){// 目标节点特殊处理，通常其rhs保持为0return;}// 获取u的所有前驱节点 Pred(u) （在反向搜索中，这些是原图中u的后继）std::vector<Node>predecessors=GetPredecessors(u);// 需要你根据图的结构实现此函数// 计算新的rhs值：取所有 (c(u, s') + g(s')) 的最小值，其中s'属于Pred(u)doublemin_rhs=std::numeric_limits<double>::infinity();for(constNode&pred:predecessors){doublecost=GetCost(u,pred);// 获取边(u, pred)的成本，需要实现。注意方向：在反向搜索中，我们关心从u到pred的成本（原图中是pred到u的成本）。doubleg_pred=g_.count(pred)?g_[pred]:std::numeric_limits<double>::infinity();if(cost+g_pred<min_rhs){min_rhs=cost+g_pred;}}rhs_[u]=min_rhs;// 如果u在队列中，先移除// （在实际实现中，可能需要一个标记或更复杂的队列管理来高效判断和移除）// 这里简化为先尝试移除（如果存在），然后再判断是否需要插入// 更高效的实现可能需要维护一个单独的队列元素存在性标记// 如果g(u) != rhs(u)，则将u以其CalculateKey插入或更新到队列U中doubleg_u=g_.count(u)?g_[u]:std::numeric_limits<double>::infinity();if(g_u!=rhs_[u]){// 这里需要实现U_.update或先删除再插入的逻辑。简单起见，可以先删除再插入，但效率较低。// 假设我们的队列不支持直接update，我们这里先简单推入新键值，在ComputeShortestPath中处理重复节点。autokey=CalculateKey(u);U_.push({key.first,key.second,u});}else{// 如果一致，且u在队列中，则应移除。这里简化处理，依赖ComputeShortestPath中处理无效节点。}}// 计算最短路径voidComputeShortestPath(){while(!U_.empty()){auto[k1_old,k2_old,u]=U_.top();U_.pop();auto[k1_new,k2_new]=CalculateKey(u);// 如果旧的键值小于新的键值，说明节点需要重新以新键值加入队列if(k1_old<k1_new||(k1_old==k1_new&&k2_old<k2_new)){U_.push({k1_new,k2_new,u});}// 如果节点u是过一致的 (g(u) > rhs(u))elseif((g_.count(u)?g_[u]:INFINITY)>(rhs_.count(u)?rhs_[u]:INFINITY)){g_[u]=rhs_[u];// 使节点一致// 更新u的所有前驱节点（在反向搜索中，这些是原图中u的后继）std::vector<Node>predecessors=GetPredecessors(u);for(constNode&pred:predecessors){UpdateVertex(pred);}}// 如果节点u是欠一致的 (g(u) < rhs(u))else{g_[u]=std::numeric_limits<double>::infinity();// 将g(u)设为无穷大，使其变为过一致或未定义// 需要更新u本身及其所有前驱节点UpdateVertex(u);// 更新自身std::vector<Node>predecessors=GetPredecessors(u);for(constNode&pred:predecessors){UpdateVertex(pred);}}// 循环终止条件：起始节点一致且队列顶部的键值不小于起始节点的键值doubleg_start=g_.count(s_start_)?g_[s_start_]:std::numeric_limits<double>::infinity();doublerhs_start=rhs_.count(s_start_)?rhs_[s_start_]:std::numeric_limits<double>::infinity();if(g_start==rhs_start&&(U_.empty()||CalculateKey(U_.top().s)>=CalculateKey(s_start_))){break;}}}public:// 主循环voidMain(){s_last_=s_start_;Initialize();ComputeShortestPath();while(s_start_!=s_goal_){// 如果g(s_start)是无穷大，说明无路径if(g_.count(s_start_)==0||g_[s_start_]==std::numeric_limits<double>::infinity()){// 处理无路径情况break;}// 选择下一个移动点：argmin_{s' in Succ(s_start)} (c(s_start, s') + g(s'))std::vector<Node>successors=GetSuccessors(s_start_);// 获取s_start在原图中的后继节点doublemin_cost=std::numeric_limits<double>::infinity();Node next_node=s_start_;for(constNode&succ:successors){doublecost=GetCost(s_start_,succ);// 获取边(s_start, succ)的成本doubleg_succ=g_.count(succ)?g_[succ]:std::numeric_limits<double>::infinity();if(cost+g_succ<min_cost){min_cost=cost+g_succ;next_node=succ;}}s_start_=next_node;// 移动到下一个节点// 移动后，扫描地图变化（在实际应用中，这里需要你根据传感器信息更新边成本）// 例如：如果检测到边(u, v)的成本发生变化// km_ = km_ + heuristic(s_last_, s_start_);// s_last_ = s_start_;// for each changed edge (u, v):// Update the edge cost c(u, v)// UpdateVertex(u)// ComputeShortestPath(); // 重新规划}}// 需要你实现的辅助函数：std::vector<Node>GetPredecessors(constNode&u);// 在反向搜索中，获取节点u的前驱（即原图结构中的后继）std::vector<Node>GetSuccessors(constNode&u);// 获取节点u在原图结构中的后继doubleGetCost(constNode&from,constNode&to);// 获取两个相邻节点之间的移动成本};实现注意事项与常见问题
- 图的表示与邻居获取：上述代码中的
GetPredecessors、GetSuccessors和GetCost函数需要你根据具体的图结构（如网格地图）来实现。在网格中，一个节点的邻居通常是其上下左右（或包括对角线）的相邻格子。 - 优先队列的高效管理：标准库的
priority_queue不支持直接更新元素的值。一个常见的优化是使用"惰性删除"策略：当从队列顶部弹出节点时，检查其键值是否最新（即其g或rhs值自该元素入队后是否未改变），如果已过时则直接忽略。你也可以考虑使用支持 decrease-key 操作的堆结构。 - 处理动态变化：当检测到边成本变化时（例如，在
Main函数的注释部分），需要更新受影响节点的rhs值并调用UpdateVertex。关键在于只更新成本实际发生变化的边所关联的节点。在机器人路径规划中，通常机器人只感知局部环境的变化。 - 避免循环：不正确的
UpdateVertex逻辑或键值计算可能导致算法在两个节点间无限循环。确保在节点变为一致（g(u) == rhs(u)）时将其从队列中移除（或在键值计算中体现其一致性），并正确更新受影响的邻居。 - 启发式函数的选择：启发式函数
h(s_start, s)应满足可容性（admissible，即不高估真实成本）以保证最优性。在网格中，曼哈顿距离或欧几里得距离是常见选择。
参考代码 D*lite算法的C++实现www.3dddown.com/csa/60495.html
实现 D* Lite 算法确实有一定挑战性，建议从简单的静态环境开始，逐步增加动态障碍物功能，并善加调试。
