CN106125757A - 一种欠驱动飞艇航迹跟踪控制方法 - Google Patents

Abstract

针对飞艇的航迹跟踪控制问题，本发明提供一种欠驱动飞艇航迹跟踪控制方法。首先根据期望航迹和实际航迹计算位置误差量，然后采用串级设计方法解算期望速度，再根据期望速度和实际速度设计航迹控制律，计算航迹控制量。实际应用中，飞艇航迹由导航系统测量得到，将由该方法计算得到的控制量传输至执行机构即可实现航迹控制功能。由该方法控制的闭环系统能够高精度跟踪期望航迹，为欠驱动飞艇航迹跟踪控制的工程实现提供了有效方案。

Description

一种欠驱动飞艇航迹跟踪控制方法

技术领域

本发明属于自动控制技术领域，特别涉及一种航天航空领域的飞行控制方法，它为欠驱动飞艇提供一种航迹跟踪控制方法。

背景技术

飞艇是指一种依靠轻于空气的气体(如氦气、氢气等)产生静浮力升空，依靠自动飞行控制系统实现定点驻留和低速机动的飞行器，具有滞空时间长、能耗低、效费比高及定点驻留等优点，广泛应用于侦察监视、对地观测、环境监测、应急救灾、科学探测等领域，具有重要应用价值和广阔的应用前景，当前已成为航空领域的研究热点。

航迹跟踪是指飞艇从给定的初始状态出发并跟踪给定的期望航迹。飞艇的空间运动具有非线性、通道耦合、不确定、易受外界扰动等特点，因此，航迹控制成为飞艇飞行控制的关键技术之一。众多研究人员针对全驱动飞艇的航迹跟踪问题，提出了一系列航迹控制方法，为飞艇航迹控制提供了可供参考借鉴的技术方案。但是，工程实际中，飞艇的控制输入量一般少于其自由度数，为典型的欠驱动运动体。因此，上述控制方法未能有效解决欠驱动飞艇的航迹跟踪问题。

发明内容

为了解决欠驱动飞艇的航迹跟踪问题，本发明提供一种欠驱动飞艇航迹跟踪控制方法。本发明所提出的航迹控制器结构框图如图1所示。首先根据期望航迹和实际航迹计算位置误差量，然后采用串级设计方法解算期望速度，再根据期望速度和实际速度设计航迹控制律，计算航迹控制量。实际应用中，飞艇航迹由导航系统测量得到，将由该方法计算得到的控制量传输至执行机构即可实现航迹控制功能。由该方法控制的闭环系统能够高精度跟踪期望航迹，为欠驱动飞艇航迹跟踪控制的工程实现提供了有效方案。

一种欠驱动飞艇航迹跟踪控制方法，包括以下步骤：

S1：给定期望航迹：P_d＝[x_d,y_d,z_d]^T，其中，x_d、y_d和z_d分别为期望x坐标、期望y坐标和期望z坐标，上标T表示向量或矩阵的转置；

S2：计算期望航迹与实际航迹之间的航迹误差量P_e；

S3：根据航迹误差量P_e求解期望速度U_d；

S4：考虑飞艇的欠驱动特性，设计航迹跟踪控制律，计算航迹控制量τ。

本发明步骤S2中，航迹误差量P_e的计算方法如下：

P_e＝P-P_d＝[x-x_d,y-y_d,z-z_d]^T (1)

P＝[x,y,z]^T为实际航迹，x、y和z分别为x坐标、y坐标和z坐标。

本发明步骤S3中，期望速度U_d的求解方法为：

1)建立飞艇空间运动的数学模型

飞艇空间运动的坐标系及运动参数定义如下：

采用地面坐标系o_ex_ey_ez_e和体坐标系o_bx_by_bz_b对飞艇的空间运动进行描述，CV为浮心，CG为重心，浮心到重心的矢量为r_G＝[x_G,y_G,z_G]^T。

运动参数定义：实际航迹P＝[x,y,z]^T；姿态角Ω＝[θ,ψ,φ]^T，θ、ψ、φ分别为俯仰角、偏航角和滚转角；速度v＝[u,v,w]^T，u、v、w分别为体坐标系中轴向、侧向和垂直方向的速度；角速度ω＝[p,q,r]^T，p、q、r分别为滚转、俯仰和偏航角速度。

记广义坐标η＝[x,y,z,θ,ψ,φ]^T，广义速度为V＝[u,v,w,p,q,r]^T，飞艇在v、w和p自由度上存在欠驱动。

2)飞艇空间运动的数学模型描述如下：

$\stackrel{\·}{\mathrm{\η}}=J\left(\mathrm{\η}\right)=\left[\begin{array}{cc}{J}_1& 0_{3\×3}\\ 0_{3\×3}& J_2\end{array}\right]V---\left(2\right)$

$M\stackrel{\·}{V}=\stackrel{\‾}{N}+\stackrel{\‾}{G}+\mathrm{\τ}---\left(3\right)$

式中

$J_1=\left[\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}-\sin \mathrm{\ψ}\cos \mathrm{\φ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\φ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}+\cos \mathrm{\ψ}\cos \mathrm{\φ}& \sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}-\cos \mathrm{\ψ}\sin \mathrm{\φ}\\ \sin \mathrm{\θ}& \cos \mathrm{\θ}\sin \mathrm{\φ}& \cos \mathrm{\θ}\cos \mathrm{\φ}\end{array}\right]---\left(4\right)$

