动力学约束下的运动规划算法——两点边界值最优控制问题 OBVP

   OBVP 即 optimal bundary value problem，即最优的BVP， BVP 问题其实就是解决 state sampled lattice planning 的基本操作方法。

   如果，我们期望无人机从一个状态移动到另一个状态，即给定初始状态和终点状态，求解两个状态之间的局部轨迹，假设我们选取的目标函数（代价函数）是无人机三轴耗费的能量之和最少，如下式所示：

   $$\begin{array}{r}{J}_{\Sigma }=\sum _{k=1}^3{J}_k,J_k=\frac{1}{T}{\int }_0^T{j}_k(t{)}^{2}dt.\end{array}$$

   状态量选取为三轴位置、三轴速度、三轴加速度、输入的控制变量选取为三轴jerk，如下式所示：

   $$\text{state:}{s}_k=(\begin{array}{c}{p}_k,{\nu }_k,a_k\end{array})\text{Input:}{u}_k=j_k$$

   系统的模型或者说状态方程如下所示：

   $${\dot{s}}_k=f_s(s_k,u_k)=(v_k,a_k,j_k)$$

   我们可以利用庞特里亚金最小值原理来求解该最优控制问题，在使用庞特里亚金最小值原理的时候，我们需要定义一个协态 ，有多少状态量就有多少协态$\lambda =({\lambda }_1,{\lambda }_2,{\lambda }_3......)$

   下面先来介绍一下庞特里亚金最小值原理，其包含三个公式，第一个公式：协态量的导数是具有最优状态量和控制量的汉密尔顿函数的关于各个状态量的导数。

   $$\dot{\lambda }(t)=-{\nabla }_{s}H\left(s^{\ast }(t),u^{\ast }(t),\lambda (t)\right)$$

   其中，$s^{\ast }、u^{\ast }$分别是最优的状态量和控制量

   第二个公式是边界条件公式，如果边界条件不确定，则需要使用该公式。

   $$\lambda (T)=-\nabla h(s^{\ast }(T))$$

   h被定义成终末状态的函数或者说终末状态的惩罚项（离我们期望的最终状态差距越大，该项值越大）。这个公式代表终末状态时间T的时候，协态与终末状态中最优的自由量的梯度的和等于0.。

   在上述的例子中，我们要求上面的问题中的终末状态的p,v,a均达到指定状态，那么这个公式就不需要使用了，因为终末状态没有任何自由度，如果说我们只希望终末状态的位置p达指定状态，v,a无所谓，那么我们可以根据这个公式写出两个边界条件来帮助我们确定待定系数。

   第三个公式：最优的控制是具有最优状态的汉密尔顿函数的最小值

   $$u^{\ast }(t)=\arg \underset{u(t)}{\min }H\left(s^{\ast }(t),u(t),\lambda (t)\right)$$

