0 专栏介绍

🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备！详细介绍全局规划(图搜索、采样法、智能算法等)；局部规划(DWA、APF等)；曲线优化(贝塞尔曲线、B样条曲线等)。

🚀详情：图解自动驾驶中的运动规划(Motion Planning)，附几十种规划算法

1 什么是D* Lite算法？

上节我们介绍了LPA*算法：路径规划 | 图解LPA*算法(附ROS C++/Python/Matlab仿真)。然而LPA*算法也有缺陷：由于LPA*算法采用从起点开始的前向扩展，所以当机器人开始移动后，若再遇到动态障碍则需要更新全体节点信息，LPA*算法将无法实现增量规划。换言之，LPA*算法的局限性是只能实现一次增量——必须固定起点，在此基础上引入动态障碍才能修正路径。

D* Lite算法结合D*算法反向搜索策略优化LPA*算法框架，使其可适应变起点的路径修正，最初由Sven Koenig和Maxim Likhachev在 2002 年提出。关于D*算法的原理，不熟悉的同学可以参考路径规划 | 图解动态A*(D*)算法(附ROS C++/Python/Matlab仿真)

2 自适应修正项

D* Lite算法是如何实现变起点路径修正的呢？

具体地，将指向终点的启发式$h\left(?,?\right)$修改为指向起点，但是这样会产生冲突：机器人运行过程中将当前位置作为新起点，此后计算的$h$值是扩展点与新起点间的代价；而第一次规划时计算的$h$值是扩展点与初始起点间的代价。

为解决这个问题，D* Lite向启发式引入一个修正项

$$km\leftarrow km+h\left(star{t}^{?},start\right)$$

此时优先级队列$U$的键值为

k = [ k 1 k 2 ] = [ min ? { g ( x ) , r h s ( x ) } + h ( x , s t a r t ) + k m min ? { g ( x ) , r h s ( x ) } ] k=\left[ \begin{array}{c} k_1\\ k_2\\\end{array} \right] =\left[ \begin{array}{c} \min \left\{ g\left( x \right) ,rhs\left( x \right) \right\} +h\left( x,start \right) +km\\ \min \left\{ g\left( x \right) ,rhs\left( x \right) \right\}\\\end{array} \right] k=[k1?k2??]=[min{g(x),rhs(x)}+h(x,start)+kmmin{g(x),rhs(x)}?]

在D* Lite算法中，即使机器人当前位置已经发生改变，依然可以充分利用之前的信息进行动态规划，而不需要重新进行无先验信息的静态规划，可视为LPA*算法的改良版本。

3 D* Lite算法流程

D* Lite算法主函数流程如下所示

其中的核心函数$\mathrm{updateVertex}\left(x\right)$如下所示

核心函数$\mathrm{computeShortestPath}\left(\right)$如下所示

可以看出其实D* Lite算法和LPA*算法并没有本质区别，只是引入一个工程技巧便可以使算法整体的效率提升

4 算法仿真与实现

4.1 ROS C++实现

核心函数$\mathrm{updateVertex}\left(x\right)$的实现

void DStarLite::updateVertex(LNodePtr u)
{
  // u != goal
  if (u->x_ != goal_.x_ || u->y_ != goal_.y_)
  {
    std::vector<LNodePtr> neigbours;
    getNeighbours(u, neigbours);

    // min_{s\in pred(u)}(g(s) + c(s, u))
    u->rhs = INF;
    for (LNodePtr s : neigbours)
    {
      if (s->g_ + getCost(s, u) < u->rhs)
      {
        u->rhs = s->g_ + getCost(s, u);
      }
    }
  }

  // u in openlist, remove u
  if (u->open_it != open_list_.end())
  {
    open_list_.erase(u->open_it);
    u->open_it = open_list_.end();
  }

  // g(u) != rhs(u)
  if (u->g_ != u->rhs)
  {
    u->key = calculateKey(u);
    u->open_it = open_list_.insert(std::make_pair(u->key, u));
  }
}

核心函数$\mathrm{computeShortestPath}\left(\right)$的实现

void DStarLite::computeShortestPath()
{
  while (1)
  {
    if (open_list_.empty())
      break;

    double k_old = open_list_.begin()->first;
    LNodePtr u = open_list_.begin()->second;
    open_list_.erase(open_list_.begin());
    u->open_it = open_list_.end();
    expand_.push_back(*u);

    // start reached
    if (u->key >= calculateKey(start_ptr_) && start_ptr_->rhs == start_ptr_->g_)
      break;

    // affected by obstacles
    if (k_old < calculateKey(u))
    {
      u->key = calculateKey(u);
      u->open_it = open_list_.insert(std::make_pair(u->key, u));
    }
    // Locally over-consistent -> Locally consistent
    else if (u->g_ > u->rhs)
    {
      u->g_ = u->rhs;
    }
    // Locally under-consistent -> Locally over-consistent
    else
    {
      u->g_ = INF;
      updateVertex(u);
    }

    std::vector<LNodePtr> neigbours;
    getNeighbours(u, neigbours);
    for (LNodePtr s : neigbours)
      updateVertex(s);
  }
}

最终效果

4.2 Python实现

核心函数$\mathrm{computeShortestPath}\left(\right)$的实现

def computeShortestPath(self) -> None:
	while True:
	    node = min(self.U, key=lambda node: node.key)
	    if node.key >= self.calculateKey(self.start) and \
	            self.start.rhs == self.start.g:
	        break
	
	    self.U.remove(node)
	    self.EXPAND.append(node)
	
	    # affected by obstacles
	    if node.key < self.calculateKey(node):
	        node.key = self.calculateKey(node)
	        heapq.heappush(self.U, node)
	    # Locally over-consistent -> Locally consistent
	    elif node.g > node.rhs:
	        node.g = node.rhs
	        for node_n in self.getNeighbor(node):
	            self.updateVertex(node_n)
	    # Locally under-consistent -> Locally over-consistent
	    else:
	        node.g = float("inf")
	        self.updateVertex(node)
	        for node_n in self.getNeighbor(node):
	            self.updateVertex(node_n)

可视化效果

完整工程代码请联系下方博主名片获取

🔥 更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …