Hybrid Astar 算法剖析和实现
Posted 穿越临界点
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了Hybrid Astar 算法剖析和实现相关的知识，希望对你有一定的参考价值。
在学习资料满天飞的大环境下，知识变得非常零散，体系化的知识并不多，这就导致很多人每天都努力学习到感动自己，最终却收效甚微，甚至放弃学习。我的使命就是过滤掉大量的无效信息，将知识体系化，以短平快的方式直达问题本质，把大家从大海捞针的痛苦中解脱出来。
文章目录
0 前言
本篇承接上篇，主要对状态空间栅格作一个介绍，并讨论如何将理论落地为代码。
1 什么是状态空间栅格
状态空间栅格是对状态空间的离散化。
下面对状态空间和离散化分别作一个简要说明。
- 状态空间
状态空间就是以单位状态向量为基底的所有向量构成的抽象空间，可以类比笛卡尔坐标系。那什么又是状态向量呢？
如果将车辆看成一个质点，那么车辆的笛卡尔坐标(x,y)就可以看作是车辆的状态向量。此时，使用Astar算法就绰绰有余了，因为当车辆被看作一个质点时，不存在运动学约束（可以简单理解为转弯半径的约束）。
但真实的车辆一般被抽象为一个刚体，刚体有大小，也就有朝向，因此需要新增一个维度 θ \\theta θ 来对该状态进行描述。那么车辆此时的状态向量就是 ( x , y , θ ) (x,y,\\theta) (x,y,θ) 了。
- 离散化
离散化就是针对状态向量的每一个维度值进行切分，切分的间隔就是离散的分辨率 r e s o l u t i o n resolution resolution ，离散之后相当于将连续的状态向量 ( x , y , θ ) (x,y,\\theta) (x,y,θ) 映射到了离散空间当中，对应离散空间中的离散状态向量 ( x i n d e x , y i n d e x , θ i n d e x ) (x_index,y_index,\\theta_index) (xindex,yindex,θindex) 。
连续状态向量到离散状态向量的转换关系如下式：
x
i
n
d
e
x
=
⌊
x
−
x
m
i
n
r
e
s
o
l
u
t
i
o
n
x
⌋
y
i
n
d
e
x
=
⌊
y
−
y
m
i
n
r
e
s
o
l
u
t
i
o
n
y
⌋
θ
i
n
d
e
x
=
⌊
θ
−
θ
m
i
n
r
e
s
o
l
u
t
i
o
n
θ
⌋
(1)
\\begincases x_index = \\lfloor\\fracx-x_minresolution_x\\rfloor\\\\[3ex] y_index = \\lfloor\\fracy-y_minresolution_y\\rfloor\\\\[3ex] \\theta_index = \\lfloor\\frac\\theta-\\theta_minresolution_\\theta\\rfloor \\endcases\\tag1
⎩⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎧xindex=⌊resolutionxx−xmin⌋yindex=⌊resolutionyy−ymin⌋θindex=⌊resolutionθθ−θmin⌋(1)
离散状态向量到连续状态向量的转换关系如下式：
x
=
x
m
i
n
+
x
i
n
d
e
x
×
r
e
s
o
l
u
t
i
o
n
x
y
=
y
m
i
n
+
y
i
n
d
e
x
×
r
e
s
o
l
u
t
i
o
n
y
θ
=
θ
m
i
n
+
θ
i
n
d
e
x
×
r
e
s
o
l
u
t
i
o
n
θ
(2)
\\begincases x = x_min + x_index \\times resolution_x\\\\[3ex] y = y_min + y_index \\times resolution_y\\\\[3ex] \\theta = \\theta_min + \\theta_index \\times resolution_\\theta \\endcases\\tag2
⎩⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎧x=xmin+xindex×resolutionxy=ymin+yindex×resolutionyθ=θmin+θindex×resolutionθ(2)
2 状态空间栅格的作用
Astar算法使用状态空间栅格对状态空间进行离散化，而后依据状态空间栅格进行 探索 。这里的探索就是子节点的拓展，所以，Astar算法使用状态空间栅格的本质就是为了离散迭代——一步步从局部最优解逼近全局最优解。
Hybrid Astar也是借助状态空间栅格进行子节点的拓展的，但仅仅是借助。
Hybrid Astar子节点拓展最根本的驱动力来自于对车辆控制的采样或者说是对输入的采样。将对输入的采样进行前向模拟（基于车辆运动学方程，后面的文章会详细介绍），此时将车辆的状态映射到状态空间栅格中，并把落入的栅格状态作为拓展之后的节点状态。再根据该状态进行后续的扩展。
有人可能会有一个疑问：为什么要将前向模拟出来的状态映射到状态空间栅格后才作为拓展后的子节点状态呢？直接用前向模拟之后的连续状态作为子节点状态不可以么？个人认为是可以的。
所以，Hybrid Astar算法使用状态空间栅格的作用仅仅是限制了连续状态向量在状态空间的落点；目前，我个人认为在Hybrid Astar算法中可以不使用状态空间栅格。
3 与地图栅格的区别和联系
占据栅格地图中一般使用地图栅格来表示障碍物。和状态空间栅格没有直接关系。
地图栅格在碰撞检测中才会使用，这里暂时不做介绍。
4 代码实现
代码实现参考公式（1）和公式（2）实现。
4.1 连续状态转离散状态
#include <Eigen/Core>
Eigen::Vector3i HybridAStar::State2Index(const Eigen::Vector3d &state) const
Eigen::Vector3i index;
index[0] = (state[0] - x_min_) / STATE_GRID_RESOLUTION;
index[1] = (state[1] - y_min_) / STATE_GRID_RESOLUTION;
index[2] = ((state[2] - (-M_PI)) / ANGULAR_RESOLUTION;
return index;
4.2 离散状态转连续状态
#include <Eigen/Core>
Eigen::Vector3i HybridAStar::Index2State(const Eigen::Vector3i &index) const
Eigen::Vector3d state;
state[0] = index[0] * STATE_GRID_RESOLUTION + x_min_;
state[1] = index[1] * STATE_GRID_RESOLUTION + y_min_;
state[2] = index[2] * ANGULAR_RESOLUTION + (-M_PI);
state[2] = NormalizeAngle(state[2]); //对角度做“归一化”
return index;
Tips：代码中NormalizeAngle的实现请参考角度归一化实现_穿越临界点的博客-CSDN博客
5 状态空间栅格的精度该如何选取
有一个很重要的点就是如何对状态空间栅格的精度进行选取。
该精度的选取需要参照下述原则：
- 一个状态栅格中保证只有一个状态节点落入。
- 状态栅格分辨率一般要高于对控制采样的精度。
- 如果使用状态栅格做碰撞检测则状态栅格的分辨率要不低于地图栅格分辨率（强烈建议不要混用这两种栅格，也就避免了考虑本条原则）。
6 总结
本篇介绍了状态空间栅格，以及它的作用。为后续Hybrid Astar算法的剖析和实现打下了空间基础，不然后面遇到各种坐标和空间变换很容易迷失方向。
下一篇我们一起探索Hybrid Astar的核心逻辑——迭代搜索。
恭喜你又坚持看完了一篇博客，又进步了一点点！如果感觉还不错就点个赞再走吧，你的点赞和关注将是我持续输出的哒哒哒动力~~
以上是关于Hybrid Astar 算法剖析和实现的主要内容，如果未能解决你的问题，请参考以下文章