采用qL-MPC技术进行小型固定翼无人机的路径跟随控制

来自论文"Predictive Path-Following Control for Fixed-Wing UAVs Using the qLMPC Framework in the Presence of Wind Disturbances"

控制架构

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

在这里插入图片描述

这里用于guidance law的无人机运动学模型为：
$\begin{cases} \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\phi/V_a \\ \dot{\gamma} = g(n_z\cos\phi-\cos\gamma)/V_a \end{cases}$
其中状态量为 $(x_p,y_p,z_p,\gamma,\chi)$ ，控制量为 $(V_a,n_z,\phi)$ 。在自动驾驶仪(Autopilot)中，采用 Successive-Loop-Closure (SLC)实现参考量 $(V_{a_m},n_{z_m},\phi_m)$ 的信号跟踪：
在这里插入图片描述

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

在这里插入图片描述

Path Following 最优控制器

对运动学模型进行二阶求导可以得到：
$\left( \begin{matrix} {{{\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 }} \\ \end{matrix} \right)=\left( \begin{matrix} {{O}_{5\times 5}} & {} & {{I}_{5}} & {} \\ {} & {} & -{{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 \\ {} & {} & 0 & 0 \\ {} & {} & 0 & \frac{g\sin \gamma }{V_{a}^{{}}} \\ \end{matrix} \right)\left( \begin{matrix} {{x}_{p}} \\ {{y}_{p}} \\ {{z}_{p}} \\ \chi \\ \gamma \\ {{{\dot{x}}}_{p}} \\ {{{\dot{y}}}_{p}} \\ {{{\dot{z}}}_{p}} \\ {\dot{\chi }} \\ {\dot{\gamma }} \\ \end{matrix} \right)+\left( \begin{matrix} {} & {{O}_{5\times 3}} & {} \\ \cos \gamma \cos \chi & 0 & 0 \\ \cos \gamma \sin \chi & 0 & 0 \\ \sin \gamma & 0 & 0 \\ -\frac{g\tan \phi }{V_{a}^{2}} & \frac{g}{{{V}_{a}}{{\cos }^{2}}\phi } & 0 \\ \frac{g(\cos \gamma -{{n}_{z}}\cos \phi )}{V_{a}^{2}} & -\frac{g{{n}_{z}}\sin \phi }{V_{a}^{{}}} & \frac{g\cos \phi }{V_{a}^{{}}} \\ \end{matrix} \right)\left( \begin{align} & {{{\dot{V}}}_{a}} \\ & {\dot{\phi }} \\ & {{{\dot{n}}}_{z}} \\ \end{align} \right)$
这里设 $\rho=(\gamma,\chi,V_a,\phi,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})^T$ ， $u=(\dot{V}_a,\dot{\phi},\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}-(O_{3\times 5},I_3,O_{3\times 2})x$ ，得到：
$\begin{pmatrix} \dot{x} \\ \dot{e} \end{pmatrix} = \begin{pmatrix} A_v(\rho) & O_{10\times3} \\ -(O_{3\times 5}|I_3|O_{3\times 2}) & O_3 \end{pmatrix}\begin{pmatrix} x \\e \end{pmatrix} + \begin{pmatrix} B_v(\rho) \\O_{3\times 3} \end{pmatrix} u + \begin{pmatrix} O_{10\times 1} \\\dot{r} \end{pmatrix}$
利用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}$
令 $P_k=(\rho_{k}^T,\rho_{k+1}^T,...\rho_{k+N-1}^T)^T$ ， $X_k=(x_{e,k+1}^T,x_{e,k+2}^T,...,x_{e,k+N}^T)^T$ ， $U_k=(u_{e,k+1}^T,u_{e,k+2}^T,...,u_{e,k+N}^T)^T$ ， $c_k = (c_{r,k+1}^T,c_{r,k+2}^T,...c_{r,k+N}^T)^T$ 得到：
$X_{k+1}=H(P_k)X_k + S(P_k)U_k+c_k \\ =L_k + S(P_k)U_k$
其中： $x_{e,k}=(x^T_k,e^T_k)^T$ ， $H(P_k) =diag([A_e(\rho_{k}),A_e(\rho_{k+1})...A_e(\rho_{k+N-1})])$ ， $S(P_k)=diag([B_e(\rho_{k}),B_e(\rho_{k+1})...B_e(\rho_{k+N-1})])$ 。采用MPC控制器进行设计时， $k + 1$ 时刻需要优化的目标函数：
$J_{k+1}=\sum_{i=1}^N(x_{k+i+1}^TQx_{k+i+1} + u_{k+i}^TRu_{k+i} + e_{k+i+1}^TTe_{k+i+1}) \\ =X_{k+1}^TH_XX_{k+1} + U_{k}^TH_UU_{k}\\=[L_k + S(P_k)U_k]^TH_X[L_k + S(P_k)U_k] + U_{k}^TH_UU_{k}\\ =U_k^T(S(P_k)^TH_XS(P_k)+H_U)U_k + 2L_k^TH_XS(P_k)U_k + L_k^TH_XL_k$
其中： $Q=Q^T>0,P=P^T>0,R=R^T>0$ ； $H_X=diag([Q,Q,...,Q])$ ， $H_U=diag([R,R,...,R])$ 。

而对于控制量 $U_k$ 和状态量 $X_{k+1}$ 有限幅，即： $U_{\min}\leq U_k\leq U_{\max}$ ， $X_{\min} \leq X_{k+1} \leq X_{\max}$ ，得到约束：
$\begin{pmatrix} I \\ -I\\ S(P_k)\\ -S(P_k) \end{pmatrix}U_k \leq \begin{pmatrix} U_{\max} \\ -U_{\min} \\ X_{k+1} - L_k \\ -X_{k+1} + L_k \end{pmatrix}$

上面的假设是基于全状态反馈的，也是就是说对于 $k + 1$ 时刻的在线优化能获取 $k$ 时刻所有的状态信息和偏差信息。

若观测量为 $Y_k = C_kX_k$ ， $Y_{\min}\leq Y_k\leq Y_{\max}$ ，则上面的约束将被修正为：
$\begin{pmatrix} I \\ -I\\ C_kS(P_k)\\ -C_kS(P_k) \end{pmatrix}U_k \leq \begin{pmatrix} U_{\max} \\ -U_{\min} \\ Y_{\max} - C_kL_k \\ -Y_{\min} + C_kL_k \end{pmatrix}$
无论如何，上述问题都可以被转化成QP问题，利用Matlab工具箱中的quadprog函数进行求解，或者说是在线优化为以下问题：
$\min_{U_k}\frac{1}{2}U_k^TF_kU_k +G_kU_k \\ A_kU_k \leq b_k$
附带相应的伪代码如下图所示：

在这里插入图片描述

参考文献

@inproceedings{bib:Samir,title={Predictive Path Following Control for Fixed Wing UAVs Using the qLMPC Framework in the Presence of Wind Disturbances},author={Rezk, Ahmed S and Calder{\'o}n, Horacio M and Werner, Herbert and Herrmann, Benjamin and Thielecke, Frank},booktitle={AIAA SCITECH 2024 Forum},pages={1594},year={2024}
}