$J_2=\left[\begin{array}{ccc}0& cos\mathrm{\φ}& -sin\mathrm{\φ}\\ 0& \sec \mathrm{\θ}\sin \mathrm{\φ}& \sec \mathrm{\θ}cos\mathrm{\φ}\\ 1& tan\mathrm{\θ}\sin \mathrm{\φ}& tan\mathrm{\θ}cos\mathrm{\φ}\end{array}\right]---\left(5\right)$

$M=\left[\begin{array}{cccccc}m+m_{11}& 0& 0& 0& {\mathrm{mz}}_G& -{\mathrm{my}}_G\\ 0& m+m_{22}& 0& -{\mathrm{mz}}_G& 0& {\mathrm{mx}}_G\\ 0& 0& m+m_{33}& {\mathrm{my}}_G& -{\mathrm{mx}}_G& 0\\ 0& -{\mathrm{mz}}_G& {\mathrm{my}}_G& I_x+I_{11}& 0& 0\\ {\mathrm{mz}}_G& 0& -{\mathrm{mx}}_G& 0& I_x+I_{22}& 0\\ 0& {\mathrm{mx}}_G& 0& -I_{xz}& 0& I_x+I_{33}\end{array}\right]---\left(6\right)$

$\stackrel{\‾}{G}=\left[\begin{array}{c}(B-G)sin\mathrm{\θ}\\ (G-B)cos\mathrm{\θ}\sin \mathrm{\φ}\\ (G-B)cos\mathrm{\θ}\cos \mathrm{\φ}\\ y_{G}G\cos \mathrm{\θ}cos\mathrm{\φ}-z_{G}Gcos\mathrm{\θ}sin\mathrm{\φ}\\ -x_{G}G\cos \mathrm{\θ}\cos \mathrm{\φ}-z_{G}Gsin\mathrm{\θ}\\ x_{G}G\cos \mathrm{\θ}\sin \mathrm{\φ}+y_{G}G\sin \mathrm{\θ}\end{array}\right]---\left(7\right)$

$\mathrm{\τ}=\left[\begin{array}{c}{\mathrm{\τ}}_u\\ 0\\ 0\\ 0\\ {\mathrm{\τ}}_m\\ {\mathrm{\τ}}_n\end{array}\right]=\left[\begin{array}{c}Tcos\mathrm{\μ}cos\mathrm{\υ}\\ 0\\ 0\\ 0\\ Tcos\mathrm{\μ}cos\mathrm{\υ}{l}_z-Tcos\mathrm{\μ}sin\mathrm{\υ}{l}_x\\ {\mathrm{Tsin\μl}}_x-{\mathrm{Tcos\μcos\υl}}_y\end{array}\right]---\left(8\right)$

$\stackrel{\‾}{N}={\[N_u,N_v,N_w,N_p,N_q,N_r\]}^T---\left(9\right)$

其中

$\begin{array}{c}{N}_u=(m+m_{22})vr-(m+m_{33})wq+m\[x_G(p^2+r^2)-y_{G}pq-z_{G}pr\]\\ +{\mathrm{QV}}^{2/3}(-C_X\cos \mathrm{\α}\cos \mathrm{\β}+C_Y\cos \mathrm{\α}\sin \mathrm{\β}+C_Z\sin \mathrm{\α})\end{array}---\left(10\right)$

$\begin{array}{c}{N}_v=(m+m_{33})wp-(m+m_{11})ur-m\[x_{G}pq-y_G(p^2+r^2)+z_{G}qr\]\\ +{\mathrm{QV}}^{2/3}(C_X\sin \mathrm{\β}+C_Y\cos \mathrm{\β})\end{array}---\left(11\right)$

$\begin{array}{c}{N}_w=(m+m_{22})vp-(m+m_{11})uq-m\[x_{G}pr+y_{G}qr-z_G(p^2+q^2)\]\\ +{\mathrm{QV}}^{2/3}(-C_X\sin \mathrm{\α}\sin \mathrm{\β}+C_Y\sin \mathrm{\α}\cos \mathrm{\β}-C_Z\cos \mathrm{\α})\end{array}---\left(12\right)$

$\begin{array}{c}{N}_p=\[(I_y+I_{22})-(I_z+I_{33})\]qr+I_{xz}pq-I_{xy}pr-I_{yz}(r^2-q^2)+\\ \[{\mathrm{mz}}_G(ur-wp)+y_G(uq-vp)\]+{\mathrm{QVC}}_l\end{array}---\left(13\right)$

$\begin{array}{c}{N}_q=\[(I_z+I_{33})-(I_x+I_{11})\]pr+I_{xy}qr-I_{yz}pq-I_{xz}(p^2-r^2)\\ +m\[x_G(vp-uq)-z_G(wq-vr)\]+{\mathrm{QVC}}_m\end{array}---\left(14\right)$

