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

技术特征：

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()表示对角矩阵。