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/Alanaab/MotionPlanning

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