$\begin{array}{c}{N}_r=\[(I_y+I_{22})-(I_x+I_{11})\]pq+I_{xz}qr-I_{xy}(p^2-p^2)+I_{yz}pr\\ +m\[y_G(wq-uvrq)-x_G(ur-wp)\]+{\mathrm{QVC}}_n\end{array}---\left(15\right)$

式中，m为飞艇质量，m₁₁、m₂₂、m₃₃为附加质量,I₁₁、I₂₂、I₃₃为附加惯量；Q为动压，α为迎角，β为侧滑角，C_X、C_Y、C_Z、C_l、C_m、C_n为气动系数；I_x、I_y、I_z分别为绕o_bx_b、o_by_b、o_bz_b的主惯量；I_xy、I_xz、I_yz分别为关于平面o_bx_by_b、o_bx_bz_b、o_by_bz_b的惯量积；T为推力大小，μ为推力矢量与o_bx_bz_b面之间的夹角，规定其在o_bx_bz_b面之左为正，υ为推力矢量在o_bx_bz_b面的投影与o_bx_b轴之间的夹角，规定其投影在o_bx_b轴之下为正；l_x、l_y、l_z表示推力作用点距原点o_b的距离。

3)解算期望速度U_d

将航迹误差量P_e变换为体坐标系中的误差

ξ_e＝J^-1P_e (16)

式中，J^-1为J的逆矩阵。

定义

ζ_e＝ξ_e-λ (17)

式中，λ＝[ρ,0,0]^T，ρ为一个正实数；

对式(17)求导，可得：

$\begin{array}{c}{\stackrel{\·}{\mathrm{\ζ}}}_e=-\mathrm{\ω}\×{\mathrm{\ζ}}_e-\mathrm{\ω}\×{\mathrm{\λ\ζ}}_e+v-J^{-1}{\stackrel{\·}{P}}_d\\ =\left[\begin{array}{ccc}1& 0& 0\\ 0& 0& -\mathrm{\ρ}\\ 0& \mathrm{\ρ}& 0\end{array}\right]\left[\begin{array}{c}u\\ q\\ r\end{array}\right]+\left[\begin{array}{c}0\\ v\\ w\end{array}\right]-\mathrm{\ω}\×{\mathrm{\ζ}}_e-J^{-1}-{\stackrel{\·}{P}}_d\end{array}---\left(18\right)$

考虑到飞艇在v、w和p自由度上存在欠驱动，定义期望速度与实际速度之间的误差为：

U_e＝U-U_d (19)

式中，U_e＝[u_e,q_e,r_e]^T，U＝[u,q,r]^T，U_d＝[u_d,q_d,r_d]^T；

由式(18)可得：

$U_d=\left[\begin{array}{c}{u}_d\\ q_d\\ r_d\end{array}\right]={\left[\begin{array}{ccc}1& 0& 0\\ 0& 0& -\mathrm{\ρ}\\ 0& \mathrm{\ρ}& 0\end{array}\right]}^{-1}{(-\mathrm{\γ}\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]{\mathrm{\ζ}}_e-\left[\begin{array}{c}0\\ v\\ w\end{array}\right])}_e+J^{-1}{\stackrel{\·}{P}}_d---\left(20\right)$

式中，γ为正实数。

本发明步骤S4的方法为：

定义以下误差量

V_e＝V-V_d (21)

式中，V＝[u,0,0,0,q,r]^T，V_d＝＝[u_d,0,0,0,q_d,r_d]^T。

对式(21)微分可得：

${\stackrel{\·}{V}}_e=\stackrel{\·}{V}-{\stackrel{\·}{V}}_d---\left(21\right)$

等号左右两侧同乘M，可得：

$M{\stackrel{\·}{V}}_e=M\stackrel{\·}{V}-M{\stackrel{\·}{V}}_d---\left(22\right)$

将式(3)代入式(22)，可得

$M{\stackrel{\·}{V}}_e=M\stackrel{\·}{V}-M{\stackrel{\·}{V}}_d=\stackrel{\‾}{N}+\stackrel{\‾}{G}+\mathrm{\τ}-M{\stackrel{\·}{V}}_d---\left(23\right)$

根据式(23)可设计如下控制律：

$\mathrm{\τ}=M{\stackrel{\·}{V}}_d+K{\stackrel{\·}{V}}_e-\stackrel{\‾}{N}-\stackrel{\‾}{G}---\left(24\right)$

式中，K＝diag(k₁,0,0,0,k₂,k₃)，diag()表示对角矩阵。

本发明针对飞艇的三维航迹跟踪问题，建立了飞艇的非线性动力学模型；以此为受控对象，考虑飞艇的欠驱动特性，采用串级设计方法，将非线性动力学模型分解为两个子系统，先根据位置误差解算出期望速度，再根据期望速度设计控制输入量。该方法的主要优点有：

1)该方法考虑了飞艇的欠驱动特性，能够解决执行机构不足或执行机构发生故障情况下的航迹跟踪问题，提高了航迹跟踪控制系统的适应性。

2)采用串级设计方法设计航迹控制律，将非线性动力学模型分解为两个子系统，先根据位置误差解算出期望速度，再根据期望速度设计控制输入量，由此简化了控制律设计难度。

控制工程师在应用过程中可以根据实际飞行任务给定任意期望航迹，并将由该方法得到的控制量传输至执行机构实现航迹跟踪控制功能。

