目录
- 0 专栏介绍
- 1 什么是D* Lite算法？
- 2 自适应修正项
- 3 D* Lite算法流程
- 4 算法仿真与实现
-
- 4.1 ROS C++实现
- 4.2 Python实现
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
(
⋅
,
⋅
)
hleft( cdot ,cdot right)
h(⋅,⋅)修改为指向起点，但是这样会产生冲突：机器人运行过程中将当前位置作为新起点，此后计算的
h
h
h值是扩展点与新起点间的代价；而第一次规划时计算的
h
h
h值是扩展点与初始起点间的代价。
为解决这个问题，D* Lite向启发式引入一个修正项
k
m
←
k
m
+
h
(
s
t
a
r
t
∗
,
s
t
a
r
t
)
kmgets km+hleft( start^*,start right)
km←km+h(start∗,start)
此时优先级队列
U
U
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{ gleft( x right) ,rhsleft( x right) right} +hleft( x,start right) +km\ min left{ gleft( x right) ,rhsleft( x right) right}\end{array} right]
k=[k1k2]=[min{g(x),rhs(x)}+h(x,start)+kmmin{g(x),rhs(x)}]
在D* Lite算法中，即使机器人当前位置已经发生改变，依然可以充分利用之前的信息进行动态规划，而不需要重新进行无先验信息的静态规划，可视为LPA*算法的改良版本。
3 D* Lite算法流程
D* Lite算法主函数流程如下所示
其中的核心函数
u
p
d
a
t
e
V
e
r
t
e
x
(
x
)
mathrm{updateVertex}left( x right)
updateVertex(x)如下所示
核心函数
c
o
m
p
u
t
e
S
h
o
r
t
e
s
t
P
a
t
h
(
)
mathrm{computeShortestPath}left( right)
computeShortestPath()如下所示
可以看出其实D* Lite算法和LPA*算法并没有本质区别，只是引入一个工程技巧便可以使算法整体的效率提升
4 算法仿真与实现
4.1 ROS C++实现
核心函数
u
p
d
a
t
e
V
e
r
t
e
x
(
x
)
mathrm{updateVertex}left( x right)
updateVertex(x)的实现
void DStarLite::updateVertex(LNodePtr u)
{
// u != goal
if (u->x_ != goal_.x_ || u->y_ != goal_.y_)
{
std::vectorLNodePtr> neigbours;
getNeighbours(u, neigbours);
// min_{sin 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));
}
}
核心函数
c
o
m
p
u
t
e
S
h
o
r
t
e
s
t
P
a
t
h
(
)
mathrm{computeShortestPath}left( right)
computeShortestPath()的实现
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::vectorLNodePtr> neigbours;
getNeighbours(u, neigbours);
for (LNodePtr s : neigbours)
updateVertex(s);
}
}
最终效果
4.2 Python实现
核心函数
c
o
m
p
u
t
e
S
h
o
r
t
e
s
t
P
a
t
h
(
)
mathrm{computeShortestPath}left( right)
computeShortestPath()的实现
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深度学习实战》
- 《机器学习强基计划》
- 《运动规划实战精讲》
- …
👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇
