State Lattcie Planner

状态栅格规划器

• 前几篇中介绍了几种路径搜索算法，其中既有基于图搜索的路径搜索算法（如A*），也有基于采样的路径搜索算法（如RRT、RRT*）。通过路径搜索算法，我们能够得到一条可通行的路径，然而，对于这条路径中的路标点（Waypoint）之间的路径往往是不符合机器人（无人机\无人车）动力学模型的。
• 前几篇中介绍了几种路径搜索算法，其中既有基于图搜索的路径搜索算法（如A*），也有基于采样的路径搜索算法（如RRT、RRT*）。通过路径搜索算法，我们能够得到一条可通行的路径，然而，对于这条路径中的路标点（Waypoint）之间的路径往往是不符合机器人（无人机\无人车）动力学模型的。
  因此，对于每一个路标点（Waypoint）之间的连接路径，我们需要进行进一步的优化，从而得到一条符合机器人（无人机\无人车）动力学约束的可通行的运动学连接（feasible motion connections）。
• 普通栅格(grid)与状态栅格的异同点
  相同之处：它们都是将连续空间离散化，简化路径规划。不同之处：状态点阵中，每个顶点的连线都是根据机器人的运动学模型生成的，都是切实可行的路径，而普通栅格(grid)的方法并没有结合模型，所以两点之间的路径，机器人并不一定能通过，举个例子：
  这也是本文重点讨论的基于采样的路径规划算法——状态栅格规划器（State Lattice Planner）。

动力学和运动学概念

为什么要在前端考虑动力学约束？

1.学院派的思路：将任务分为前端和后端，前者寻找一个地位空间里寻找一个可行的粗糙的路径，以及后端在机器人状态空间里的一个轨迹优化。问:既然后端已经有了轨迹优化了，为什么在前端还需要考虑动力学约束呢？答：如果前端完全不考虑动力学约束的话，整个轨迹优化的压力全在后端，如果前端考虑了动力学约束的话，会有一个比较好的初始路径给后端优化，减轻了后端的压力。
2.且轨迹优化往往只能在局部进行。规划优化往往是在path的基础上做的局部的变形。1.从全局来看，轨迹优化往往是在原来直线上做一些修改，在前端生成的path上做的局部变形。2.从数学方法上来看，原有路径会被用很多条多项式轨迹代替，整个路径就被分成了很多轨迹的合成，所以也说轨迹优化是局部路径规划。

如果在前端路径寻找的过程中完全不考虑动力学约束，那么就会出现这种情况，一个无人机原始就有一个向右的速度，而根据不考虑动力学的A*会规划出一条最短路径这条路径可能是从障碍物的左边走的，这就要求无人机必须做一个急刹车和急转弯向左边飞去，这就大大增加了无人机运动难度和碰到障碍物的概率。而后端的轨迹优化不可能将轨迹从左侧优化到右侧。这是其中一个例子，还有一个例子，比如说自动泊车的例子，如果完全不考虑动力学约束的话，规划了一条初始路径可能直接会让车子倒着行走，毕竟车子要先开出停车位置，再开进去另一个停车位置。而且找到的轨迹可能无法满足车子的动力学约束。

一些运动学模型

状态栅格搜索算法

第二课是在构建一个搜索树，第三课则是在构建search graph。RRT则是在构建search tree中不断更新搜索路径。路径规划算法核心是怎么根据不同的场景去灵活的构建search tree。目的是为了 两个点连接上都是要求是一条可行的运动轨迹。
这就有两种方法，一种是离散机器人的控制空间，一种是离散机器人的状态空间。

1.控制空间。

实际上，这些在栅格地图上进行的四连接或者八连接，就是机器人的控制空间。四连接或者八连接都是在对机器人具体的速度，方向做出要求。
1.控制空间的离散的理解。也就是说，当你给定一个激励时（无论这个激励是加速度还是速度啥的），再给定一个 t 经过时间的积分会生成一些从初始地点到其他点的可行的轨迹。相当于车子给定油门，方向盘打过一定方向，车子就会在一段时间后到达某处。
2.控制空间离散的特点。1.容易实现的，实际上就是速度等对时间的积分。2.没有导向性的，也就是没有启发性的，这导致了到目标之前可能会走很多的弯路，这也导致了它算法的低效性。

转移状态空间模型。状态里面可以包括位置，速度，加速度等。输入可以是加速度，jerk,snap等。通过多阶积分得到末状态。需要注意的是 矩阵A 是特别重要的，可以帮助我们大大简化计算。
当得到末状态之后，我们需要还原出到末状态的轨迹。这要用到一条状态转移方程。

F（t）是零状态响应，G（t）是零输入响应。

矩阵A能够大大简化计算

Sample1

这是将输入分为9，25份然后一步步积分得到控制空间离散。实际上并不需要得到整个空间的离散，在一些启发函数的帮助下，我们可以只搜索局部的空间离散。这可以大大提高搜索的效率

2.状态空间

状态空间的离散理解。它就是指定了初末两点的状态，包括方向等，要求反算出来这两点之间的状态，包括它的速度和时间等。
状态空间的离散的特点。1.具有导向性的，我们可以直接指定终点，自然而然的规划出道终点的轨迹。这大大提高算法的效率。2.是难以实现的，两点之间轨迹要计算出u , T ，这是困难的。

Sample1

按照一定规则找到它的各个邻居，之后反算出两个点之间的轨迹，这样也会生成一些很规律的轨迹。
Sample2

两层lattice graph, 按照圆等分的规律来采样起点周围的点，可以看到一共经历了两轮，采样了两个圆上的点，只有第一层的轨迹是不一样的，因为只有第一层收到机器人初始状态的影响，收到边界条件的影响，第一层到第二次的状态都是类似的，所以规划出来的轨迹也是类似的。

状态空间和控制空间的比较

1.可以看到控制空间的积分轨迹有很多是不合格的，是没有目的的，超出道路范围的，而在道路上直线直接采样的状态点规划出来的轨迹则是都合格的。
2.控制空间的轨迹会很大收到初始状态的影响，自由度会很小，当初始状态的方向有障碍物的时候，初始状态的速度、方向经过积分之后规划的路径会有很大一部分撞到障碍物。规划的轨迹很多是同质化的。
3.状态空间是难以实现的，得用BVP

BVP

Boundary Value Problem

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

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

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

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

4）理论上这个状态栅格地图可以离线建立，然后在线规划，可以实现实时规划
可供参考文章: https://www.cnblogs.com/zhjblogs/p/15193318.html.
其他人笔记: https://blog.csdn.net/space_dandy/article/details/114396542.