Optimal Boundary Value Problem (OBVP) 学习笔记

引言

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 dt$$
其中，第一项反映了末状态与理想状态的差别，可理解为惩罚项，第二项反映了状态转移过程的代价 (transition cost)。

为求解最优的$u(t)$（即最优$j$），可以使用庞特里亚金极小值原理：引入costate $\lambda =({\lambda }_1,{\lambda }_2,{\lambda }_3)$，（实际上是$\lambda (t)=({\lambda }_1(t),{\lambda }_2(t),{\lambda }_3(t))$，为形式简洁，将t省略）,构建Hamiltonian funciton
$$\begin{array}{rl}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{array}$$

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

庞特里亚金极小值原理 (Pontryagin’s minimum principle)是最优控制范畴里的概念。wiki百科 - Pontryagin’s minimum principle
对于初始状态给定的问题，
$$\begin{gathered} \dot{\lambda }(t)=-{\nabla }_{s}H\left(s^{\ast }(t),u^{\ast }(t),\lambda (t)\right) \\ \lambda 的导数=-(H对p求导,H对v求导,H对a求导) \end{gathered}$$
最优控制输入为
$$u^{\ast }(t)=\arg \underset{u(t)}{\min }H\left(s^{\ast }(t),u(t),\lambda (t)\right)$$
如果$h(s(T))$是连续的，还有terminal boundary condition成立:
$$\lambda (T)=-\nabla h\left(s^{\ast }(T)\right)$$

下面开始求解，由庞特里亚金极小值原理，
$$\dot{\lambda }=-{\nabla }_{s}H\left(s^{\ast },j^{\ast },\lambda \right)=\left(0,-{\lambda }_1,-{\lambda }_2\right)$$

注意上式的意义：$\lambda (t)$这一向量第一项的导数为0，第二项的导数为第一项的相反数，第三项的导数为第二项的相反数。

引入待定系数$\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{array}{rl}& \\ j^{\ast }(t)& =\arg \underset{j(t)}{\min }H\left(s^{\ast }(t),j(t),\lambda (t)\right)\\ & \\ & \end{array}$$