附图说明

图1为本发明所述飞艇航迹控制器结构图；

图2为本发明所述飞艇航迹跟踪控制方法步骤流程图；

图3为本发明所述飞艇坐标系及运动参数定义；

图4为本发明所述飞艇航迹跟踪控制结果；

图5为本发明所述飞艇航迹跟踪控制误差。

图中符号说明如下：

P_d P_d＝[x_d,y_d,z_d]^T为飞艇的期望航迹；

P P＝[x,y,z]^T为飞艇的实际航迹；

P_e P_e为飞艇的航迹误差；

U_d U_d为期望速度；

o_ex_ey_ez_e o_ex_ey_ez_e表示地面坐标系；

o_bx_by_bz_b o_bx_by_bz_b表示飞艇体坐标系；

x x为实际航迹的x坐标；

y y为实际航迹的y坐标；

z z为实际航迹的z坐标；

θ θ为俯仰角；

ψ ψ为偏航角；

φ φ为滚转角；

τ τ为控制量；

u u为体坐标系中的轴向速度；

v v为体坐标系中的侧向速度；

w w为体坐标系中的垂直方向的速度；

p p为滚转角速度；

q q俯仰角速度；

r r为偏航角速度。

具体实施方式

下面结合具体实施例，对本发明作进一步的说明：

一种欠驱动飞艇航迹跟踪控制方法，其具体步骤如下：

步骤一：给定期望航迹

P_d＝[40cos(0.02πt)m,40sin(0.02πt)m,(0.05t)m]^T

步骤二：航迹误差量计算

计算期望航迹与实际航迹之间的误差量：

P_e＝P-P_d＝[x-x_d,y-y_d,z-z_d]^T (1)

其中，P＝[x,y,z]^T为实际航迹，x、y和z分别为x坐标、y坐标和z坐标，为连续变化值。

初始航迹为：

P₀＝[x₀,y₀,z₀]^T＝[10m,20m,0.02m]^T。

步骤三：期望速度解算

1)建立飞艇空间运动的数学模型

为便于描述，飞艇空间运动的坐标系及运动参数定义如下。如图3所示，采用地面坐标系o_ex_ey_ez_e和体坐标系o_bx_by_bz_b对飞艇的空间运动进行描述，CV为浮心，CG为重心，浮心到重心的矢量为r_G＝[x_G,y_G,z_G]^T。运动参数定义：实际航迹P＝[x,y,z]^T，x、y、z分别为为x坐标、y坐标和z坐标；姿态角Ω＝[θ,ψ,φ]^T，θ、ψ、φ分别为俯仰角、偏航角和滚转角；速度v＝[u,v,w]^T，u、v、w分别为体坐标系中轴向、侧向和垂直方向的速度；角速度ω＝[p,q,r]^T，p、q、r分别为滚转、俯仰和偏航角速度。记广义坐标η＝[x,y,z,θ,ψ,φ]^T，广义速度为V＝[u,v,w,p,q,r]^T。飞艇在v、w和p自由度上存在欠驱动。

飞艇空间运动的数学模型描述如下：

$\stackrel{\·}{\mathrm{\η}}=J\left(\mathrm{\η}\right)=\left[\begin{array}{cc}{J}_1& 0_{3\×3}\\ 0_{3\×3}& J_2\end{array}\right]V---\left(2\right)$

$M\stackrel{\·}{V}=\stackrel{\‾}{N}+\stackrel{\‾}{G}+\mathrm{\τ}---\left(3\right)$

式中

$J_1=\left[\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}-\sin \mathrm{\ψ}\cos \mathrm{\φ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\φ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}+\cos \mathrm{\ψ}\cos \mathrm{\φ}& \sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}-\cos \mathrm{\ψ}\sin \mathrm{\φ}\\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}\sin \mathrm{\φ}& \cos \mathrm{\θ}\cos \mathrm{\φ}\end{array}\right]---\left(4\right)$

$J_2=\left[\begin{array}{ccc}0& cos\mathrm{\φ}& -sin\mathrm{\φ}\\ 0& \sec \mathrm{\θ}\sin \mathrm{\φ}& \sec \mathrm{\θ}cos\mathrm{\φ}\\ 1& tan\mathrm{\θ}\sin \mathrm{\φ}& tan\mathrm{\θ}cos\mathrm{\φ}\end{array}\right]---\left(5\right)$

$M=\left[\begin{array}{cccccc}m+m_{11}& 0& 0& 0& {\mathrm{mz}}_G& -{\mathrm{my}}_G\\ 0& m+m_{22}& 0& -{\mathrm{mz}}_G& 0& {\mathrm{mx}}_G\\ 0& 0& m+m_{33}& {\mathrm{my}}_G& -{\mathrm{mx}}_G& 0\\ 0& -{\mathrm{mz}}_G& {\mathrm{my}}_G& I_x+I_{11}& 0& 0\\ {\mathrm{mz}}_G& 0& -{\mathrm{mx}}_G& 0& I_x+I_{22}& 0\\ 0& {\mathrm{mx}}_G& 0& -I_{xz}& 0& I_x+I_{33}\end{array}\right]---\left(6\right)$

