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*算法的核心概念如下所示：
：从节点
移动到节点
的代价，若
；
：从节点
，但
：当前节点
-
节点状态：LPA*根据
- 局部一致状态
：表明节点
- 局部过一致状态
：表明节点
- 局部欠一致状态
：表明节点
，节点过渡到局部过一致状态，在后续转为情况(2)处理；
- 局部一致状态
-
：存放局部不一致节点的优先级队列，键值为
按照先
后
的优先级弹出数值较小的节点，
是启发函数，LPA*算法三种节点状态的转换关系如下所示
：核心函数，修正节点
：核心函数，感知动态障碍信息(体现为节点突变为局部欠一致状态)，将局部不一致节点优化到局部一致状态，从而获得最优可行路径。
3 LPA*算法流程
LPA*算法主函数流程如下所示
核心函数如下所示
4 步步图解：算法实例
以下面的栅格地图为例，红色栅格表示起点，蓝色栅格表示终点，黄色栅格表示开节点表中的节点
LPA*算法的静态规划阶段如下图所示
LPA*算法动态路径修正阶段如下所示
5 算法仿真与实现
5.1 ROS C++实现
核心函数如下所示
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实现
核心函数如下所示
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)
完整工程代码请联系下方博主名片获取
🔥 更多精彩专栏：
文章出处登录后可见！
已经登录？立即刷新
