CN104865970A - 一种无人机飞行轨迹跟踪控制系统 - Google Patents

Abstract

本发明公开的一种无人机飞行轨迹跟踪控制系统，包括轨迹制导器、纵向控制器和侧向控制器。在本发明的无人机飞行轨迹跟踪控制系统作用下，无人机能够很快实现爬升，消除高度误差，并保持航迹跟踪，总体轨迹跟踪精度较高。并且本发明的跟踪控制系统能够充分利用传感器测量反馈得到的飞行状态信息，通过融合更多的有用信息，达到了很高的控制精度。适用于飞行器这类复杂多变量系统。

Description

一种无人机飞行轨迹跟踪控制系统

技术领域

本发明涉及固定翼无人机飞行控制技术领域，具体是一种无人机飞行轨迹跟踪控制系统。

背景技术

无人机飞行轨迹控制系统的任务是使无人机能自动地按预定航线飞行，对飞行器偏离参考航路线距离的控制。

经典PID控制在无人机轨迹跟踪控制中的应用最为广泛，也有较好的使用效果。然而，它是一种单变量设计方法，最适合用于单变量系统，而无人机属于复杂的多变量系统，各个状态之间的耦合较为严重，使用PID控制在设计上通常忽略各个状态之间的耦合影响，控制参数较多，控制参数的设计过程也较为复杂。

发明内容

本发明要解决的问题是提供是一种无人机飞行轨迹跟踪控制系统，该方法基于信息融合控制，适用于无人机这种复杂的多变量系统控制，控制参数少，控制过程简单。

本发明公开的一种无人机飞行轨迹跟踪控制系统，包括轨迹制导器、纵向控制器和侧向控制器；

所述轨迹制导器的纵向制导律为：

$V_g=\sqrt{{{\stackrel{\·}{x}}_g}^2+{{\stackrel{\·}{y}}_g}^2+{{\stackrel{\·}{h}}_g}^2}---\left(1\right)$

${\mathrm{\α}}_g=\frac{\mathrm{mg}(1-\cos {\mathrm{\φ}}_g)}{{\mathrm{QSC}}_{\mathrm{L\α}}\cos {\mathrm{\φ}}_g}---\left(2\right)$

${\mathrm{\θ}}_g={\mathrm{\γ}}_g+{\mathrm{\α}}_g+k_h(\mathrm{\Δh}-\mathrm{\Δ}{h}_g)+k_{\stackrel{\·}{h}}\mathrm{\Δ}\stackrel{\·}{h}=\arcsin \left(\frac{{\stackrel{\·}{h}}_g}{V_g}\right)-{\mathrm{\α}}_g+k_h(\mathrm{\Δh}-\mathrm{\Δ}{h}_g)+k_{\stackrel{\·}{h}}\mathrm{\Δ}\stackrel{\·}{h}---\left(3\right)$

$q_g=\frac{g}{V_g}\cos {\mathrm{\θ}}_g\sin {\mathrm{\φ}}_g\tan {\mathrm{\φ}}_g---\left(4\right)$

所述轨迹制导器中的横侧向制导律为：

β_g＝0  (5)

${\mathrm{\φ}}_g=\arctan \frac{{\stackrel{\·}{\mathrm{\ψ}}}_g{V}_g}{g}+k_{d}d+k_{\stackrel{\·}{d}}\stackrel{\·}{d},$ 其中

p_g＝0  (7)

$r_g=\frac{g}{V_g}\cos {\mathrm{\θ}}_g\sin {\mathrm{\φ}}_g---\left(8\right)$

所述纵向控制器的纵向控制律基于信息融合控制算法原理设计为：

$U_{\mathrm{lon}}\left(k\right)={\tilde{R}}_{\mathrm{lon}}(k+1){\tilde{x}}_{\mathrm{lon}}(k+1)-{\tilde{R}}_{\mathrm{lon}}(k+1)A_{\mathrm{lon}}{X}_{\mathrm{lon}}\left(k\right)---\left(9\right)$

式中，

${\tilde{R}}_{\mathrm{lon}}(k+1)={[R_{\mathrm{lon}}+{B_{\mathrm{lon}}}^T{P_{\mathrm{lon}}}^{-1}(k+1)B_{\mathrm{lon}}]}^{-1}{B_{\mathrm{lon}}}^T{P_{\mathrm{lon}}}^{-1}(k+1)---\left(10\right)$