$\stackrel{\‾}{G}=\left[\begin{array}{c}(B-G)sin\mathrm{\θ}\\ (G-B)cos\mathrm{\θ}\sin \mathrm{\φ}\\ (G-B)cos\mathrm{\θ}\cos \mathrm{\φ}\\ y_{G}G\cos \mathrm{\θ}cos\mathrm{\φ}-z_{G}Gcos\mathrm{\θ}sin\mathrm{\φ}\\ -x_{G}G\cos \mathrm{\θ}\cos \mathrm{\φ}-z_{G}Gsin\mathrm{\θ}\\ x_{G}G\cos \mathrm{\θ}\sin \mathrm{\φ}+y_{G}G\sin \mathrm{\θ}\end{array}\right]---\left(7\right)$

$\mathrm{\τ}=\left[\begin{array}{c}{\mathrm{\τ}}_u\\ 0\\ 0\\ 0\\ {\mathrm{\τ}}_m\\ {\mathrm{\τ}}_n\end{array}\right]=\left[\begin{array}{c}Tcos\mathrm{\μ}cos\mathrm{\υ}\\ 0\\ 0\\ 0\\ Tcos\mathrm{\μ}cos\mathrm{\υ}{l}_z-Tcos\mathrm{\μ}sin\mathrm{\υ}{l}_x\\ {\mathrm{Tsin\μl}}_x-{\mathrm{Tcos\μcos\υl}}_y\end{array}\right]---\left(8\right)$

$\stackrel{\‾}{N}={\[N_u,N_v,N_w,N_p,N_q,N_r\]}^T---\left(9\right)$

其中

$\begin{array}{c}{N}_u=(m+m_{22})vr-(m+m_{33})wq+m\[x_G(p^2+r^2)-y_{G}pq-z_{G}pr\]\\ +{\mathrm{QV}}^{2/3}(-C_X\cos \mathrm{\α}\cos \mathrm{\β}+C_Y\cos \mathrm{\α}\sin \mathrm{\β}+C_Z\sin \mathrm{\α})\end{array}---\left(10\right)$

$\begin{array}{c}{N}_v=(m+m_{33})wp-(m+m_{11})ur-m\[x_{G}pq-y_G(p^2+r^2)+z_{G}qr\]\\ +{\mathrm{QV}}^{2/3}(C_X\sin \mathrm{\β}+C_Y\cos \mathrm{\β})\end{array}---\left(11\right)$

$\begin{array}{c}{N}_w=(m+m_{22})vp-(m+m_{11})uq-m\[x_{G}pr+y_{G}qr-z_G(p^2+q^2)\]\\ +{\mathrm{QV}}^{2/3}(-C_X\sin \mathrm{\α}\sin \mathrm{\β}+C_Y\sin \mathrm{\α}\cos \mathrm{\β}-C_Z\cos \mathrm{\α})\end{array}---\left(12\right)$

$\begin{array}{c}{N}_p=\[(I_y+I_{22})-(I_z+I_{33})\]qr+I_{xz}pq-I_{xy}pr-I_{yz}(r^2-q^2)+\\ \[{\mathrm{mz}}_G(ur-wp)+y_G(uq-vp)\]+{\mathrm{QVC}}_l\end{array}---\left(13\right)$

$\begin{array}{c}{N}_q=\[(I_z+I_{33})-(I_x+I_{11})\]pr+I_{xy}qr-I_{yz}pq-I_{xz}(p^2-r^2)\\ +m\[x_G(vp-uq)-z_G(wq-vr)\]+{\mathrm{QVC}}_m\end{array}---\left(14\right)$

$\begin{array}{c}{N}_r=\[(I_y+I_{22})-(I_x+I_{11})\]pq-I_{xz}qr-I_{xy}(p^2-p^2)+I_{yz}pr\\ +m\[y_G(wq-vr)-x_G(ur-wp)\]+{\mathrm{QVC}}_n\end{array}---\left(15\right)$

式中，m为飞艇质量，m₁₁、m₂₂、m₃₃为附加质量,I₁₁、I₂₂、I₃₃为附加惯量；Q为动压，α为迎角，β为侧滑角，C_X、C_Y、C_Z、C_l、C_m、C_n为气动系数；I_x、I_y、I_z分别为绕o_bx_b、o_by_b、o_bz_b的主惯量；I_xy、I_xz、I_yz分别为关于平面o_bx_by_b、o_bx_bz_b、o_by_bz_b的惯量积；T为推力大小，μ为推力矢量与o_bx_bz_b面之间的夹角，规定其在o_bx_bz_b面之左为正，υ为推力矢量在o_bx_bz_b面的投影与o_bx_b轴之间的夹角，规定其投影在o_bx_b轴之下为正；l_x、l_y、l_z表示推力作用点距原点o_b的距离。

本实施例中的飞艇参数见表1。

表1 飞艇参数

2)解算期望速度U_d

将航迹误差P_e变换为体坐标系中的误差

ξ_e＝J^-1P_e (16)

式中，J^-1为J的逆矩阵。

定义

ζ_e＝ξ_e-λ (17)

式中，λ＝[0.5,0,0]^T。

对式(17)求导，可得：

