针对移动机器人的Lattice Planner -- 原理篇(剖析论文，详细学习了state lattice planner)

本文中有很多名词都是译自论文中，很多都不知道用中文怎么表示。对这些名词后面都用了原文的词汇。论文主要是 Pivtoraiko M 两篇05年的论文，08年的那篇这位虎哥已经解析并注释讲解了PR的源码了，可以移步进行学习。

深蓝学院作业代码请移步至 github
运动规划课程链接

Lattice Planner

如之前的A*栅格地图的四连接/八连接，这是一种对控制空间的离散化，而对于PRM，在空间中进行点的采样，则是对状态空间进行离散化，但是他们都有缺陷，那就是当考虑真正的有数学描述的机器人模型的时候，机器人并不能视之为随意移动的质点了，因此，本文学习并总结了Lattice Planner.

“lattice” 译为晶格。晶格规划？听起来是一个很抽象的名字，不知道这个晶格具体指什么，和原来栅格化地图中的"grid"又有什么联系。我们引用Mihail Pivtoraiko的原话：

The state lattice that we develop here can be viewed as a generalization of a grid.

概括一下，使用晶格(lattice) 的目的就是为了简化多个网格(grid)，使得原来的网格图演化成一个新的晶格图，如Fig.1[2]所示，红色点标出来的就是晶格，而连接他们之间的边则表示一个状态到另一个状态的转移，Fig.2中最浅的灰色部分就是我们建立完毕的 Lattice 地图。黑色实线和黑色加粗线代表什么，以及这个地图是如何生成的，详看后文，这里只需要记住 Lattice planning 就是在自己建立的新的 Lattice 地图上进行路径规划。

Fig.1 State Lattice

Lattice Planner主要分为在控制空间上进行晶格生成和在状态空间上进行晶格生成，称为 Control Lattice Planner(CLP), State Lattice Planner(SLP)。

1. Control Lattice Planning

由于控制空间的 Lattice Plan 比较简单，本文就简要带过，主要讲述 State Lattice Planning.

设机器人状态方程如下如下：
$$\begin{gathered} \text{State: }\mathbf{s}=\left(\begin{array}{l}x\\ y\\ z\\ \dot{x}\\ \dot{y}\\ \dot{z}\end{array}\right)\text{Input: }u=\left(\begin{array}{c}\ddot{x}\\ \ddot{y}\\ \ddot{z}\end{array}\right) \\ \dot{s}=A\cdot s+B\cdot u \\ A=\left[\begin{array}{ccccc}0& I_3& 0& \cdots & 0\\ 0& 0& I_3& \cdots & 0\\ ⋮& \ddots & \ddots & \ddots & ⋮\\ 0& \cdots & \cdots & 0& I_3\\ 0& \cdots & \cdots & 0& 0\end{array}\right]B=\left[\begin{array}{c}0\\ 0\\ ⋮\\ 0\\ I_1\end{array}\right] \end{gathered}$$
我们固定输入量$u$ 和时间 T，然后我们进行前向积分，就可以知道T后的机器人状态，如Fig.2 所示。尤其是当机器人是线性系统的时候。在知道T时刻机器人的末状态之后，就可以通过状态转移方程：
$$s(t)=e^{At}{s}_0+[{\int }_0^t{e}^{A(t-\sigma )}Bd\sigma ]u_m，$$
来还原出过程之中的各个状态，其中矩阵A是状态转移矩阵。假设$t=\tau$，此时我们想要求解$s(\tau )$，但$e^{At}$的求解是一件很麻烦的事，我们可以利用泰勒展开
$$e^{At}=I+\frac{At}{1!}+\frac{(At{)}^2}{2!}+\frac{(At{)}^3}{3!}+\cdots +\frac{(At{)}^k}{k!}+\cdots$$
而且此时我们知道，A矩阵有一个性质，就是nilpotent，也就是说它在多次方之后会变为0，那高次项就都被消去了，这使得 $e^{At}$ 在(n-1)次矩阵多项式中都具有封闭的表达式，更多的求解方式可参考现代控制理论。

Fig.2 Control Space