$\left\{\begin{array}{c}{\hat{x}}_{\mathrm{lon}}\left(k\right)=P_{\mathrm{lon}}\left(k\right){C_{\mathrm{lon}}}^T{Q}_{\mathrm{lon}}{y_{\mathrm{lon}}}^{*}\left(k\right)+P_{\mathrm{lon}}\left(k\right){A_{\mathrm{lon}}}^T{[P_{\mathrm{lon}}(k+1)+B_{\mathrm{lon}}{R_{\mathrm{lon}}}^{-1}{B_{\mathrm{lon}}}^T]}^{-1}{\hat{x}}_{\mathrm{lon}}(k+1)\\ {\hat{x}}_{\mathrm{lon}}(k+N)=P_{\mathrm{lon}}\left(N\right){C_{\mathrm{lon}}}^T{Q}_{\mathrm{lon}}{y_{\mathrm{lon}}}^{*}\left(N\right)\end{array}\right.---\left(11\right)$

$\left\{\begin{array}{c}{P_{\mathrm{lon}}}^{-1}\left(k\right)={C_{\mathrm{lon}}}^T{Q}_{\mathrm{lon}}{C}_{\mathrm{lon}}+{A_{\mathrm{lon}}}^T{[P_{\mathrm{lon}}(k+1)+B_{\mathrm{lon}}{R_{\mathrm{lon}}}^{-1}{B_{\mathrm{lon}}}^T]}^{-1}{A}_{\mathrm{lon}}\\ {P_{\mathrm{lon}}}^{-1}(k+N)={C_{\mathrm{lon}}}^T{Q}_{\mathrm{lon}}{C}_{\mathrm{lon}}\end{array}\right.---\left(12\right)$

${\tilde{R}}_{\mathrm{lon}}={(R_{\mathrm{lon}}+{B_{\mathrm{lon}}}^T{P_{\mathrm{lon}}}^{-1}{B}_{\mathrm{lon}})}^{-1}{B_{\mathrm{lon}}}^T{P_{\mathrm{lon}}}^{-1}---\left(13\right)$

所述侧向控制器的横侧向控制律基于信息融合控制算法原理设计为：

$U_{\mathrm{lat}}\left(k\right)={\tilde{R}}_{\mathrm{lat}}(k+1){\hat{x}}_{\mathrm{lat}}(k+1)-{\tilde{R}}_{\mathrm{lat}}(k+1)A_{\mathrm{lat}}{X}_{\mathrm{lat}}\left(k\right)---\left(14\right)$

式中，

${\tilde{R}}_{\mathrm{lat}}(k+1)={[R_{\mathrm{lat}}+{B_{\mathrm{lat}}}^T{P_{\mathrm{lat}}}^{-1}(k+1)B_{\mathrm{lat}}]}^{-1}{B_{\mathrm{lat}}}^T{P_{\mathrm{lat}}}^{-1}(k+1)---\left(15\right)$

$\left\{\begin{array}{c}{\hat{x}}_{\mathrm{lat}}\left(k\right)=P_{\mathrm{lat}}\left(k\right){C_{\mathrm{lat}}}^T{Q}_{\mathrm{lat}}{y_{\mathrm{lat}}}^{*}\left(k\right)+P_{\mathrm{lat}}\left(k\right){A_{\mathrm{lat}}}^T{[P_{\mathrm{lat}}(k+1)+B_{\mathrm{lat}}{R_{\mathrm{lat}}}^{-1}{B_{\mathrm{lat}}}^T]}^{-1}{\hat{x}}_{\mathrm{lat}}(k+1)\\ {\hat{x}}_{\mathrm{lat}}(k+N)=P_{\mathrm{lat}}\left(N\right){C_{\mathrm{lat}}}^T{Q}_{\mathrm{lat}}{y_{\mathrm{lat}}}^{*}(k+N)\end{array}\right.---\left(16\right)$

$\left\{\begin{array}{c}{P_{\mathrm{lat}}}^{-1}\left(k\right)={C_{\mathrm{lat}}}^T{Q}_{\mathrm{lat}}{C}_{\mathrm{lat}}+{A_{\mathrm{lat}}}^T{[P_{\mathrm{lat}}(k+1)+B_{\mathrm{lat}}{R_{\mathrm{lat}}}^{-1}{B_{\mathrm{lat}}}^T]}^{-1}{A}_{\mathrm{lat}}\\ {P_{\mathrm{lat}}}^{-1}(k+N)={C_{\mathrm{lat}}}^T{Q}_{\mathrm{lat}}{C}_{\mathrm{lat}}\end{array}\right.---\left(17\right)$

${\tilde{R}}_{\mathrm{lat}}={(R_{\mathrm{lat}}+{B_{\mathrm{lat}}}^T{P_{\mathrm{lat}}}^{-1}{B}_{\mathrm{lat}})}^{-1}{B_{\mathrm{lat}}}^T{P_{\mathrm{lat}}}^{-1}---\left(18\right)$

通过本发明的追踪控制系统得到无人机的控制信号，包括油门开度、升降舵偏角、副翼偏角和方向舵偏角，该控制信号输出到无人机系统中，能够控制飞机跟踪预定参考飞行轨迹。在本发明的无人机飞行轨迹跟踪控制系统作用下，无人机能够很快实现爬升，消除高度误差，并保持航迹跟踪，总体轨迹跟踪精度较高。并且本发明的跟踪控制系统能够充分利用传感器测量反馈得到的飞行状态信息，通过融合更多的有用信息，达到了很高的控制精度。适用于飞行器这类复杂多变量系统。

附图说明

图1为本发明的无人机飞行轨迹跟踪控制系统原理框图；

图2是本发明的无人机飞行轨迹跟踪控制系统的高度跟踪控制响应图；

图3是本发明的无人机飞行轨迹跟踪控制系统的高度跟踪误差曲线图；

图4是本发明的无人机飞行轨迹跟踪控制系统的航迹跟踪控制响应图；

图5是本发明的无人机飞行轨迹跟踪控制系统的航迹跟踪偏航距曲线图。

具体实施方式

下面结合附图，对本发明提出的一种无人机飞行轨迹跟踪控制系统进行详细说明。

本发明提出了一种基于信息融合控制理论的无人机飞行轨迹跟踪控制系统。信息融合控制理论是把控制问题从决策或估计的角度考察时，所有关于控制量的约束信息，均视为关于控制量(被估计量)的传感信息，由此估计出控制量，称作融合控制量。这些约束通常以某种指标函数达到极值的形式出现，所以融合解通常为最优解。因此，信息融合控制是一种最优控制，适用于无人机这种多变量系统。

如图1所示，本发明的无人机飞行轨迹跟踪控制系统包括轨迹制导器、纵向控制器和侧向控制器。

1、轨迹制导器：

无人机做纵向运动时，被控量为速度V、迎角α、俯仰角θ和俯仰角速率q，控制量为油门开度和升降舵偏角。此时，根据飞行器动力学和运动学原理，轨迹制导器中的纵向制导律设计为：

$V_g=\sqrt{{{\stackrel{\·}{x}}_g}^2+{{\stackrel{\·}{y}}_g}^2+{{\stackrel{\·}{h}}_g}^2}---\left(1\right)$

${\mathrm{\α}}_g=\frac{\mathrm{mg}(1-\cos {\mathrm{\φ}}_g)}{\frac{1}{2}\mathrm{\ρ}{V}^2{\mathrm{SC}}_{\mathrm{L\α}}\cos {\mathrm{\φ}}_g}---\left(2\right)$

${\mathrm{\θ}}_g={\mathrm{\γ}}_g+{\mathrm{\α}}_g+k_h(\mathrm{\Δh}-\mathrm{\Δ}{h}_g)+k_{\stackrel{\·}{h}}\mathrm{\Δ}\stackrel{\·}{h}=\arcsin \left(\frac{{\stackrel{\·}{h}}_g}{V_g}\right)+{\mathrm{\α}}_g+k_h(\mathrm{\Δh}-\mathrm{\Δ}{h}_g)+k_{\stackrel{\·}{h}}\mathrm{\Δ}\stackrel{\·}{h}---\left(3\right)$

$q_g=\frac{g}{V_g}\cos {\mathrm{\θ}}_g\sin {\mathrm{\φ}}_g\tan {\mathrm{\φ}}_g---\left(4\right)$ 式中，g是重力加速度，ρ是空气密度，S是产生升力的等效面积，C_Lα是升力线斜率，x,y,h是无人机轨迹X轴方向位置、Y轴方向位置以及高度，Δh为高度差，γ是航迹倾斜角，φ是滚转角，下标g表示给定值。k_h、k_h为轨迹纵向制导参数，引入高度差及其导数的目的是为了通过俯仰运动消除高差误差。

无人机作横侧向运动时，通常采用协调转弯飞行，此时垂直向上的升力分量与重力平衡，水平方向的升力分量和离心力平衡。此时，轨迹制导器中的横侧向制导律为：

β_g＝0  (5)

${\mathrm{\φ}}_g=\arctan \frac{{\stackrel{\·}{\mathrm{\ψ}}}_g{V}_g}{g}+k_{d}d+k_{\stackrel{\·}{d}}\stackrel{\·}{d},$ 其中

p_g＝0  (7)

$r_g=\frac{g}{V_g}\cos {\mathrm{\θ}}_g\sin {\mathrm{\φ}}_g---\left(8\right)$

式中，β是侧滑角，ψ是偏航角，p是滚转角速率，r是偏航角速率，d为偏航距，k_d、为轨迹横侧向制导参数。引入偏航距及其导数的目的是为了通过滚转运动消除与参考飞行轨迹之间的航迹偏差。

上述期望信息是根据已知的飞行轨迹信息解算获得，并分别输出到纵向控制器和横侧向控制。

2、纵向控制器与横侧向控制器：

在设计纵向控制器与横侧向控制器之前，先阐述一下设计的原理。

设受扰被控对象的离散线性状态方程和输出方程可表示为

$\left\{\begin{array}{c}x(k+1)=\mathrm{Ax}\left(k\right)+\mathrm{Bu}\left(k\right),k=\mathrm{0,1,2},...,N-1\\ y\left(k\right)=\mathrm{Cx}\left(k\right)\end{array}\right.---\left(9\right)$

式中，x为状态向量，u为控制输入向量，y为系统输出向量，A,B,C为系统矩阵，N是为末端时刻即数据窗口长度。

控制目标是求解最优控制序列，使系统输出跟踪期望轨迹，并使指标

$J={\left|\right|y^{*}(k+N)-y(k+N)\left|\right|}_Q^2+\underset{j=0}{\overset{N-1}{\mathrm{\Σ}}}[{\left|\right|y^{*}(k+j)-y(k+j)\left|\right|}_Q^2+{\left|\right|u(k+j)\left|\right|}_R^2]---\left(10\right)$

为最小。式中，y^*为期望输出向量，Q,R为可调节的权重矩。

根据王志胜等编著的《融合估计与融合控制》(科学出版社出版)书中的基本信息融合控制理论。可以得到上述跟踪控制系统的信息融合控制算法如下：

$\stackrel{\‾}{u}\left(k\right)=-\stackrel{\‾}{K}\left(k\right)x\left(k\right)-\stackrel{\‾}{v}\left(k\right)---\left(11\right)$

式中，

$\stackrel{\‾}{K}\left(k\right)={[R+B^T{P}^{-1}(k+1)B]}^{-1}{B}^T{P}^{-1}(k+1)A---\left(12\right)$

$\stackrel{\‾}{v}\left(k\right)={-[R+B^T{P}^{-1}(k+1)B]}^{-1}{B}^T{P}^{-1}(k+1)\hat{x}(k+1)---\left(13\right)$

$\left\{\begin{array}{c}\hat{x}\left(k\right)=P\left(k\right)C^T{\mathrm{Qy}}^{*}\left(k\right)+P\left(k\right)\left\{A^T{[P(k+1)+{\mathrm{BR}}^{-1}{B}^T]}^{-1}\hat{x}(k+1)\right\}\\ \hat{x}(k+N)=P\left(N\right)C^T{\mathrm{Qy}}^{*}\left(N\right)\end{array}\right.---\left(14\right)$

$\left\{\begin{array}{c}{P}^{-1}\left(k\right)=C^T\mathrm{QC}+A^T{[P(k+1)+{\mathrm{BR}}^{-1}{B}^T]}^{-1}A\\ P^{-1}(k+N)=C^T\mathrm{QC}\end{array}\right.---\left(15\right)$

式中，为控制输入向量u的估计值，y^*为已知的系统输出期望值即参考输入信号，为协状态估计值，P^-1为协状态信息量。

基于上述信息融合控制算法原理，下面推导在无人机飞行轨迹跟踪控制中的应用。

将无人机的纵向线性状态方程与输出方程离散化为

$\left\{\begin{array}{c}{X}_{\mathrm{lon}}(k+1)=A_{\mathrm{lon}}{X}_{\mathrm{lon}}\left(k\right)+B_{\mathrm{lon}}{U}_{\mathrm{lon}}\left(k\right)\\ Y_{\mathrm{lon}}\left(k\right)=C_{\mathrm{lon}}{X}_{\mathrm{lon}}\left(k\right)\end{array}\right.---\left(16\right)$

式中，X_lon＝[ΔV,Δα,Δθ,Δq]^T，U_lon＝[Δδ_e,Δδ_T]^T，δ_e为升降舵偏角，δ_T为油门开度，A_lon、B_lon、C_lon是飞机纵向线性模型经过离散化后的系统矩阵，Δ表示与平衡态的偏量，Y_lon为输出向量，所包含的变量依据控制需求而定。

利用信息融合控制算法，推导可得无人机飞行控制系统的纵向控制律为

$U_{\mathrm{lon}}\left(k\right)={\tilde{R}}_{\mathrm{lon}}(k+1){\tilde{x}}_{\mathrm{lon}}(k+1)-{\tilde{R}}_{\mathrm{lon}}(k+1)A_{\mathrm{lon}}{X}_{\mathrm{lon}}\left(k\right)---\left(17\right)$

式中，

${\tilde{R}}_{\mathrm{lon}}(k+1)={[R_{\mathrm{lon}}+{B_{\mathrm{lon}}}^T{P_{\mathrm{lon}}}^{-1}(k+1)B_{\mathrm{lon}}]}^{-1}{B_{\mathrm{lon}}}^T{P_{\mathrm{lon}}}^{-1}(k+1)---\left(18\right)$

$\left\{\begin{array}{c}{\hat{x}}_{\mathrm{lon}}\left(k\right)=P_{\mathrm{lon}}\left(k\right){C_{\mathrm{lon}}}^T{Q}_{\mathrm{lon}}{y_{\mathrm{lon}}}^{*}\left(k\right)+P_{\mathrm{lon}}\left(k\right){A_{\mathrm{lon}}}^T{[P_{\mathrm{lon}}(k+1)+B_{\mathrm{lon}}{R_{\mathrm{lon}}}^{-1}{B_{\mathrm{lon}}}^T]}^{-1}{\hat{x}}_{\mathrm{lon}}(k+1)\\ {\hat{x}}_{\mathrm{lon}}(k+N)=P_{\mathrm{lon}}\left(N\right){C_{\mathrm{lon}}}^T{Q}_{\mathrm{lon}}{y_{\mathrm{lon}}}^{*}\left(N\right)\end{array}\right.---\left(19\right)$

$\left\{\begin{array}{c}{P_{\mathrm{lon}}}^{-1}\left(k\right)={C_{\mathrm{lon}}}^T{Q}_{\mathrm{lon}}{C}_{\mathrm{lon}}+{A_{\mathrm{lon}}}^T{[P_{\mathrm{lon}}(k+1)+B_{\mathrm{lon}}{R_{\mathrm{lon}}}^{-1}{B_{\mathrm{lon}}}^T]}^{-1}{A}_{\mathrm{lon}}\\ {P_{\mathrm{lon}}}^{-1}(k+N)={C_{\mathrm{lon}}}^T{Q}_{\mathrm{lon}}{C}_{\mathrm{lon}}\end{array}\right.---\left(20\right)$

${\tilde{R}}_{\mathrm{lon}}={(R_{\mathrm{lon}}+{B_{\mathrm{lon}}}^T{P_{\mathrm{lon}}}^{-1}{B}_{\mathrm{lon}})}^{-1}{B_{\mathrm{lon}}}^T{P_{\mathrm{lon}}}^{-1}---\left(21\right).$

将无人机的横侧向线性状态方程与输出方程离散化为

$\left\{\begin{array}{c}{X}_{\mathrm{lat}}(k+1)=A_{\mathrm{lat}}{X}_{\mathrm{lat}}\left(k\right)+B_{\mathrm{lat}}{U}_{\mathrm{lat}}\left(k\right)\\ Y_{\mathrm{lat}}\left(k\right)=C_{\mathrm{lat}}{X}_{\mathrm{lat}}\left(k\right)\end{array}\right.---\left(22\right)$

式中，X_lat＝[Δβ,Δφ,Δp,Δr]^T，U_lat＝[Δδ_a,Δδ_r]^T，δ_a为副翼偏角，δ_r为升降舵偏角，A_lat、B_lat、C_lat为离散化后的系统矩阵，Y_lat为输出向量，所包含的变量依据控制需求而定。

利用信息融合控制算法，推导可得无人机飞行控制系统的横侧向控制律为

$U_{\mathrm{lat}}\left(k\right)={\tilde{R}}_{\mathrm{lat}}(k+1){\tilde{x}}_{\mathrm{lat}}(k+1)-{\tilde{R}}_{\mathrm{lat}}(k+1)A_{\mathrm{lat}}{X}_{\mathrm{lat}}\left(k\right)---\left(23\right)$

式中，

${\tilde{R}}_{\mathrm{lat}}(k+1)={[R_{\mathrm{lat}}+{B_{\mathrm{lat}}}^T{P_{\mathrm{lat}}}^{-1}(k+1)B_{\mathrm{lat}}]}^{-1}{B_{\mathrm{lat}}}^T{P_{\mathrm{lat}}}^{-1}(k+1)---\left(24\right)$

$\left\{\begin{array}{c}{\hat{x}}_{\mathrm{lat}}\left(k\right)=P_{\mathrm{lat}}\left(k\right){C_{\mathrm{lat}}}^T{Q}_{\mathrm{lat}}{y_{\mathrm{lat}}}^{*}\left(k\right)+P_{\mathrm{lat}}\left(k\right){A_{\mathrm{lat}}}^T{[P_{\mathrm{lat}}(k+1)+B_{\mathrm{lat}}{R_{\mathrm{lat}}}^{-1}{B_{\mathrm{lat}}}^T]}^{-1}{\hat{x}}_{\mathrm{lat}}(k+1)\\ {\hat{x}}_{\mathrm{lat}}(k+N)=P_{\mathrm{lat}}(k+N){C_{\mathrm{lat}}}^T{Q}_{\mathrm{lat}}{y_{\mathrm{lat}}}^{*}(k+N)\end{array}\right.---\left(25\right)$

$\left\{\begin{array}{c}{P_{\mathrm{lat}}}^{-1}\left(k\right)={C_{\mathrm{lat}}}^T{Q}_{\mathrm{lat}}{C}_{\mathrm{lat}}+{A_{\mathrm{lat}}}^T{[P_{\mathrm{lat}}(k+1)+B_{\mathrm{lat}}{R_{\mathrm{lat}}}^{-1}{B_{\mathrm{lat}}}^T]}^{-1}{A}_{\mathrm{lat}}\\ {P_{\mathrm{lat}}}^{-1}(k+N)={C_{\mathrm{lat}}}^T{Q}_{\mathrm{lat}}{C}_{\mathrm{lat}}\end{array}\right.---\left(26\right)$

${\tilde{R}}_{\mathrm{lat}}={(R_{\mathrm{lat}}+{B_{\mathrm{lat}}}^T{P_{\mathrm{lat}}}^{-1}{B}_{\mathrm{lat}})}^{-1}{B_{\mathrm{lat}}}^T{P_{\mathrm{lat}}}^{-1}---\left(27\right).$

针对某无人机进行了仿真验证。无人机模型为非线性全量方程描述。

参考轨迹指令为一条等高度圆弧轨迹。无人机初试位置要低于参考轨迹。需要利用轨迹跟踪控制系统，使无人机能够俯仰、爬升、滚转、偏航等运动，最终消除高度差和偏航距，从而实现精确轨迹跟踪。

选取的纵向控制参数为Q＝diag(10²,10^-1,10⁸,10^-1),R＝I；横侧向参数为Q＝diag(10^-2,10⁸,10³,10^-2),R＝I。通过数值仿真得到了飞行轨迹的跟踪响应结果，如图2和3所示。

从上述仿真结果看出，所提出的设计方法具有如下特点：

(1)在本发明的跟踪控制系统作用下，无人机能够很快实现爬升，消除高度误差，并保持航迹跟踪，总体轨迹跟踪精度较高。通过固定R参数，通过选取不同Q参数，可以获得不同程度的跟踪效果，能够达到很高的跟踪精度。

(2)本发明的跟踪控制系统能够充分利用传感器测量反馈得到的飞行状态信息，通过融合更多的有用信息，达到了很高的控制精度。

(3)所提出的跟踪控制系基于多变量控制理论，适用于飞行器这类复杂多变量系统。

本发明具体应用途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进，这些改进也应视为本发明的保护范围。

Claims (3)

1.一种无人机飞行轨迹跟踪控制系统，其特征在于，包括轨迹制导器、纵向控制器和侧向控制器；

所述轨迹制导器的纵向制导律为：

$V_g=\sqrt{{{\stackrel{.}{x}}_g}^2+{{\stackrel{.}{y}}_g}^2+{{\stackrel{.}{h}}_g}^2}---\left(1\right)$

${\mathrm{\α}}_g=\frac{\mathrm{mg}(1-\cos {\mathrm{\φ}}_g)}{{\mathrm{QSC}}_{\mathrm{L\α}}\cos {\mathrm{\φ}}_g}---\left(2\right)$

${\mathrm{\θ}}_g={\mathrm{\γ}}_g+{\mathrm{\α}}_g+k_h(\mathrm{\Δh}-{\mathrm{\Δh}}_g)+k_h\mathrm{\Δ}\stackrel{.}{h}=\arg \sin \left(\frac{{\stackrel{.}{h}}_g}{V_g}\right)+{\mathrm{\α}}_g+k_h(\mathrm{\Δh}-{\mathrm{\Δh}}_g)+k_h\mathrm{\Δ}\stackrel{.}{h}---\left(3\right)$

$q_g=\frac{g}{V_g}\cos {\mathrm{\θ}}_g\sin {\mathrm{\φ}}_g\tan {\mathrm{\φ}}_g---\left(4\right)$

所述轨迹制导器中的横侧向制导律为：

β_g＝0              (5)

${\mathrm{\φ}}_g=\arctan \frac{{\stackrel{.}{\mathrm{\ψ}}}_g{V}_g}{g}+k_{d}d+k_{\stackrel{.}{d}}\stackrel{.}{d},$ 其中

p_g＝0           (7)

$r_g=\frac{g}{V_g}\cos {\mathrm{\θ}}_g\sin {\mathrm{\φ}}_g---\left(8\right)$

所述纵向控制器的纵向控制律为：

$U_{\mathrm{lon}}\left(k\right)={\tilde{R}}_{\mathrm{lon}}(k+1){\hat{x}}_{\mathrm{lon}}(k+1)-{\tilde{R}}_{\mathrm{lon}}(k+1)A_{\mathrm{lon}}{X}_{\mathrm{lon}}\left(k\right)---\left(9\right)$

所述侧向控制器的横侧向控制律为：

$U_{\mathrm{lat}}\left(k\right)={\tilde{R}}_{\mathrm{lat}}(k+1){\hat{x}}_{\mathrm{lat}}(k+1)-{\tilde{R}}_{\mathrm{lat}}(k+1)A_{\mathrm{lat}}{X}_{\mathrm{lat}}\left(k\right)---\left(10\right).$

2.根据权利要求1所述的无人机飞行轨迹跟踪控制系统，其特征在于，所述纵向控制器的纵向控制律中，

${\tilde{R}}_{\mathrm{lon}}(k+1)={[R_{\mathrm{lon}}+{B_{\mathrm{lon}}}^T{P_{\mathrm{lon}}}^{-1}(k+1)B_{\mathrm{lon}}]}^{-1}{B_{\mathrm{lon}}}^T{P_{\mathrm{lon}}}^{-1}(k+1)---\left(11\right)$

$\left\{\begin{array}{c}{\hat{x}}_{\mathrm{lon}}\left(k\right)=P_{\mathrm{lon}}\left(k\right){C_{\mathrm{lon}}}^T{Q}_{\mathrm{lon}}{y_{\mathrm{lon}}}^{*}\left(k\right)+P_{\mathrm{lon}}\left(k\right){A_{\mathrm{lon}}}^T{[P_{\mathrm{lon}}(k+1)+B_{\mathrm{lon}}{R_{\mathrm{lon}}}^{-1}{B_{\mathrm{lon}}}^T]}^{-1}{\hat{x}}_{\mathrm{lon}}(k+1)\\ {\hat{x}}_{\mathrm{lon}}(k+N)=P_{\mathrm{lon}}\left(N\right){C_{\mathrm{lon}}}^T{Q}_{\mathrm{lon}}{y_{\mathrm{lon}}}^{*}\left(N\right)\end{array}\right.---\left(12\right)$

$\left\{\begin{array}{c}{P_{\mathrm{lon}}}^{-1}\left(k\right)={C_{\mathrm{lon}}}^T{Q}_{\mathrm{lon}}{C}_{\mathrm{lon}}+{A_{\mathrm{lon}}}^T[(P_{\mathrm{lon}}(k+1)+B_{\mathrm{lon}}{R_{\mathrm{lon}}}^{-1}{B_{\mathrm{lon}}}^T){]}^{-1}{A}_{\mathrm{lon}}\\ {P_{\mathrm{lon}}}^{-1}(k+N)={C_{\mathrm{lon}}}^T{Q}_{\mathrm{lon}}{C}_{\mathrm{lon}}\end{array}\right.---\left(13\right)$

式中，

${\tilde{R}}_{\mathrm{lon}}={(R_{\mathrm{lon}}+{B_{\mathrm{lon}}}^T{P_{\mathrm{lon}}}^{-1}{B}_{\mathrm{lon}})}^{-1}{B_{\mathrm{lon}}}^T{P_{\mathrm{lon}}}^{-1}---\left(14\right).$

3.根据权利要求1所述的无人机飞行轨迹跟踪控制系统，其特征在于，所述侧向控制器的横侧向控制律中，

${\tilde{R}}_{\mathrm{lat}}(k+1)={[R_{\mathrm{lat}}+{B_{\mathrm{lat}}}^T{P_{\mathrm{lat}}}^{-1}(k+1)B_{\mathrm{lat}}]}^{-1}{B_{\mathrm{lat}}}^T{P_{\mathrm{lat}}}^{-1}(k+1)---\left(15\right)$

$\left\{\begin{array}{c}{\hat{x}}_{\mathrm{lat}}\left(k\right)=P_{\mathrm{lat}}\left(k\right){C_{\mathrm{lat}}}^T{Q}_{\mathrm{lat}}{y_{\mathrm{lat}}}^{*}\left(k\right)+P_{\mathrm{lat}}\left(k\right){A_{\mathrm{lat}}}^T{[P_{\mathrm{lat}}(k+1)+B_{\mathrm{lat}}{R_{\mathrm{lat}}}^{-1}{B_{\mathrm{lat}}}^T]}^{-1}{\hat{x}}_{\mathrm{lat}}(k+1)\\ {\hat{x}}_{\mathrm{lat}}(k+N)=P_{\mathrm{lat}}(k+N){C_{\mathrm{lat}}}^T{Q}_{\mathrm{lat}}{y_{\mathrm{lat}}}^{*}(k+N)\end{array}\right.---\left(16\right)$

$\left\{\begin{array}{c}{P_{\mathrm{lat}}}^{-1}\left(k\right)={C_{\mathrm{lat}}}^T{Q}_{\mathrm{lat}}{C}_{\mathrm{lat}}+{A_{\mathrm{lat}}}^T{[P_{\mathrm{lat}}(k+1)+B_{\mathrm{lat}}{R_{\mathrm{lat}}}^{-1}{B_{\mathrm{lat}}}^T]}^{-1}{A}_{\mathrm{lat}}\\ {P_{\mathrm{lat}}}^{-1}(k+N)={C_{\mathrm{lat}}}^T{Q}_{\mathrm{lat}}{C}_{\mathrm{lat}}\end{array}\right.---\left(17\right)$

式中，

${\tilde{R}}_{\mathrm{lat}}={(R_{\mathrm{lat}}+{B_{\mathrm{lat}}}^T{P_{\mathrm{lat}}}^{-1}{B}_{\mathrm{lat}})}^{-1}{B_{\mathrm{lat}}}^T{P_{\mathrm{lat}}}^{-1}---\left(18\right).$