$\begin{array}{c}{\stackrel{\·}{\mathrm{\ζ}}}_e=-\mathrm{\ω}\×{\mathrm{\ζ}}_e-\mathrm{\ω}\×{\mathrm{\λ\ζ}}_e+v-J^{-1}{\stackrel{\·}{P}}_d\\ =\left[\begin{array}{ccc}1& 0& 0\\ 0& 0& -\mathrm{\ρ}\\ 0& \mathrm{\ρ}& 0\end{array}\right]\left[\begin{array}{c}u\\ q\\ r\end{array}\right]+\left[\begin{array}{c}0\\ v\\ w\end{array}\right]-\mathrm{\ω}\×{\mathrm{\ζ}}_e-J^{-1}{\stackrel{\·}{P}}_d\end{array}---\left(18\right)$

考虑到飞艇在v、w和p自由度上存在欠驱动，定义期望速度与实际速度之间的速度误差为：

U_e＝U-U_d (19)

式中，U_e＝[u_e,q_e,r_e]^T，U＝[u,q,r]^T，U_d＝[u_d,q_d,r_d]^T。

由式(18)可得：

$U_d=\left[\begin{array}{c}{u}_d\\ q_d\\ r_d\end{array}\right]={\left[\begin{array}{ccc}1& 0& 0\\ 0& 0& -\mathrm{\ρ}\\ 0& \mathrm{\ρ}& 0\end{array}\right]}^{-1}{(-\mathrm{\γ}\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]{\mathrm{\ζ}}_e-\left[\begin{array}{c}0\\ v\\ w\end{array}\right])}_e+J^{-1}{\stackrel{\·}{P}}_d---\left(20\right)$

式中，γ＝2。

步骤四：航迹跟踪控制律设计

定义以下误差量

V_e＝V-V_d (21)

式中，V＝[u,0,0,0,q,r]^T，V_d＝＝[u_d,0,0,0,q_d,r_d]^T。

对式(21)微分可得：

${\stackrel{\·}{V}}_e=\stackrel{\·}{V}-{\stackrel{\·}{V}}_d---\left(21\right)$

等号左右两侧同乘M，可得：

$M{\stackrel{\·}{V}}_e=M\stackrel{\·}{V}-M{\stackrel{\·}{V}}_d---\left(22\right)$

将式(3)代入式(22)，可得

$M{\stackrel{\·}{V}}_e=M\stackrel{\·}{V}-M{\stackrel{\·}{V}}_d=\stackrel{\‾}{N}+\stackrel{\‾}{G}+\mathrm{\τ}-M{\stackrel{\·}{V}}_d---\left(23\right)$

根据式(23)可设计如下控制律：

$\mathrm{\τ}=M{\stackrel{\·}{V}}_d+K{\stackrel{\·}{V}}_e-\stackrel{\‾}{N}-\stackrel{\‾}{G}---\left(24\right)$

式中，K＝diag(5,0,0,0,8,8)，diag()表示对角矩阵。

实施例中的欠驱动飞艇航迹跟踪控制结果如图4、图5所示。图4给出了飞艇航迹跟踪控制结果，由图4可得：飞艇由初始位置出发，能够准确地跟踪期望航迹，验证了本发明所提出的航迹跟踪控制方法的有效性；图5给出了航迹跟踪控制误差，由图5可得，本发明所提出的航迹控制方法能够高精度地跟踪给定的期望航迹。

Claims (4)

1.一种欠驱动飞艇航迹跟踪控制方法，其特征在于，包括以下步骤：

S1：给定期望航迹：P_d＝[x_d,y_d,z_d]^T，其中，x_d、y_d和z_d分别为期望x坐标、期望y坐标和期望z坐标，上标T表示向量或矩阵的转置；

S2：计算期望航迹与实际航迹之间的航迹误差量P_e；

S3：根据航迹误差量P_e求解期望速度U_d；

S4：考虑飞艇的欠驱动特性，设计航迹跟踪控制律，计算航迹控制量τ。

2.根据权利要求1所述的欠驱动飞艇航迹跟踪控制方法，其特征在于，步骤S2中，航迹误差量P_e的计算方法如下：

P_e＝P-P_d＝[x-x_d,y-y_d,z-z_d]^T (1)

P＝[x,y,z]^T为实际航迹，x、y和z分别为x坐标、y坐标和z坐标。

3.根据权利要求1所述的欠驱动飞艇航迹跟踪控制方法，其特征在于，步骤S3中期望速度U_d的求解方法为：

1)建立飞艇空间运动的数学模型

飞艇空间运动的坐标系及运动参数定义如下：

采用地面坐标系o_ex_ey_ez_e和体坐标系o_bx_by_bz_b对飞艇的空间运动进行描述，CV为浮心，CG为重心，浮心到重心的矢量为r_G＝[x_G,y_G,z_G]^T；

运动参数定义：实际航迹P＝[x,y,z]^T；姿态角Ω＝[θ,ψ,φ]^T，θ、ψ、φ分别为俯仰角、偏航角和滚转角；速度v＝[u,v,w]^T，u、v、w分别为体坐标系中轴向、侧向和垂直方向的速度；角速度ω＝[p,q,r]^T，p、q、r分别为滚转、俯仰和偏航角速度；

