引言

Boundary value problem (BVP)：给出机器人在起始点与终止点的状态，设计出一条状态转移的轨迹。是stated sampled lattice planning的基础，在motion planning技术栈中的位置如下。
在这里插入图片描述
Optimal boundary value problem：按某种原则设计出一条最优轨迹。

建模

对于二维/三维空间中的机器人，通常在每个维度上分别进行轨迹设计。此处以三维空间中的无人机为例，考察其在一个轴向的运动。

无人机状态 $s = (p, v, a)$

使用jerk作为控制输入： $u = j$

状态方程：
$\dot{s}=f_s(s,u)=(v,a,j)$

目标：最小化jerk二次方的积分，即
$\min J : = \frac{1}{T}\int_{0}^{T}j(t)^{2}dt$

待求解的量是 $u (t)$

求解

寻找最优轨迹的一般形式是极小化代价函数
$J=h(s(T))+\int_{0}^{T} g(s(t), u(t)) \cdot d t$
其中，第一项反映了末状态与理想状态的差别，可理解为惩罚项，第二项反映了状态转移过程的代价 (transition cost)。

为求解最优的 $u (t)$ （即最优 $j$ ），可以使用庞特里亚金极小值原理：引入costate $\lambda=(\lambda_1,\lambda_2,\lambda_3)$ ，构建Hamiltonian funciton
$\begin{aligned} H(s, j, \lambda) &=\frac{1}{T} j^{2}+\lambda^{T} f_{s}(s, j) \\ &=\frac{1}{T} j^{2}+\lambda_{1} v+\lambda_{2} a+\lambda_{3} j \end{aligned}$

在继续向下之前，首先简要介绍庞特里亚金极小值原理 (Pontryagin’s minimum principle)：

下面开始求解，由庞特里亚金极小值原理，
$\dot{\lambda}=-\nabla_{s} H\left(s^{*}, j^{*}, \lambda\right)=\left(0,-\lambda_{1},-\lambda_{2}\right)$

引入待定系数 $\alpha,\beta,\gamma$ ，易写出
$\lambda(t)=\frac{1}{T}\left[\begin{array}{c} -2 \alpha \\ 2 \alpha t+2 \beta \\ -\alpha t^{2}-2 \beta t-2 \gamma \end{array}\right]$

进而，可得最优jerk
$\begin{aligned} j^{*}(t) &=\arg \min _{j(t)} H\left(s^{*}(t), j(t), \lambda(t)\right) \\ &= arg \min _{j(t)}\left [ \frac{1}{T} j^{2}+\frac{1}{T}(-\alpha t^{2}-2 \beta t-2 \gamma)j \right ] \\ &=\frac{1}{2} \alpha t^{2}+\beta t+\gamma \end{aligned}$

通过对jerk的三次积分，可得最优的 $s (t)$
$s^{*}(t)=\left[\begin{array}{c} \frac{\alpha}{120} t^{5}+\frac{\beta}{24} t^{4}+\frac{\gamma}{6} t^{3}+\frac{a_{0}}{2} t^{2}+v_{0} t+p_{0} \\ \frac{\alpha}{24} t^{4}+\frac{\beta}{6} t^{3}+\frac{\gamma}{2} t^{2}+a_{0} t+v_{0} \\ \frac{\alpha}{6} t^{3}+\frac{\beta}{2} t^{2}+\gamma t+a_{0} \end{array}\right]$

按对末状态的要求，分三种情况讨论。

情况一：Fully Defined End Translational State

要求末状态 $s (T)$ 的每个分量严格等于给定值

设期望的末状态 $s(T)=(p_f,v_f,a_f)$ ，以 $s (T)$ 第一维为例，有
$s^{*}(T)-s(0)=\frac{\alpha}{120}T^5+\frac{\beta}{24}T^4+\frac{r}{6}T^3+\frac{a_0}{2}T^2+v_{0}T+p_0-p_0=p_f-p_0$

记 $\Delta p=p_{f}-p_{0}-v_{0} T-\frac{1}{2} a_{0} T^{2}$ ，移项整理得
$\frac{\alpha}{120}T^5+\frac{\beta}{24}T^4+\frac{r}{6}T^3=\Delta p$

类似地，写出 $s (T)$ 所有维度上的改变量，并写作矩阵形式，有
$\left[\begin{array}{ccc} \frac{1}{120} T^{5} & \frac{1}{24} T^{4} & \frac{1}{6} T^{3} \\ \frac{1}{24} T^{4} & \frac{1}{6} T^{3} & \frac{1}{2} T^{2} \\ \frac{1}{6} T^{3} & \frac{1}{2} T^{2} & T \end{array}\right]\left[\begin{array}{c} \alpha \\ \beta \\ \gamma \end{array}\right]=\left[\begin{array}{c} \Delta p \\ \Delta v \\ \Delta a \end{array}\right]$

其中，
$\left[\begin{array}{c} \Delta p \\ \Delta v \\ \Delta a \end{array}\right]=\left[\begin{array}{c} p_{f}-p_{0}-v_{0} T-\frac{1}{2} a_{0} T^{2} \\ v_{f}-v_{0}-a_{0} T \\ a_{f}-a_{0} \end{array}\right]$

