Hybrid Astar 算法剖析和实现（二）-CFANZ编程社区

文章目录

0 前言

本篇承接上篇，主要对状态空间栅格作一个介绍，并讨论如何将理论落地为代码。

1 什么是状态空间栅格

状态空间栅格是对状态空间的离散化。

下面对状态空间和离散化分别作一个简要说明。

状态空间

状态空间就是以单位状态向量为基底的所有向量构成的抽象空间，可以类比笛卡尔坐标系。那什么又是状态向量呢？

如果将车辆看成一个质点，那么车辆的笛卡尔坐标(x,y)就可以看作是车辆的状态向量。此时，使用Astar算法就绰绰有余了，因为当车辆被看作一个质点时，不存在运动学约束（可以简单理解为转弯半径的约束）。

但真实的车辆一般被抽象为一个刚体，刚体有大小，也就有朝向，因此需要新增一个维度 $\theta$ 来对该状态进行描述。那么车辆此时的状态向量就是 $(x,y,\theta)$ 了。

离散化

离散化就是针对状态向量的每一个维度值进行切分，切分的间隔就是离散的分辨率 $r e s o l u t i o n$ ，离散之后相当于将连续的状态向量 $(x,y,\theta)$ 映射到了离散空间当中，对应离散空间中的离散状态向量 $(x_{index},y_{index},\theta_{index})$ 。

连续状态向量到离散状态向量的转换关系如下式：
$\begin{cases} x_{index} = \lfloor{\frac{x-x_{min}}{resolution_x}}\rfloor\\[3ex] y_{index} = \lfloor{\frac{y-y_{min}}{resolution_y}}\rfloor\\[3ex] \theta_{index} = \lfloor{\frac{\theta-\theta_{min}}{resolution_\theta}}\rfloor \end{cases}\tag1$
离散状态向量到连续状态向量的转换关系如下式：
$\begin{cases} 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} \end{cases}\tag2$

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;
}