State Lattice Planner(状态栅格规划)

参考文献：

Efficient constrained path planning via search in state lattices
Differentially Constrained Mobile Robot Motion Planning in State Lattices
Spatiotemporal state lattices for fast trajectory planning in dynamic on-road driving scenarios

1. 整体思想

状态栅格，顾名思义就在在普通的栅格化地图的基础上，加多了机器人运动学模型的约束，确保机器人能沿着相邻两点之间生成的路径运动，并且能够具有状态连续性（速度连续等）。

2. 普通栅格(grid)与状态栅格的异同点

相同之处：它们都是将连续空间离散化，简化路径规划。

不同之处：状态点阵中，每个顶点的连线都是根据机器人的运动学模型生成的，都是切实可行的路径，而普通栅格(grid)的方法并没有结合模型，所以两点之间的路径，机器人并不一定能通过，举个例子：

如果现在机器人是一辆四轮小车，图1是栅格地图，图二是根据四轮小车的运动学模型建立的规则点阵（红色点，没画全，意思一下）。假设小车初始角度水平向右，对于小车而言，图1的蓝色路径，它是没办法走的，因为B点处得转角是90度，而小车转弯是有一定弧度的，对于图2而言，假设小车将的转弯角度离散化成三个方向，每个节点之间的路径都可通行。

3. 状态栅格生成

对于普通栅格点，直接给定栅格距离等，直接生成就好，而状态点阵需要结合机器人的运动空间去生成。

有两种生成方法，分别为对控制空间的采样和对状态空间的采样。

3.1 控制空间的采样（前向采样）

已知机器人的运动学模型，则输入我们输入控制量u和积分时间T,向前积分预测后面时刻机器人的状态，代码里这里的操作是暴力遍历，从而得到一系列状态的集合，成为控制空间的采样。例如下图：a图是四轮小车，b图是小车当前可运动的六种可能性，c是部分采样，d是完全采样。

所以控制空间采样并没有预先固定点阵的位置，而是通过控制空间的采样，形成一定规则的，可行的状态点阵。

3.2 状态空间的采样（逆向采样）

逆向，也就是指定了相邻点，结合机器人的模型，逆向算出当前点到相邻点的可能路径，放张图就很好理解前向和逆向采样的区别。

逆向采样的计算较为复杂，还涉及到了求解边界值（BVP）的问题，经典的两点边界值问题是多项式方程求解系数的过程，这个如何求，具体这里不做介绍。具体见最下面参考博客链接和论文链接。

可以留意到，状态空间采样是考虑了目标点的反向规划，同时考虑了障碍物环境信息和机器人的运动学模型，所以相比于控制空间，状态空间采样更加常用。

4 借助状态栅格进行运动规划

新建的状态点阵图是有向图，因此在建立了状态图的基础上，可以使用任何图搜索算法进行路径规划，比如A*、D*等等，具体的如何设置代价函数等等，不在此讨论。

思考：state lattice算法适用于什么对象，有什么局限性或显著有点

1）运动能力有一定约束的对象，比如四轮机器人（不包括麦克纳姆轮），不能原地转弯，并且转角也有一定限制，因为这类机器人更适合通过离散化运动空间来做规划，当然比如人形机器人等等，这种运动能力强，能原地转弯，360度都可行的机器人，用类似采样的方法，个人觉得就有点失去了机器人本身运动能力的意义了；

2）因为本质上还是采样的运动空间，所以在这个状态点阵图上规划路径，不是严格最优的路径，做不要严格的时间最优或者距离最优，只能是相对的在采样图上的距离或者时间最优；

3）State lattice最友好的地方在于可以比较简单地对路径进行多维约束，比如一个小车，考虑的规划维数为平面X和Y坐标，以及车头朝向和路径曲率（x, y, theta，curvature），这些约束可以很好应用于生成候选状态点阵，需要结合模型规划就显得简单了；

4）理论上这个状态栅格地图可以离线建立，然后在线规划，可以实现实时规划

疑惑：有一个点还不是很明白，状态栅格点图建立的是局部地图呢，还是全局地图呢？Efficient constrained path planning via search in state lattices 一文中，在第4.2节生成Path primitives时，对于采样代码是结束条件这样描述的：This process terminates at a certain radial distance from the origin when all paths at that distance can be composed. 从这句话可以理解到，状态栅格算法生成的地图，是基于一定范围内的地图，按我的理解，也就是运行过程中需要根据机器人信息，多次建立状态栅格地图，与离线地图又相悖了，欢迎讨论指教！

https://github.com/amslabtech/state_lattice_planner

参考文献：

Efficient constrained path planning via search in state lattices
Differentially Constrained Mobile Robot Motion Planning in State Lattices
Spatiotemporal state lattices for fast trajectory planning in dynamic on-road driving scenarios

参考博客：

https://blog.csdn.net/space_dandy/article/details/114396542

https://zhuanlan.zhihu.com/p/14

转自：https://zhuanlan.zhihu.com/p/403822524