记广义坐标η＝[x,y,z,θ,ψ,φ]^T，广义速度为V＝[u,v,w,p,q,r]^T，飞艇在v、w和p自由度上存在欠驱动；

2)飞艇空间运动的数学模型描述如下：

$\stackrel{\·}{\mathrm{\η}}=J\left(\mathrm{\η}\right)=\left[\begin{array}{cc}{J}_1& 0_{3\×3}\\ 0_{3\×3}& J_2\end{array}\right]V---\left(2\right)$

$M\stackrel{\·}{V}=\stackrel{\‾}{N}+\stackrel{\‾}{G}+\mathrm{\τ}---\left(3\right)$

式中

$J_1=\left[\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}-\sin \mathrm{\ψ}\cos \mathrm{\φ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\φ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}+\cos \mathrm{\ψ}\cos \mathrm{\φ}& \sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}-\cos \mathrm{\ψ}\sin \mathrm{\φ}\\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}\sin \mathrm{\φ}& \cos \mathrm{\θ}\cos \mathrm{\φ}\end{array}\right]---\left(4\right)$

$J_2=\left[\begin{array}{ccc}0& cos\mathrm{\φ}& -\sin \mathrm{\φ}\\ 0& \sec \mathrm{\θ}\sin \mathrm{\φ}& \sec \mathrm{\θ}cos\mathrm{\φ}\\ 1& tan\mathrm{\θ}\sin \mathrm{\φ}& tan\mathrm{\θ}cos\mathrm{\φ}\end{array}\right]---\left(5\right)$

$M=\left[\begin{array}{cccccc}m+m_{11}& 0& 0& 0& {\mathrm{mz}}_G& -{\mathrm{my}}_G\\ 0& m+m_{22}& 0& -{\mathrm{mz}}_G& 0& {\mathrm{mx}}_G\\ 0& 0& m+m_{33}& {\mathrm{my}}_G& -{\mathrm{mx}}_G& 0\\ 0& -{\mathrm{mz}}_G& {\mathrm{my}}_G& I_x+I_{11}& 0& 0\\ {\mathrm{mz}}_G& 0& -{\mathrm{mx}}_G& 0& I_x+I_{22}& 0\\ 0& {\mathrm{mx}}_G& 0& -I_{xz}& 0& I_x+I_{33}\end{array}\right]---\left(6\right)$

$\stackrel{\‾}{G}=\left[\begin{array}{c}(B-G)sin\mathrm{\θ}\\ (G-B)cos\mathrm{\θ}\sin \mathrm{\φ}\\ (G-B)cos\mathrm{\θ}\cos \mathrm{\φ}\\ y_{G}G\cos \mathrm{\θ}cos\mathrm{\φ}-z_{G}Gcos\mathrm{\θ}sin\mathrm{\φ}\\ -x_{G}G\cos \mathrm{\θ}\cos \mathrm{\φ}-z_{G}Gsin\mathrm{\θ}\\ x_{G}G\cos \mathrm{\θ}\sin \mathrm{\φ}+y_{G}G\sin \mathrm{\θ}\end{array}\right]---\left(7\right)$

$\mathrm{\τ}=\left[\begin{array}{c}{\mathrm{\τ}}_u\\ 0\\ 0\\ 0\\ {\mathrm{\τ}}_m\\ {\mathrm{\τ}}_n\end{array}\right]=\left[\begin{array}{c}Tcos\mathrm{\μ}cos\mathrm{\υ}\\ 0\\ 0\\ 0\\ T{\mathrm{cos\μcos\υl}}_z-T{\mathrm{cos\μsin\υl}}_x\\ T{\mathrm{sin\μl}}_x-T{\mathrm{cos\μcos\υl}}_y\end{array}\right]---\left(8\right)$

$\stackrel{\‾}{N}={\[N_u,N_v,N_w,N_p,N_q,N_r\]}^T---\left(9\right)$

其中

$\begin{array}{c}{N}_u=(m+m_{22})vr-(m+m_{33})wq+m\[x_G(p^2+r^2)-y_{G}pq-z_{G}pr\]\\ +{\mathrm{QV}}^{2/3}(-C_X\cos \mathrm{\α}\cos \mathrm{\β}+C_Y\cos \mathrm{\α}\sin \mathrm{\β}+C_Z\sin \mathrm{\α})\end{array}---\left(10\right)$

$\begin{array}{c}{N}_v=(m+m_{33})wp-(m+m_{11})ur-m\[x_{G}pq-y_G(p^2+r^2)+z_{G}qr\]\\ +{\mathrm{QV}}^{2/3}(C_X\sin \mathrm{\β}+C_Y\cos \mathrm{\β})\end{array}---\left(11\right)$

$\begin{array}{c}{N}_w=(m+m_{22})vp-(m+m_{11})uq-m\[x_{G}pr+y_{G}qr-z_G(p^2+q^2)\]\\ +{\mathrm{QV}}^{2/3}(-C_X\sin \mathrm{\α}\sin \mathrm{\β}+C_Y\sin \mathrm{\α}\cos \mathrm{\β}-C_Z\cos \mathrm{\α})\end{array}---\left(12\right)$

