如何对小型固定翼无人机进行最优的路径跟随控制？

控制架构

文章继续采用的是 ULTRA-Extra无人机，相关参数如下：

这里用于guidance law的无人机运动学模型为：
$$\{\begin{array}{l}{\dot{x}}_p=V_a\cos \gamma \cos \chi +V_w\cos {\gamma }_w\cos {\chi }_w\\ {\dot{y}}_p=V_a\cos \gamma \sin \chi +V_w\cos {\gamma }_w\sin {\chi }_w\\ {\dot{z}}_p=V_a\sin \gamma +V_w\sin {\gamma }_w\\ \dot{\chi }=g\tan \varphi /V_a\\ \dot{\gamma }=g(n_z\cos \varphi -\cos \gamma )/V_a\end{array}$$
其中状态量为$(x_p,y_p,z_p,\gamma ,\chi )$，控制量为$(V_a,n_z,\varphi )$。在自动驾驶仪(Autopilot)中，采用 Successive-Loop-Closure (SLC)实现参考量$(V_{a_m},n_{z_m},{\varphi }_m)$的信号跟踪：

自动驾驶仪中依然采用横纵向通道的SLC实现控制，相应的控制逻辑如下：

Path Following 最优控制器

对运动学模型进行二阶求导可以得到：
$$\left(\begin{array}{c}{\dot{x}}_p\\ {\dot{y}}_p\\ {\dot{z}}_p\\ \dot{\chi }\\ \dot{\gamma }\\ {\ddot{x}}_p\\ {\ddot{y}}_p\\ {\ddot{z}}_p\\ \ddot{\chi }\\ \ddot{\gamma }\\ {\dot{V}}_a\\ \dot{\varphi }\\ {\dot{n}}_z\end{array}\right)=\left(\begin{array}{ccccc}{O}_{5\times 5}& & I_5& & O_{5\times 3}\\ & & -V_a\cos \gamma \sin \chi & -V_a\sin \gamma \cos \chi & \\ & & V_a\cos \gamma \cos \chi & -V_a\sin \gamma \sin \chi & \\ O_{5\times 5}& O_{5\times 3}& 0& V_a\cos \gamma & O_{5\times 3}\\ & & 0& 0& \\ & & 0& \frac{g\sin \gamma }{V_a^{}}& \\ & & & O_{3\times 13}& \end{array}\right)\left(\begin{array}{c}{x}_p\\ y_p\\ z_p\\ \chi \\ \gamma \\ {\dot{x}}_p\\ {\dot{y}}_p\\ {\dot{z}}_p\\ \dot{\chi }\\ \dot{\gamma }\\ V_a\\ \varphi \\ n_z\end{array}\right)+\left(\begin{array}{ccc}& O_{5\times 3}& \\ \cos \gamma \cos \chi & 0& 0\\ \cos \gamma \sin \chi & 0& 0\\ \sin \gamma & 0& 0\\ -\frac{g\tan \varphi }{V_a^2}& \frac{g}{V_a{\cos }^2\varphi }& 0\\ \frac{g(\cos \gamma -n_z\cos \varphi )}{V_a^2}& -\frac{g{n}_z\sin \varphi }{V_a^{}}& \frac{g\cos \varphi }{V_a^{}}\\ & I_3& \end{array}\right)\left(\begin{array}{rl}& {\dot{V}}_a\\ & \dot{\varphi }\\ & {\dot{n}}_z\end{array}\right)$$
这里设$\rho =(\gamma ,\chi ,V_a,\varphi ,n_z{)}^T$，$x=(x_p,y_p,z_p,\chi ,\gamma ,{\dot{x}}_p,{\dot{y}}_p,{\dot{z}}_p,\dot{\chi },\dot{\gamma },V_a,\varphi ,n_z{)}^T$，$u=({\dot{V}}_a,\dot{\varphi },{\dot{n}}_z{)}^T$，得到：
$$\dot{x}=A_v(\rho )x+B_v(\rho )u$$

假设要跟踪的量为$r=(x_r,y_r,z_r{)}^T$，构造跟踪向量$e=(x_r-x_p,y_r-y_p,z_r-z_p{)}^T=r-(x_p,y_p,z_p{)}^T$，$\dot{e}=\dot{r}-({\dot{x}}_p,{\dot{y}}_p,{\dot{z}}_p{)}^T=\dot{r}-Cx$,有：
$$\left(\begin{array}{c}\dot{x}\\ \dot{e}\end{array}\right)=\left(\begin{array}{cc}{A}_v(\rho )& O_{13\times 3}\\ -C& O_{3\times 3}\end{array}\right)\left(\begin{array}{c}x\\ e\end{array}\right)+\left(\begin{array}{c}{B}_v(\rho )\\ O_{3\times 3}\end{array}\right)u+\left(\begin{array}{c}{O}_{13\times 1}\\ \dot{r}\end{array}\right)$$
上市被描述为：
$${\dot{x}}_e=A_e(\rho )x_e+B_e(\rho )u+c_e$$
其中，
$$C=\left(\begin{array}{c}{O}_{3\times 5}|I_3|O_{3\times 5}\end{array}\right)$$

利用4阶Runge-Kutta法可以将上式可以离散化为一个LPV状态空间方程（linear parameter varying state-space representation）：
$$x_{e,k+1}=A_e({\rho }_k)x_{e,k}+B_e({\rho }_k)u_{e,k}+c_{r,k}$$
其中，$T_s$是采样时间，
$$\begin{gathered} A_e({\rho }_k)=\frac{1}{24}{A}_e({\rho }_k{)}^4{T}_s^4+\frac{1}{6}{A}_e^3({\rho }_k)T_s^3+\frac{1}{2}{A}_e({\rho }_k{)}^2{T}_s^2+A_e({\rho }_k)T_s+I \\ {B}_e({\rho }_k)=\frac{1}{24}{A}_e({\rho }_k{)}^3{B}_e({\rho }_k)T_s^4+\frac{1}{6}{A}_e^2({\rho }_k)B_e({\rho }_k)T_s^3+\frac{1}{2}{A}_e({\rho }_k)B_e({\rho }_k)T_s^2+B_e({\rho }_k)T_s \end{gathered}$$

上述轨迹跟踪问题可以转化为：
$$\begin{gathered} \underset{u(t)}{\min }J[u(t)]={\int }_{t_0}^{t_f}1+x(t{)}^{T}Qx(t)+u(t{)}^{T}Ru(t)dt \\ \dot{x}(t)=A_v(\rho )x(t)+B_v(\rho )u(t) \\ x(t_0)=x_0,Ex(t_f)=(x_r,y_r,z_r{)}^T \\ {d}_{\min }\le Dx(t)\le d_{\max } \end{gathered}$$
其中：$E=(I_3,O_{3\times 10})$, $D=(O_{3\times 10},I_3)$，$Q=Q^T\ge 0,R=R^T\ge 0$，$d_{\min }=(V_{a\min },{\varphi }_{a\min },n_{z\min }{)}^T$,$d_{\max }=(V_{a\max },{\varphi }_{a\max },n_{z\max }{)}^T$​。令$\frac{\partial H}{\partial u}=2Ru+B_v(\rho {)}^T\lambda =0$，得到：
$$u=-\frac{1}{2}{R}^{-1}{B}_v(\rho {)}^T\lambda$$
构造Hamilton函数$H=1+x^{T}Qx+u^{T}Ru+{\lambda }^T[A_v(\rho )x+B_v(\rho )u]$，令$\rho =x$：
$$\{\begin{array}{l}\dot{\lambda }=-\frac{\partial H}{\partial x}=-(2Qx+{\lambda }^T\frac{\partial }{\partial x}(A_v(\rho )x+B_v(\rho )u))\\ \dot{x}=\frac{\partial H}{\partial \lambda }=A_v(\rho )x+B_v(\rho )u\end{array}$$
其中，
$$\begin{gathered} \frac{\partial }{\partial x}[A_v(\rho )x]=? \\ \frac{\partial }{\partial x}[B_v(\rho )u]=-\frac{1}{2}\frac{\partial }{\partial x}[B_v(\rho )R^{-1}{B}_v(\rho {)}^T\lambda ]=? \end{gathered}$$
其中$H(t_f)=0$，应该采用打靶法得到$t_f$和${\lambda }_0$，能使得：
$$\begin{gathered} ||Ex(t_f)-(x_r,y_r,z_r{)}^T||\le {\epsilon }_1 \\ ||H(t_f)||\le {\epsilon }_2 \\ {d}_{\min }\le Dx(t)\le d_{\max } \end{gathered}$$
获取上述的量后，如何就可以用Matlab的ode45函数，或者直接采用bvp4c将上述两点边值问题（BVP），迭代出最优轨迹和最优策略。