但这种状态建图法有一点缺陷，和Dijkstra一样，比较盲目，你不知道自己是要往左边还是往右边，只是不断的尝试各种输入量$u$的积分，所以导致在规划过程中的效率不够高。所以这时我们就提到我们这篇论文的核心部分，state lattice planner.

2. State Lattice Planning

我们可以选择在状态空间采样，我直接在空间中采样一个机器人状态（包括位置与姿态），然后反解出路径，即机器人如何选定一个$u$在T上积分，刚好可以达到我们采样的这个状态。优点显而易见，这使得算法具有启发性和贪心性，难点在于难以实现。

2.1 逆路径生成器

在 State Lattice中，我们需要根据采样点与当前点的c状态在他们之间生成符合运动学和车辆约束的路径。这个问题被称为Boundary Value Problem(BVP)，它是是状态抽样格规划的基础。由于没有通解，它往往演变成一个复杂的数值优化问题。本文基于深蓝学院和AtsushiSakai/**PythonRobotics**总结了两种路径生成方式，分别为插值法和最优边界求解OBVP.

2.1.2 样条插值法

百度百科定义：一种以 可变样条 来作出一条经过一系列点的光滑曲线的数学方法。插值样条是由一些多项式组成的，每一个多项式都是由相邻的两个数据点决定的，这样，任意的两个相邻的多项式以及它们的导数在连接点处都是连续的。

有一篇博客写的很简单明了，大家可以直接跳转 -> https://www.cnblogs.com/yanshw/p/11194058.html

本文主要谈一下PythonRobotics中对样条插值的应用，让我们看下它的源码：

def generate_trajectory(s, km, kf, k0):

    '''
    根据参数 [s, km, kf, k0]前向积分得到轨迹
    s: 弧长
    km: 运行(time / 2.0)时的曲率
    kf: 末端的曲率
    k0: 初始的曲率
    '''
    n = s / ds  # 生成多少个状态
    time = s / v  # 走这个弧长要多久
    
    if isinstance(time, type(np.array([]))): time = time[0]
    if isinstance(km, type(np.array([]))): km = km[0]
    if isinstance(kf, type(np.array([]))): kf = kf[0]
    
    # 时间t为自变量，曲率为因变量，根据已知的样本做插值
    tk = np.array([0.0, time / 2.0, time]) # 时间样本
    kk = np.array([k0, km, kf])  # 曲率样本
    t = np.arange(0.0, time, time / n) 
    fkp = scipy.interpolate.interp1d(tk, kk, kind="quadratic")
    kp = [fkp(ti) for ti in t] # 得到时间轴上每个t对应的曲率k
    dt = float(time / n)

    #  plt.plot(t, kp)
    #  plt.show()

    state = State()
    x, y, yaw = [state.x], [state.y], [state.yaw]

    for ikp in kp:
        state = update(state, v, ikp, dt, L)
        x.append(state.x)
        y.append(state.y)
        yaw.append(state.yaw)

    return x, y, yaw

2.1.3 OBVP

a) 我们可以在BVP问题中加入一个优化目标，确保我们生成成的状态曲线更加符合我们的工程实践，比如我们定一个Objective，要使得加速度的平方和最小，而且状态转移的过程要尽可能的快，如下所示：
$$J={\int }_0^{T}g(x,u)dt={\int }_0^T(1+u^{T}Ru)dt={\int }_0^T(1+a_x^2+a_y^2+a_z^2)dt$$
b) 状态量，输入量和系统方程表征如下：
$$x=\left(\begin{array}{c}{p}_x\\ p_y\\ p_z\\ v_x\\ v_x\\ v_z\end{array}\right),u=\left(\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right),\dot{x}=f(x,u)=\left(\begin{array}{c}{v}_x\\ v_y\\ v_z\\ a_x\\ a_y\\ a_z\end{array}\right)$$
c) 边界条件表征如下
$$initialstate:x(0)=\left(\begin{array}{c}{p}_{x0}\\ p_{y0}\\ p_{z0}\\ v_{x0}\\ v_{y0}\\ v_{z0}\end{array}\right),endstate:x(T)=\left(\begin{array}{c}{p}_{xf}\\ p_{yf}\\ p_{zf}\\ ?\\ ?\\ ?\end{array}\right)$$

问题解决求解

计算过程中的求导和合并同类型等操作可以通过Matlab设立符号来进行简化，如下所示：

clear;clc;
syms x pfx p0x v0x pfy p0y v0y pfz p0z v0z T  % define variable
DPx = pfx - p0x - v0x*T;
DPy = pfy - p0y - v0y*T;
DPz = pfz - p0z - v0z*T;
f1 = simplify(DPx*DPx + DPy*DPy + DPz*DPz);
result1 = simplify(diff(f1, T)); % derivation
result2 = simplify(9 * (DPx*DPx + DPy*DPy + DPz*DPz));
result3 = simplify(T^4 - (result1 * result2))

根据Pontryain’s 的极小值原理:

写出汉密顿函数如下：
$$\begin{array}{rl}H(x,u,\lambda )& =g(x,u)+{\lambda }^{T}f(x,u)\\ & =1+a_x^2+a_y^2+a_z^2+{\lambda }_1{v}_x+{\lambda }_2{v}_y+{\lambda }_3{v}_z+{\lambda }_4{a}_x+{\lambda }_5{a}_y+{\lambda }_6{a}_z\end{array}$$
其中$\lambda =({\lambda }_1,{\lambda }_2,{\lambda }_3,{\lambda }_4,{\lambda }_5,{\lambda }_6)$为态变量(costate)

因此我们可以得到 $\mathbf{\lambda }$ 的导数以及 $\mathbf{\lambda }(t)$ :
$$\dot{\lambda }=-\nabla H(x^{\ast },u^{\ast },\lambda )=(000-{\lambda }_1-{\lambda }_2-{\lambda }_3{)}^T$$

$$\lambda (t)=\left(\begin{array}{c}2{\alpha }_1\\ 2{\alpha }_2\\ 2{\alpha }_3\\ -2{\alpha }_{1}t-2{\beta }_1\\ -2{\alpha }_{2}t-2{\beta }_2\\ -2{\alpha }_{3}t-2{\beta }_3\end{array}\right)$$

所以我们可以得到我们的最优控制输入$u^{\ast }$
$$u^{\ast }=\arg \underset{u(t)}{\min }H(x^{\ast }(t),u(t),\lambda (t))=\left(\begin{array}{c}{\alpha }_{1}t+{\beta }_1\\ {\alpha }_{2}t+{\beta }_2\\ {\alpha }_{3}t+{\beta }_3\end{array}\right)$$
通过(2)式, 我们得到最优的状态量 $x^{\ast }(t)$为：
$$x^{\ast }=\left(\begin{array}{c}\frac{1}{6}{\alpha }_1{t}^3+\frac{1}{2}{\beta }_1{t}^2+v_{x0}t+p_{x0}\\ \frac{1}{6}{\alpha }_2{t}^3+\frac{1}{2}{\beta }_2{t}^2+v_{y0}t+p_{y0}\\ \frac{1}{6}{\alpha }_3{t}^3+\frac{1}{2}{\beta }_3{t}^2+v_{z0}t+p_{z0}\\ \frac{1}{2}{\alpha }_1{t}^2+{\beta }_{1}t+v_{x0}\\ \frac{1}{2}{\alpha }_2{t}^2+{\beta }_{2}t+v_{y0}\\ \frac{1}{2}{\alpha }_3{t}^2+{\beta }_{3}t+v_{z0}\end{array}\right),$$
在极小值原理中，我们有边界条件如下：
$$\lambda (T)=-\nabla h(x^{\ast }(T))$$
因为末端速度这个维度未制定,因此 $h(x^{\ast }(T))$ 显然和变量 $v$没什么关系，因此求导为0，
$$\left(\begin{array}{c}{\lambda }_4(T)\\ {\lambda }_5(T)\\ {\lambda }_6(T)\end{array}\right)=\left(\begin{array}{c}-2{\alpha }_{1}T-2{\beta }_1\\ -2{\alpha }_{2}T-2{\beta }_2\\ -2{\alpha }_{3}T-2{\beta }_3\end{array}\right)=0$$
所以我们可以得到一个新的 $\lambda (t)$:
$$\mathbf{\beta }=-\mathbf{\alpha }T$$
然后我们再利用终末条件 $x(T)$ 来求解得到 $\mathbf{\alpha }$:
$$\mathbf{\alpha }=-\frac{3({\mathbf{P}}_{\mathbf{f}}-{\mathbf{P}}_0-{\mathbf{v}}_{0}T)}{T^3}=-\frac{3\Delta \mathbf{P}}{T^3},$$
其中 ${\mathbf{P}}_{\mathbf{f}}$ 是末端的位置状态, ${\mathbf{P}}_0$ 和 ${\mathbf{v}}_0$ 是起始的位置状态. 所以我们得到最优的控制输入为 $u^{\ast }$:
$$u^{\ast }=\left(\begin{array}{c}{\alpha }_1(t-T)\\ {\alpha }_2(t-T)\\ {\alpha }_3(t-T)\end{array}\right)=\left(\begin{array}{c}\frac{-3\Delta P_x}{T^3}(t-T)\\ \frac{-3\Delta P_y}{T^3}(t-T)\\ \frac{-3\Delta P_z}{T^3}(t-T)\end{array}\right).$$

$$\begin{array}{rl}J& ={\int }_0^{T}1+(u^{\ast }(t{)}^T{u}^{\ast }(t))dt\\ & =T+\frac{{3\Vert \mathbf{\Delta }\mathbf{P}\Vert }^2}{T^3}，\end{array}$$

其中 ${\Vert \mathbf{\Delta }\mathbf{P}\Vert }^2=\Delta P_x^2+\Delta P_y^2+\Delta P_z^2$.

根据式(3)，我们知道 $J$ 和 $T$ 无关. 如果我们想要得到 $J$ 的最小值，我们需要 $J$ 的导数为0。

$$T^4+(2V_{0x}\Delta P_x+2V_{0y}\Delta P_y+2V_{0z}\Delta P_z)(9{\Vert \mathbf{\Delta }\mathbf{P}\Vert }^2)=0$$
我们使用两种方式来对生成的状态进行验证，如Fig.3所示，代码已提交至我的 GitHub.

Fig.3 OBVP求解出来的最优轨迹

2.2 建立 State Lattice

在此项目中，每一个Lattice Node有6个维度，包括 3D position 和 3D velocity.

2.3 路径等效化

${\tau }_1,{\tau }_2$是拥有相同端点的路径，当${\tau }_1$包含在${\tau }_2$的$Q$区域中时，则令${\tau }_1={\tau }_2$，判断条件如下所示：
∀ q ∈ τ 1 , ∀ q ′ ∈ τ 2 , Q = { q ′ ∣ ρ ( q ′ , q ) < δ c } , \forall q\in\tau_1,\forall q'\in\tau_2,Q=\{q'|\rho(q',q)<\delta_c\}, ∀q∈τ1​,∀q′∈τ2​,Q={q′∣ρ(q′,q)<δc​},
其中${\delta }_c$是用来判定${\tau }_1$是否包含在${\tau }_2$内的距离阈值。如 Fig.4 的阴影部分。

Fig.4 路径等效化

路径等效化是 Lattice Planner 中非常重要的核心做法，因为他在不损失路径表示的情况下大大简化了搜索空间[1]。

2.4 控制集

如同在原来的4-neighbor栅格地图中一样，我们可以分出最小的一个路径集， 即上下左右四，如Fig.5(a)所示。而且可以看到途中的灰色路径代表了所有可行路径，通过这个点的任何路径都可以用这个用最小路径集中的分量表示，如在Fig.5(a)中的分量就有向上，向左，向右，向下，这被命名为**“原义(primitives)”**，这在运动规划中是一个很常见的名词。让我们再看一个例子如Fig.5(b) [4] ,这种优势显而易见，我们如果能在 state lattice 中也定义这么一个原义，那我们就可以将无限多的路径表示为很多个这种有限小的原义集合。

Fig.5 4链接图中分离出来的最小路径集合

2.4.1 生成路径原义

要注意到，在lattice空间下，我们所生成的路径都是符合机器人运动学的，毕竟路径是代表状态的转移过程。所以为了满足我们上面的目标，我们再引入一个概念，那就是路径分解。当我们生成路径的时候，如果该路径经过了另外一个节点 $B$，且原点 $A$ 到此点的路径 $subpath1$ 和此点到伪目标点 $C$ 的路径 $subpath2$ 相加等于原点到伪目标点的路径 $Mainpath$，那么说明该路径可以被分解。(至于这个伪目标点是什么，继续往下看)，如 Fig.6 所示。

Fig.6 路径分解

现在让我们来看下如何生成一个最小路径原义，具体算法如下所示：

结合算法流程我们很容易明白路径分解在生成最小运动原义过程中所起到的意义，也就是你$A$的 ManhattanDistance $R_b$ 下生成的路径可由在点 $A$ 的 ManhattanDistance $R_{s1}$，和在点 $B$ 的ManhattanDistance $R_{s2}$ $(R_{s1},R_{s2}<R_b)$生成的路径组合代替，那我生成这个路径的意义就已经丧失了，因为我可以由当前已生成的最小原义集合在$A，B$两点叠加生成。在不断的增加R，不断地加入新的 $OrigPath$到 $Primset$ 中，直到找到的路径都可以被分解，也就是可以由当前已生成的最小原义集合表示的时候，说明我们已经生成了可以表示所有路径的最小原义集合，也就是我们的控制集(control set)，然后将这个最小原义集应用在各个点上就可以了。如Fig.1所示。

2.5.2 启发式函数

给定一个起始状态 $Q_i=(x_i,y_i,{\theta }_i,k_i)$，和终止状态 $Q_i=(x_f,y_f,{\theta }_f,k_f)$，对于阿克曼等nonholonomic vechine 的启发函数估计中则需要包含平动的代价和旋转的代价。因此可以考虑欧式距离和带权重的航向误差，启发函数如下所示：
$$h=L_2((x_i,y_i),(x_f,y_f))+w_0(|{\theta }_f-{\theta }_i|)$$
其中$w_0$为heading error的加权值, $|{\theta }_f-{\theta }_i|$是航向误差。

Fig.7 heading error

当然，启发函数的制定是根据任务所定，多种多样，这只是[1]中提到的一种方式。但这种估计方式是有缺点的，如同A*算法中提到的一样，我们的启发式估计成本必须小于实际成本，也就是说 $h(t)<h\ast$，但是上述方式中不恰当的加权$w_0$可能会使得估计成本超出实际成本。那该采取怎么样的启发函数呢？[1]中给出了一种为Look_Up_Table(LUT)的启发函数设计方案。

这个Look_Up_Table其实就是预计算，举例： 将Lattice origin点到其附近的点（该点与Lattice origin的距离小于3）的成本预先计算好纳入这个查找表中，为什么是距离小于3呢？因为只有在接近机器人的时候，曲率和方向才会对路径产生较大的影响，对于超出这个距离的部分大豆都是直接冲的，可以直接使用简单的欧几里得距离作为路径启发代价的评判。

3.一点小总结

• Lattice plan只适合于那些有一定限制的机器人，比如阿克曼小车之类的，但是对于无人机或具有全向底盘的AGV等，则不合适，因为制定heading的方向对于全自由度的机器人显然没什么意义，反而大大牺牲了机器人的灵活性。
• Look_up_table这种预计算思想显然非常有效，不止体现在上述预先计算成本当中，很多的优化迭代方法在实现的时候，都会建立这么一个查找表，以方便再下一次进行优化迭代的时候能够从之前计算过的最优点中找到一个最相近的点作为近似解，从而进行迭代寻优，这显然大大提升了算法的性能，比如再2.1.2中的PythonRobotic中就也用到了这种LUT的方法。
  

参考资料

[1] Pivtoraiko M, Kelly A. Efficient constrained path planning via search in state lattices[C]//International Symposium on Artificial Intelligence, Robotics, and Automation in Space. Germany: Munich, 2005: 1-7.
[2] Pivtoraiko M, Knepper R A, Kelly A. Differentially constrained mobile robot motion planning in state lattices[J]. Journal of Field Robotics, 2009, 26(3): 308-333.

[3]Howard T M, Green C J, Kelly A, et al. State space sampling of feasible motions for high‐performance mobile robot navigation in complex environments[J]. Journal of Field Robotics, 2008, 25(6‐7): 325-345.

[4] Learning Control Sets for Lattice Planner – university of WATERLOO

[4]深蓝学院移动机器人Motion Planning