$\begin{array}{c}{N}_p=\[(I_y+I_{22})-(I_z+I_{33})\]qr+I_{xz}pq-I_{xy}pr-I_{yz}(r^2-q^2)+\\ \[{\mathrm{mz}}_G(ur-wp)+y_G(uq-vp)\]+{\mathrm{QVC}}_l\end{array}---\left(13\right)$

$\begin{array}{c}{N}_q=\[(I_z+I_{33})-(I_x+I_{11})\]pr+I_{xy}qr-I_{yz}pq-I_{xz}(p^2-r^2)\\ +m\[x_G(vp-uq)-z_G(wp-vr)\]+{\mathrm{QVC}}_m\end{array}---\left(14\right)$

$\begin{array}{c}{N}_r=\[(I_y+I_{22})-(I_x+I_{11})\]pq-I_{xz}qr-I_{xy}(q^2-p^2)+I_{yz}pr\\ +m\[y_G(wq-vr)-x_G(ur-wp)\]+{\mathrm{QVC}}_n\end{array}---\left(15\right)$

式中，m为飞艇质量，m₁₁、m₂₂、m₃₃为附加质量,I₁₁、I₂₂、I₃₃为附加惯量；Q为动压，α为迎角，β为侧滑角，C_X、C_Y、C_Z、C_l、C_m、C_n为气动系数；I_x、I_y、I_z分别为绕o_bx_b、o_by_b、o_bz_b的主惯量；I_xy、I_xz、I_yz分别为关于平面o_bx_by_b、o_bx_bz_b、o_by_bz_b的惯量积；T为推力大小，μ为推力矢量与o_bx_bz_b面之间的夹角，规定其在o_bx_bz_b面之左为正，υ为推力矢量在o_bx_bz_b面的投影与o_bx_b轴之间的夹角，规定其投影在o_bx_b轴之下为正；l_x、l_y、l_z表示推力作用点距原点o_b的距离；

3)解算期望速度U_d

将航迹误差量P_e变换为体坐标系中的误差

ξ_e＝J^-1P_e (16)

式中，J^-1为J的逆矩阵；

定义

ζ_e＝ξ_e-λ (17)

式中，λ＝[ρ,0,0]^T，ρ为一个正实数；

对式(17)求导，可得：

$\begin{array}{c}{\stackrel{\·}{\mathrm{\ζ}}}_e=-\mathrm{\ω}\×{\mathrm{\ζ}}_e-\mathrm{\ω}\×{\mathrm{\λ\ζ}}_e+v-J^{-1}{\stackrel{\·}{P}}_d\\ =\left[\begin{array}{ccc}1& 0& 0\\ 0& 0& -\mathrm{\ρ}\\ 0& \mathrm{\ρ}& 0\end{array}\right]\left[\begin{array}{c}u\\ q\\ r\end{array}\right]+\left[\begin{array}{c}0\\ v\\ w\end{array}\right]-\mathrm{\ω}\×{\mathrm{\ζ}}_e-J^{-1}{\stackrel{\·}{P}}_d\end{array}---\left(18\right)$

考虑到飞艇在v、w和p自由度上存在欠驱动，定义期望速度与实际速度之间的误差为：

U_e＝U-U_d (19)

式中，U_e＝[u_e,q_e,r_e]^T，U＝[u,q,r]^T，U_d＝[u_d,q_d,r_d]^T；

由式(18)可得：

$U_d=\left[\begin{array}{c}{u}_d\\ q_d\\ r_d\end{array}\right]={\left[\begin{array}{ccc}1& 0& 0\\ 0& 0& -\mathrm{\ρ}\\ 0& \mathrm{\ρ}& 0\end{array}\right]}^{-1}{\left(-\mathrm{\γ}\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]{\mathrm{\ζ}}_e-\left[\begin{array}{c}0\\ v\\ w\end{array}\right]\right)}_e+J^{-1}{\stackrel{\·}{P}}_d---\left(20\right)$

式中，γ为正实数。

4.根据权利要求1所述的欠驱动飞艇航迹跟踪控制方法，其特征在于，步骤S4的方法为：

定义以下误差量

V_e＝V-V_d (21)

式中，V＝[u,0,0,0,q,r]^T，V_d＝＝[u_d,0,0,0,q_d,r_d]^T。

对式(21)微分可得：

${\stackrel{\·}{V}}_e=\stackrel{\·}{V}-{\stackrel{\·}{V}}_d---\left(21\right)$

等号左右两侧同乘M，可得：

$M{\stackrel{\·}{V}}_e=M\stackrel{\·}{V}-M{\stackrel{\·}{V}}_d---\left(22\right)$

将式(3)代入式(22)，可得

$M{\stackrel{\·}{V}}_e=MV-M{\stackrel{\·}{V}}_d=\stackrel{\‾}{N}+\stackrel{\‾}{G}+\mathrm{\τ}-M{\stackrel{\·}{V}}_d---\left(23\right)$

根据式(23)可设计如下控制律：

$\mathrm{\τ}=M{\stackrel{\·}{V}}_d+K{\stackrel{\·}{V}}_e-\stackrel{\‾}{N}-\stackrel{\‾}{G}---\left(24\right)$

式中，K＝diag(k₁,0,0,0,k₂,k₃)，diag()表示对角矩阵。