路径规划算法中的OBVP原理介绍

1. 前景介绍

例如一个5次多项式表示两点间的轨迹方程
$$x(t)=c_5{t}^5+c_4{t}^4+c_3{t}^3+c_2{t}^2+c_1{t}^1+c_0$$
已知在t=0和t=T时刻，机器人的位置、速度、加速度如下

	位置p	速度v	加速度a
t=0	a	0	0
t=T	b	0	0

那么求解得：
$$\left[\begin{array}{c}a\\ b\\ 0\\ 0\\ 0\\ 0\end{array}\right]=\left[\begin{array}{c}x(0)\\ x(T)\\ x^{\prime }(0)\\ x^{\prime }(T)\\ x^{\prime \prime }(0)\\ x^{\prime \prime }(T)\end{array}\right]=\left[\begin{array}{c}x(0)\\ x(T)\\ v(0)\\ v(T)\\ a(0)\\ a(T)\end{array}\right]=\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 1\\ T^5& T^4& T^3& T^2& T& 1\\ 0& 0& 0& 0& 1& 0\\ 5T^4& 4T^3& 3T^2& 2T& 1& 0\\ 0& 0& 0& 2& 0& 0\\ 20T^3& 12T^2& 6T& 2& 0& 0\end{array}\right]\left[\begin{array}{c}{c}_5\\ c_4\\ c_3\\ c_2\\ c_1\\ c_0\end{array}\right]$$
上面的过程就是BVP(Boundary Value Problem)的解法，但是如果多项式的系数特别多，且机器人的状态变量依旧是位置、速度、加速度等，那么我们会得到很多个满足条件的解，但是我们不知道哪一个解是最优的，因此我们需要OBVP(Optimal Boundary Value Problem)方法。

---

2. 原理介绍

1：一般最小化目标函数由来两部分组成，final state:最终状态的惩罚项H，transition cost：整个系统的损失G。
$$J=H+G=h(s(T))+{\int }_0^{T}g(s(t),u(t))dt$$
其中$s$为状态变量，$u$为输入变量
2：构建系统的Hamiltonian(哈米而顿)矩阵$H$和协变量$\lambda$
$$\begin{gathered} H(s,u,\lambda )=g(s,u)+{\lambda }^{T}f(s,u) \\ \lambda =({\lambda }_1,{\lambda }_2,{\lambda }_3) \end{gathered}$$
其中$f(s,u)$为机器人的状态模型(状态方程，$\dot{s}=f(s,u)$),协变量的个数等于状态变量的个数。
3：根据Pontrayagin’s 最小值原理(详情参见附录1)得出最优的轨迹$s^{\ast }$和输入$u^{\ast }$,步骤如下
首先，通过Hamiltonian矩阵对协变量进行求导
$$\dot{\lambda }(t)=-{\nabla }_{s}H(s^{\ast }(t),u^{\ast }(t),\lambda (t))$$
然后，将得到的协变量代入Hamiltonian矩阵中，并且最小化输入变量,得出最优的输入变量
$$u^{\ast }(t)=\arg \underset{u(t)}{\min }H(s^{\ast }(t),u(t),\lambda (t))$$
其次，通过将最优输入变量进行积分，则得到最优轨迹的函数表达式$s^{\ast }$
最后，通过最优变量并结合边界条件，得出最优轨迹$s^{\ast }$表达式中的参数
$$\lambda (T)=-\nabla h(s^{\ast }(T))$$

---

3. 案例说明

对于一个3轴的无人机系统，OBVP的求解过程如下：

3.1 已知量

(1)：目标函数:
$$J_{\sum }=\sum _{k=1}^3{J}_k,J_k=\frac{1}{T}{\int }_0^T{j}_k{(t)}^{2}dt$$
由于无人机在$(x,y,z)$三轴上进行运动，故分别对每一轴进行最小化处理。

(2)： 变量

状态变量：$s_k=(p_k,v_k,a_k)$,这里为了下面推导方便，隐去k(k表示无人机的三个轴)

输入变量：$j_k$，这里为了下面推导方便，隐去k(k表示无人机的三个轴)

(3)：系统初值

系统0时刻的状态值$s(0)=(p_0,v_0,a_0{)}^T$

T时刻的状态值$s_f(T)=(p_f,v_f,a_f{)}^T$

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

3.2 求解

根据前面的原理描述，最小化目标函数的过程如下

