路径规划 | 图解LPA*算法(附ROS C++/Python/Matlab仿真)

0 专栏介绍

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

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

---

1 什么是LPA*算法？

在路径规划 | 图解动态A*(D*)算法(附ROS C++/Python/Matlab仿真)中我们介绍了D*算法，它最大的优势是可以同时兼容静态环境和存在未知动态变化的场景。

然而，D*虽然具备动态性，但由于算法只传播障碍信息，故规划路径只能保证可行性。例如D*算法中的实例

在将障碍移除后，该信息不会被感知。而终身规划A*(Lifelong Planning A*, LPA*)算法通过结合增量式和启发式算法，在保证动态可行性的同时增强了最优性。

2 LPA*算法核心概念一览

LPA*算法的核心概念如下所示：

• $c\left(x,y\right)$：从节点$x$移动到节点$y$的代价，若$x$、$y$间存在障碍则$c\left(x,y\right)=\inf$；
• $g\left(x\right)$：从节点$x$到起点的预测最短距离，类似D*算法中的$k\left(x\right)$，但$k\left(x\right)$是单调递减的历史最小值，而$g\left(x\right)$可能增大；
• $rhs\left(x\right)$：当前节点$x$到起点的数值最短距离，计算方法是

$$rhs\left(x\right)=\underset{x^{\prime }\in \mathrm{neighbor}\left(x\right)}{\min }\left\{g\left(x^{\prime }\right)+c\left(x,x^{\prime }\right)\right\}$$

• 节点状态：LPA*根据$g\left(x\right)$与$rhs\left(x\right)$的关系将节点分为三种状态

  1. 局部一致状态$g\left(x\right)=rhs\left(x\right)$：表明节点$x$处暂时没有更优路径；
  2. 局部过一致状态$g\left(x\right)>rhs\left(x\right)$：表明节点$x$处存在更理想的父节点使路径更优，此时将设置$g\left(x\right)=rhs\left(x\right)$，节点便恢复为局部一致状态；
  3. 局部欠一致状态$g\left(x\right)<rhs\left(x\right)$：表明节点$x$或其邻域内出现动态障碍使$rhs\left(x\right)$异常升高，此时将设置$g\left(x\right)=\infty$，节点过渡到局部过一致状态，在后续转为情况(2)处理；

• $U$：存放局部不一致节点的优先级队列，键值为
  $$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,goal\right)\\ \min \left\{g\left(x\right),rhs\left(x\right)\right\}\end{array}\right]$$

  按照先$k_1$后$k_2$的优先级弹出数值较小的节点，$h(.)$是启发函数，LPA*算法三种节点状态的转换关系如下所示

• $\mathrm{updateVertex}\left(x\right)$：核心函数，修正节点$x$的$rhs\left(x\right)$值并传播信息到邻域；
• $\mathrm{computeShortestPath}\left(\right)$：核心函数，感知动态障碍信息(体现为节点突变为局部欠一致状态)，将局部不一致节点优化到局部一致状态，从而获得最优可行路径。

3 LPA*算法流程

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

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

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

4 步步图解：算法实例

以下面的栅格地图为例，红色栅格表示起点，蓝色栅格表示终点，黄色栅格表示开节点表中的节点

LPA*算法的静态规划阶段如下图所示

LPA*算法动态路径修正阶段如下所示

5 算法仿真与实现

5.1 ROS C++实现

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

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

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

    // goal reached
    if (u->key >= this->calculateKey(goal_ptr_) && goal_ptr_->rhs == goal_ptr_->g_)
      break;

    // Locally over-consistent -> Locally consistent
    if (u->g_ > u->rhs)
    {
      u->g_ = u->rhs;
    }
    // Locally under-consistent -> Locally over-consistent
    else
    {
      u->g_ = INF;
      this->updateVertex(u);
    }

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

5.2 Python实现

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

def computeShortestPath(self) -> None:
    '''
    Perceived dynamic obstacle information to optimize global path.
    '''
    while True:
        node = min(self.U, key=lambda node: node.key)
        if node.key >= self.calculateKey(self.goal) and \
                self.goal.rhs == self.goal.g:
            break

        self.U.remove(node)
        self.EXPAND.append(node)

        # Locally over-consistent -> Locally consistent
        if node.g > node.rhs:
            node.g = node.rhs
        # 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深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系