从而解出
$\left[\begin{array}{l} \alpha \\ \beta \\ \gamma \end{array}\right]=\frac{1}{T^{5}}\left[\begin{array}{lll} 720 & -360 T & 60 T^{2} \\ -360 T & 168 T^{2} & -24 T^{3} \\ 60 T^{2} & -24 T^{3} & 3 T^{4} \end{array}\right]\left[\begin{array}{c} \Delta p \\ \Delta v \\ \Delta a \end{array}\right]$

情况二：Partially Defined End Translational State

要求末状态 $s (T)$ 的部分分量等于给定值

对于 $s (T)$ 第 $i$ 维的的分量 $s_{i}(T)$ ，若给定 $s_{i}(T)$ 的值，则该维度上受积极约束，并称下标 $i$ 属于积极集： $i\in \mathcal{A}$

此时，由庞特里亚金极小值原理，对于所有自由分量 $s_{j}(T)$ ，其对应的 $\lambda_{j}(T)$
$\lambda_{j}(T)=\frac{\partial h\left(s^{*}(T)\right)}{\partial s_{j}}, \text { for } j \notin \mathcal{A}$

由上文 $s^{*}(t)$ 的表达式可知， $s^{*}(T)$ 是一个只与 $T$ 有关的函数，因此
$\lambda_{j}(T)=\frac{\partial h\left(s^{*}(T)\right)}{\partial s_{j}}=0, \text { for } j \notin \mathcal{A}$

即对于末状态的所有自由分量，其相应的 $\lambda_{j}(T)=0$

下面举一个例子。

Example：固定终点的 $p$ 与 $v$ ，对 $a$ 不做要求，求最优jerk

由 $p$ 与 $v$ 的末状态，有
$\left[\begin{array}{cc} \frac{1}{120} T^{5} & \frac{1}{24} T^{4} & \frac{1}{6} T^{3} \\ \frac{1}{24} T^{4} & \frac{1}{6} T^{3} & \frac{1}{2} T^{2} \\ \end{array}\right]\left[\begin{array}{c} \alpha \\ \beta \\ \end{array}\right]=\left[\begin{array}{c} \Delta p \\ \Delta v \\ \end{array}\right]$

由于 $a$ 是自由分量，因此 $\lambda(T)$ 的第三个分量为0，即
$-\alpha T-2 \beta -\frac{2}{T} \gamma=0$

以上两式联立，有
$\left[\begin{array}{ccc} \frac{1}{120} T^{5} & \frac{1}{24} T^{4} & \frac{1}{6} T^{3} \\ \frac{1}{24} T^{4} & \frac{1}{6} T^{3} & \frac{1}{2} T^{2} \\ T & 2 & \frac{2}{T} \end{array}\right]\left[\begin{array}{c} \alpha \\ \beta \\ \gamma \end{array}\right]=\left[\begin{array}{c} \Delta p \\ \Delta v \\ 0 \end{array}\right]$

解得
$\left[\begin{array}{c} \alpha \\ \beta \\ \gamma \end{array}\right]=\frac{1}{T^{5}}\left[\begin{array}{lc} 320 & -120 T \\ -200 T & 72 T^{2} \\ 40 T^{2} & -12 T^{3} \end{array}\right]\left[\begin{array}{c} \Delta p \\ \Delta v \end{array}\right]$

类似地，还可以解出固定 $p$ 与 $a$ 、固定 $v$ 与 $a$ 等共计6种末状态部分固定的问题。详见参考[1]。

情况三：Motion Primitive Cost

未指定末状态的任意分量

简言之，一个motion primitive就是一套【机器人初始状态、运动时间、 $p, v, a$ 的某种组合构成的末状态】的组合，详见参考[1]。

这一类情况不是要求解出具体的 $\alpha,\beta,\gamma$ ，而是计算出评估一个motion primitive的代价函数的具体表达式：
$\begin{aligned} J &= \frac{1}{T}\int_{0}^{T}j(t)^{2}dt \\ &= \frac{1}{T}\int_{0}^{T}\left (\frac{1}{2} \alpha t^{2}+\beta t+\gamma\right ) ^{2}dt \\ &=\frac{1}{20}\alpha^2T^4+\frac{1}{4}\alpha \beta T^3+\frac{1}{3}\left (\alpha \gamma + \beta^2 \right )T^2+\beta \gamma T+\gamma^2 \end{aligned}$

回归到sample in state space的state lattice planning，这一代价函数可以用于评估不同的候选motion primitive。

总结

前两种情况是对具体任务的求解，第三种情况是对motion primitive的评价——在运动规划过程中，按某种规则生成一系列候选motion primitive，使用情况三中代价函数进行评价，筛选出某个（些）合适的motion primitive。

参考

[1] Mueller M W, Hehn M, D’Andrea R. A computationally efficient motion primitive for quadrocopter trajectory generation[J]. IEEE transactions on robotics, 2015, 31(6): 1294-1310.
[2] https://www.shenlanxueyuan.com/course/450