1：确定目标函数$J$中的$H$和$G$函数
$$\begin{gathered} H=\{\begin{array}{llc}0& if& s=s(T)\\ \infty & other& \end{array} \\ G=\frac{1}{T}\times j_k(t{)}^2 \end{gathered}$$

其中H为阶跃函数，表示当时间$t=T$时，机器人当前状态必须与设定的状态相同，即位置、速度、加速度要和设置的保持一致，这就导致了目标函数不可导，因此并不能严格的按照Pontrayagin’s 最小值原理进行求解。

2：构建系统的Hamiltonian(哈米而顿)矩阵$H$和协变量$\lambda$
$$\begin{gathered} \lambda =({\lambda }_1,{\lambda }_2,{\lambda }_3) \\ H(s,u,\lambda )=\frac{1}{T}{j}^2+{\lambda }^T{f}_s(s,u) \\ =\frac{1}{T}{j}^2+{\lambda }_{1}v+{\lambda }_{2}a+{\lambda }_{3}j \end{gathered}$$
其中协变量$\lambda$的个数和状态变量个数相同。

3：通过Hamiltonian矩阵对协变量进行求导
$$\dot{\lambda }(t)=-{\nabla }_{s}H(s^{\ast }(t),u^{\ast }(t),\lambda (t))=(0,-{\lambda }_1,-{\lambda }_2)$$
通过常微分方程可得,协变量为
$$\lambda (t)=\left[\begin{array}{c}{\lambda }_1(t)\\ {\lambda }_2(t)\\ {\lambda }_3(t)\end{array}\right]=\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]$$
其中上式方程中的系数$(\alpha ,\beta )$均为了方便计算而设计的，大家可以与我不一致。

4：将得到的协变量代入Hamiltonian矩阵中，并且最小化输入变量,得出最优的输入变量
$$\begin{gathered} \because u^{\ast }(t)=j^{\ast }(t)=\arg \underset{j(t)}{\min }H(s^{\ast }(t),j(t),\lambda (t)) \\ \because H(s^{\ast }(t),j(t),\lambda (t))=\frac{1}{T}{j}^2+{\lambda }_1{v}^{\ast }+{\lambda }_2{a}^{\ast }+{\lambda }_{3}j \\ \therefore \frac{\delta H}{\delta j}=\frac{2}{T}j+{\lambda }_3=0 \\ \to j=-\frac{T}{2}{\lambda }_3 \\ \to u^{\ast }=j^{\ast }=\frac{1}{2}\alpha t^2+\beta t+\gamma \end{gathered}$$

同时，将最优输入变量代入目标函数中，化简得
$$J(T)=\frac{1}{T}{\int }_0^T{j}^{2}dt={\gamma }^2+\beta \gamma T^2+\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$$
5：由于将输入变量$u^{\ast }or{j}^{\ast }$进行积一次分，则得到最优的加速度$a^{\ast }$；积两次分，则得到最优的速度$v^{\ast }$；积三次分，则得到最优的位置$p^{\ast }$,并结合系统0时刻的初值，可得到最优的轨迹方程$s^{\ast }$。
$$s^{\ast }(t)=\left[\begin{array}{c}{p}^{\ast }(t)\\ v^{\ast }(t)\\ a^{\ast }(t)\end{array}\right]=\left[\begin{array}{c}\iiint u^{\ast }(t)+p_0\\ \iint u^{\ast }(t)+v_0\\ \int u^{\ast }(t)+a_0\end{array}\right]=\left[\begin{array}{c}\int v^{\ast }(t)+p_0\\ \int a^{\ast }(t)+v_0\\ \int u^{\ast }(t)+a_0\end{array}\right]$$
将最优的输入变量$u^{\ast }$代入得
$$s^{\ast }(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]$$
6：由于目标函数在t=T时刻并不可导，故不能通过Pontrayagin’s 最小值原理中的边界条件直接确定最优轨迹$s^{\ast }$。的参数$(\alpha 、\beta 、\gamma )$，而通过系统在$t=T$时的状态量进行确定。
$$s^{\ast }(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\mathrm{T}+p_0\\ \frac{\alpha }{24}{T}^4+\frac{\beta }{6}{T}^3+\frac{\gamma }{2}{T}^2+a_0\mathrm{T}+v_0\\ \frac{\alpha }{6}{\mathrm{T}}^3+\frac{\beta }{2}{T}^2+\gamma T+a_0\end{array}\right]=s_f(T)=\left[\begin{array}{c}{p}_f\\ v_f\\ a_f\end{array}\right]$$
化简得
$$\begin{gathered} \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-\frac{1}{2}{a}_0{T}^2\\ v_f-v_0-a_{0}T\\ a_f-a_0\end{array}\right] \end{gathered}$$
因此
$$\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 v\\ \Delta a\end{array}\right]$$
7：小结一下