---

   那么接下来，我们正式来看一下如何使用上面介绍的庞特里亚金最小值原理来求解本文开始处介绍的问题，我们需要定义一个Hamiltonian汉密尔顿函数，它由状态量s（就是本问题中的p,v,a）, 控制量u（也就是我们这里的jerk）,以及协态$\lambda$ 组成

   $$\begin{array}{rl}H(s,u,\lambda )& =\frac{1}{T}{j}^2+{\lambda }^T{f}_s(s,u)\\ & =\frac{1}{T}{j}^2+{\lambda }_1\nu +{\lambda }_{2}a+{\lambda }_{3}j\end{array}$$

   其中第一项中的$\frac{1}{T}{j}^2$中的$j^2$是目标函数$J_k=\frac{1}{T}{\int }_0^T{j}_k(t{)}^{2}dt.$的被积分项,若目标函数的被积分项改为其他项，则此处也要进行对应修改。

   由庞特里亚金最小值原理的第一个公式，即将Hamiltonian汉密尔顿函数分别对状态变量p、v、a求偏导可得下式：

   $$\dot{\lambda }=-{\nabla }_{s}H(s^{\ast },u^{\ast },\lambda )=(0,-{\lambda }_1,-{\lambda }_2)$$

   我们得到了一个$\lambda$的微分方程，然后我们对其积分获得$\lambda$的表达式，${\lambda }_1$是关于p的导数，${\lambda }_2$是关于v的导数，${\lambda }_3$是关于a的导数，所以${\lambda }_1$的导数是${\lambda }_2$，${\lambda }_2$的导数是${\lambda }_3$。所以很容易写出下面的表达式

   $$\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]$$

   我们没有使用边界条件公式，那么如何来求解上面的系数α、β、γ呢？因为我们会指定初始状态和最终想要达到的目标状态，所以，我们可以利用$s(0)=(p_0,v_0,a_0)$、$s(T)=(p_T,v_T,a_T)$这两个条件来求解系数α、β、γ。

   我们将这三个系数放在这里，先利用庞特里亚金最小值原理的第三个公式求最优的控制$u^{\ast }$,前面我们已经得出$H(s,u,\lambda )=\frac{1}{T}{j}^2+{\lambda }_1\nu +{\lambda }_{2}a+{\lambda }_{3}j$，然而第三个公式中使用的是最优的$s^{\ast }$,而不是s，$s^{\ast }(t)=(p^{\ast },v^{\ast },a^{\ast })$ 所以：

   $$\begin{array}{rl}H(s^{\ast },u,\lambda )& =\frac{1}{T}{j}^2+{\lambda }^T{f}_s(s,u)\\ & =\frac{1}{T}{j}^2+{\lambda }_1{v}^{\ast }+{\lambda }_2{a}^{\ast }+{\lambda }_{3}j,\end{array}$$

   我们先把待定系数α、β、γ视为已知量，利用$\lambda$关于α、β、γ的表达式，进一步可以得到$\lambda$的值，则上面的$H(s^{\ast },u,\lambda )$表达式中，仅含有唯一变量$j$，则求下式的最小值

   $$\begin{array}{rl}{u}^{\ast }(t)=j^{\ast }(t)& =\arg \underset{u(t)}{\min }H\left(s^{\ast }(t),u(t),\lambda (t)\right)\\ & =\arg \underset{u(t)}{\min }\frac{1}{T}{j}^2+{\lambda }_1{v}^{\ast }+{\lambda }_2{a}^{\ast }+{\lambda }_{3}j\end{array}$$

   可以等价转换成，求一个$j$使得下式最小

   $$\begin{array}{r}\frac{1}{T}{j}^2+{\lambda }_{3}j\end{array}$$

   即让上式对$j$求导等于0，即

   $$\begin{array}{r}\frac{2}{T}j+{\lambda }_3=0\end{array}$$

   解得：

   $$j^{\ast }=-\frac{T}{2}{\lambda }_3$$

   将其代入到$H\left(s^{\ast }(t),u(t),\lambda (t)\right)$的表达式中，并且将$\lambda$关于α、β、γ的表达式也代入后，可得：

   $$\begin{array}{rl}{u}^{\ast }(t)& =j^{\ast }(t)=\arg \underset{j(t)}{\min }H(s^{\ast }(t),j(t),\lambda (t))\\ & =\frac{1}{2}\alpha t^2+\beta t+\gamma \end{array}$$

   至此，我们得到了仅含有待定系数$\alpha 、\beta 、\gamma$的最优输入量$u^{\ast }$的表达式，由最优的输入量$u^{\ast }$进行一次积分，可以得到最优状态量中的$a^{\ast }$,再进行一次积分得到$v^{\ast }$，再进行一次积分得到$p^{\ast }$，则最优的状态量$s^{\ast }$的表达式如下所示：

   $$s^{\ast }(t)=\left[\begin{array}{r}\frac{\alpha }{120}{t}^5+\frac{\beta }{24}{t}^4+\frac{\gamma }{6}{t}^3+\frac{a_0}{2}{t}^2+{\nu }_{0}t+p_0\\ \frac{\alpha }{24}{t}^4+\frac{\beta }{6}{t}^3+\frac{\gamma }{2}{t}^2+a_{0}t+{\nu }_0\\ \frac{\alpha }{6}{t}^3+\frac{\beta }{2}{t}^2+\gamma t+a_0\end{array}\right]$$

   将t=0代入到上式，即利用初始条件$s(0)=(p_0,v_0,a_0)$，可以确定上式中由于积分产生的系数$p_0、v_0、a_0$

   再把t=T代入上式，利用终止状态的条件$s(T)=(p_f,v_f,a_f)$，得到以下关系式

   $$\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-{\nu }_{0}T-\frac{1}{2}{a}_0{T}^2\\ {\nu }_f-{\nu }_0-a_{0}T\\ a_f-a_0\end{array}\right]$$

   由此，可以解得待定系数$\alpha 、\beta 、\gamma$

   $$\left[\begin{array}{c}\alpha \\ \beta \\ \gamma \end{array}\right]=\frac{1}{T^5}\left[\begin{array}{ccc}720& -360T& 60T^2\\ -360T& 168T^2& -24T^3\\ 60T^2& -24T^3& 3T^4\end{array}\right]\left[\begin{array}{c}\Delta p\\ \Delta \nu \\ \Delta a\end{array}\right]$$

   将上述解得的待定系数$\alpha 、\beta 、\gamma$代入到上面推导出的$u^{\ast }(t)$、$s^{\ast }(t)$的表达式中，即可得到所需的最优输入量（控制量）和最优状态量，其仅与时间T有关了，为时间T的函数。

   此外，得到最优的$u^{\ast }(t)$后，由于$u^{\ast }(t)=j^{\ast }(t)$、$J_k=\frac{1}{T}{\int }_0^T{j}_k(t{)}^{2}dt.$，我们可以把最优的目标函数值也求出来（还需要将$\alpha 、\beta 、\gamma$代入，代入后下面$J$的表达式就只跟T相关了）：

   $$J={\gamma }^2+\beta \gamma T+\frac{1}{3}{\beta }^2{T}^2+\frac{1}{3}\alpha \gamma T^2+\frac{1}{4}\alpha \beta T^3+\frac{1}{20}{\alpha }^2{T}^4$$

   即，我们只需要给定一个时间T，所有的量都确定了，如果不给定T，则目标函数的最小值，只需要将$J$对T求导等于0，解出最优的T，所有的量也可以确定了，至此，我们完整的解决了文章开头给出的问题。

---

   在上面的例子中，我们默认机器人必须到达指定的终末状态，即当机器人到达指定的终末状态时，终末状态的惩罚项为0，否则为无穷大：

   $$h(s(T))=\{\begin{array}{ll}0,& ifs=s(T)\\ \infty ,& otherwise\end{array}$$

   若，我们不指定所有的终末状态，比如只指定状态量p，而不指定v和a，即v和a是自由的，对于这些自由的状态，需要利用庞特里亚金最小值原理的第二个公式，添加边界条件

   $$given{s}_i(T),i\in I$$

   上式中属于$i$中的状态是指定的状态，不属于$i$的状态是自由状态，我们需要构建如下的边界条件

   $${\lambda }_j(T)=\frac{\partial h(s^{\ast }(T))}{\partial s_j},forj\ne i$$

---

---

   参考资料：

   1、深蓝学院-移动机器人运动规划

   2、庞特里亚金最小值原理解最优控制问题

---