【路径规划】状态格算法 State Lattices Algorithm

EFFICIENT CONSTRAINED PATH PLANNING VIA SEARCH IN STATE LATTICES

Mihail Pivtoraiko and Alonzo Kelly

---

---

1. 逆路径生成问题

路径生成问题一般是指给定控制输入，得到在该控制输入作用下产生的路径，一般通过航迹推演等方法可以完成。而逆路径生成问题（Inverse Path Generation） 则刚好反过来，给定起点和目标点（甚至可以是一系列路径上的点），希望找到一个可行路径并推导得到相应的控制输入。

在【参数化最优控制】中提到的方法给状态格算法提供了解决这个问题的基础。

2. 状态格

状态格算法本质上是一种图的构造，需要构造出一张反映当前环境可达性的图，即状态格。状态格的节点即为离散化后的各个状态，而两个节点之间的有向边则表示其间的可达路径。如何判断两个节点之间是否能生成边呢，就需要借助上述逆路径生成问题的解决方法。

状态格的规律性

状态格具有高度的规律性，即在不考虑障碍物的情况下，如果从一个节点可以到达另一个节点，那么这两个节点经过同样过程的平移后，依旧具有相同的可达性。

上式中n为整数，${\Delta }_l$为离散化后坐标轴的单位长度

那么，只要找到以一个节点为起点的边，就可以通过平移的方式铺满到所有节点，完成整个状态格。

3. 准备工作：路径等效 Path Equivalence

给定两条端点相同的路径，简单地说，若其距离足够接近，则可以视作等效路径。

路径等效是这个状态格算法的关键，经过路径等效，环境中的无数条路径就可以变为有限条不等效的路径，便于用来搜索。

这个思想同样符合机器人的实际控制过程，任何控制都不可能确保机器人完全沿着路径行驶，故没有必要区分太相近的路径，等效路径的思想可以提高规划效率。

4. 产生路径库 Path Primitives

基于状态格的规律性，只需要找到一个节点出发的边，再复制铺满整个空间即可。将这样的由一个节点出发得到边称为Primitives，“路径库”，就像积木一样，通过路径库内基础路径的组合，就可以近乎完整地表达整个环境内的可行路径。

路径库首先应该完整，其次应该避免重复，假如路径库里有abc三条路径，那么应该避免ab的组合与c等效的这种情况。

产生路径库的算法如下：

• 从起点开始，每次考察起点附近R范围内的节点，R从1开始，每一次迭代时R自增1
• 在R增大的过程中，直到某一次迭代无法再找到新的primitives，算法结束。
• 对于每个范围R，尝试找到起点Qi与该范围内每个节点Qf的可行路径OrigPath，一旦找到可行路径，即检查这条可行路径沿途足够近的范围内的其他节点qs，从起点开始，按顺序检查每个qs，一旦发现满足【这条可行路径可被分解】，则放弃这条可行路径，检查下一条可行路径。当某条可行路径不能被分解，将其加入PrimSet，即路径库，并标记找到primitives
• 【路径分解】：Qi→qs、qs→Qf均可行，且Qi→qs+qs→Qf与Qi→Qf等效

细节补充

作者提到，为了让节点到一些节点的路径尽可能是直线，可以对朝向进行不均匀的离散。
比如，以45°为间隔离散，可以让起点以直线路径到达8个最近邻，再加上arctan(1/2)=26.6°间隔的离散，可以从起点以直线路径到达其另外八个邻点（图中灰色箭头）

当然，在现有算法基础上还可以再增大R以获得更多的primitives，获得更好的朝向分辨率，然而计算成本会相应增加，得不偿失。

下图为以上述朝向离散方法获得的一个路径库

轨迹方程为

可以满足指定起点、终点的状态（位姿和曲率）

那么将起点的路径库复制到每一个点，即可以获得一个完整的状态格。状态格能很好地反应各节点之间的可达性，并且距离和朝向的离散化实际上可以反映机器人的最小旋转半径/最大旋转曲率。

5. 利用状态格寻径

在对状态离散时，将离散间隔选为costmap的分辨率，则可以将costmap覆盖在状态格上，这时则有了障碍物信息，通过A*、Dijkstra等图搜索方法，即可利用状态格进行路径规划。

在利用A*进行搜索时，启发式成本h可以如下设置

即体现了距离和朝向差，其中$w_{\Theta }$为朝向所占的权重

上述启发式成本加入了朝向，其缺陷是可能超过了真实的隐藏成本，会导致最终无法到达目标。只有在距离很近时，朝向才比较重要，而当距离远了的时候，朝向其实没必要作为隐藏成本的一部分，所以可以对机器人的最小转弯半径进行限制，这意味着短距离内机器人的朝向不会发生太大的变化，这样就可以把朝向从隐藏成本中去掉，提高了规划的成功率。