环境感知与规划专题（十一）——状态栅格规划器（State Lattice Planner）

浙江大学空天信息技术硕士

前言

前几篇中介绍了几种路径搜索算法，其中既有基于图搜索的路径搜索算法（如A*），也有基于采样的路径搜索算法（如RRT、RRT*）。通过路径搜索算法，我们能够得到一条可通行的路径，然而，对于这条路径中的路标点（Waypoint）之间的路径往往是不符合机器人（无人机\无人车）动力学模型的。

因此，对于每一个路标点（Waypoint）之间的连接路径，我们需要进行进一步的优化，从而得到一条符合机器人（无人机\无人车）动力学约束的可通行的运动学连接（feasible motion connections）。

如上图中实线为前端路径搜索得到的可通行路径，而虚线为后端轨迹优化得到的符合动力学模型的可通行路径。经典的运动规划算法框架，往往由前端路径搜索与后端轨迹优化组成，在得到满足运动学的可通行的运动连接后，才能够将这条优化后的轨迹指令下达至控制器执行。

这也是本文重点讨论的基于采样的路径规划算法——状态栅格规划器（State Lattice Planner）。

状态栅格搜索（State Lattice Search）

在构建状态栅格之前，我们需要对被控对象进行运动学分析，即建立对应的运动学模型，这里以简单的无人车模型为例，如下图所示，我们可以将其简化为自行车模型：

运动学模型主要描述了位置、速度、加速度、转向角度等等状态量在时间上的状态转移，以及对于我们所关心的这些状态量的边界条件进行约束。

在得到被控对象的运动学方程后，我们可以开始正式构建状态栅格，对于State Lattice Search，我们通常有两种采样方式：

基于控制空间（Control Space)的采样

在RRT*算法中，我们对空间中的位置点进行随机采样，那么，在控制空间中，我们应该如何进行采样呢？

很简单，在已知被控对象的情况下，我们固定输入量u（控制量）与积分时间T，由此进行前向积分即可得到被控对象从任意初始状态s_{0}过度到的终止状态s_{f}，前向积分的方式能够简单粗暴地对被控对象在任意时间T之后的状态进行预测，从而得到一系列状态集合，这就是在控制空间中的采样。

由于输入量u与积分时间T都是任意给定的，因此，基于控制空间的采样往往是没有明确的目的性的，它的采样结果只能分布在某些状态附近，但不能精确地采样到某一个给定状态。

基于状态空间（State Space)的采样

而另一种采样方式则是对状态空间的采样。这种采样方式则是在给定被控对象的初始以及目标状态后，反向计算中间过程，相对于前者，该采样方式在工程上较难实施，我们需要对其进行两点边界值问题的求解，才能求得两个状态之间轨迹。

两点边界值问题（BVP）

经典的两点边界值问题，其实就是多项式方程求系数的过程，以多旋翼无人机为例，我们将无人机的状态方程表示为五阶多项式形式，那么，对其状态方程求各阶导数，即可求得位置、速度、加速度等状态量关于时间的表达式，那么，我们只需要将两点边界状态信息代入多项式方程，即可联立求得多项式系数的值，如此一来，一条可通行的轨迹就被解析化了。