$$\begin{gathered} u^{\ast }=j^{\ast }=\frac{1}{2}\alpha t^2+\beta t+\gamma \\ {s}^{\ast }(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] \\ \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 v\\ \Delta a\end{array}\right] \\ J(T)=\frac{1}{T}{\int }_0^T{j}^{2}dt={\gamma }^2+\beta \gamma T^2+\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 \end{gathered}$$
从上式可看出，目标函数$J$仅仅关于$T$的函数，因此对$J$进行求极值（涉及多项式求根的问题，参见附录2），然后代入上式，进行$u^{\ast },s^{\ast },[\alpha ,\beta ,\gamma ]$等参数的计算。

3.3 边界问题扩展

大家注意到一点，上述的案例中，我们要求无人机必须在$t=T$时刻到达$s_f=(p_f,v_f,a_f)$状态，因此目标函数中最终状态的惩罚项$H$为
$$H=\{\begin{array}{llc}0& if& s=s(T)\\ \infty & other& \end{array}$$
这就导致了目标函数在最终状态并不可导，使得Pontrayagin’s 最小值原理的边界条件失效，因此需要通过无人机最终状态值进行各参数的确定。
但是在实际过程中，我们可能并不需要机器人在$t=T$时刻一定到达$s_f$状态，而是只满足其中一个或者两个条件即可。例如，我们仅仅需要机器人在$T$时刻到达$p_f$位置，而对它当前的速度和加速度并不关心，即$s_f=(p_f,?,?)$。此时需要Pontrayagin’s 最小值原理的边界条件帮助我们确定方程的系数。数学表达如下：
$$\lambda (T)=-\nabla h(s^{\ast }(T))$$
表达式转化如下：
$$\begin{gathered} given{s}_i(T),i\in I \\ {\lambda }_j(T)=\frac{\partial h(s^{\ast }(T))}{\partial s_j},forj\ne i \end{gathered}$$
其中$I$表示状态变量中已确定的参数，例如$s_f=(p_f,?,?)$中，$I=p_f$

---

附录

1.Pontrayagin’s 最小值原理

2. 多项式求根

通过矩阵特征值求解多项式的根，具体步骤如下

1.多项式方程如下
$$p(x)=c_4{x}^4+c_3{x}^3+c_2{x}^2+c_{1}x+c_0$$
2.矩阵的特征值表达式如下
$$Ay=\lambda y$$
如果多项式的根为矩阵的特征值，那么
$$Ay=xy$$
这也就是说存在一个矩阵A和向量y，使得多项式方程化成$Ay=xy$

3.再次观察上述的多项式
$$\begin{gathered} c_4{x}^4+c_3{x}^3+c_2{x}^2+c_{1}x+c_0=0 \\ \to x^4=-\frac{c_3}{c_4}{x}^3-\frac{c_2}{c_4}{x}^2-\frac{c_1}{c_4}x-\frac{c_0}{c_4} \\ \because x^3=1\times x^3+0\times x^2+0\times x+0 \\ \because x^2=0\times x^3+1\times x^2+0\times x+0 \\ \because x^1=0\times x^3+0\times x^2+1\times x+0 \\ \because 1=0\times x^3+0\times x^2+0\times x+1 \end{gathered}$$
令$y=(x^3,x^2,x,1{)}^T$,那么矩阵$A$为
$$A=\left[\begin{array}{cccc}-\frac{c_3}{c_4}& -\frac{c_2}{c_4}& -\frac{c_1}{c_4}& -\frac{c_0}{c_4}\\ 1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\end{array}\right]$$
即
$$\left[\begin{array}{cccc}-\frac{c_3}{c_4}& -\frac{c_2}{c_4}& -\frac{c_1}{c_4}& -\frac{c_0}{c_4}\\ 1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}{x}^3\\ x^2\\ x\\ 1\end{array}\right]=x\left[\begin{array}{c}{x}^3\\ x^2\\ x\\ 1\end{array}\right]$$
此时求解A的特征值，就为上述多项式的根。参考：https://blog.csdn.net/yulongshanmuxi/article/details/115612487

注意:码字不易，转载请注明出处，谢谢