[发明专利]一种输入饱和的飞艇航迹控制方法

摘要

本发明提供一种输入饱和的飞艇航迹控制方法，针对执行机构具有输入饱和特性的飞艇的航迹控制问题，建立了其空间运动的数学模型；以此模型为受控对象，采用反步控制方法设计了输入饱和航迹控制律，解决了执行机构输入饱和约束下的航迹控制问题。

主权项

一种输入饱和的飞艇航迹控制方法，其特征在于，包括以下步骤：步骤S100：设定指令航迹ηd＝[xd,yd,zd,θd,ψd,φd]T，其中xd、yd、zd、θd、ψd和φd分别为指令x坐标、指令y坐标、指令z坐标、指令俯仰角、指令偏航角和指令滚转角；步骤S200：误差量计算：计算所述指令航迹与所述飞艇的实际航迹之间的误差量e；步骤S300：输入饱和航迹控制律设计：选取虚拟控制量，采用反步控制方法设计输入饱和航迹控制律，计算航迹控制量u；步骤S400：用所得航迹控制量u对所述飞艇进行控制后，判断控制结果是否满足航迹控制误差小于1m，如果满足则控制结束，如果不满足则调整滑模面设计参数c，虚拟控制参数ki和航迹控制参数ρi的取值后，重复步骤S300；所述步骤S300包括以下步骤：步骤310：建立所述飞艇的空间运动数学模型步骤S311：飞艇空间运动的坐标系及运动参数定义：地面坐标系oexyz和体坐标系obxbybzb对飞艇的空间运动进行描述，CV为浮心，CG为重心，浮心到重心的矢量为rG＝[xG,yG,zG]T；运动参数定义：位置P＝[x,y,z]T，x、y、z分别为轴向、侧向和竖直方向的位移；姿态角Ω＝[θ,ψ,φ]T，θ、ψ、φ分别为俯仰角、偏航角和滚转角；速度v＝[u,v,w]T，u、v、w分别为体坐标系中轴向、侧向和垂直方向的速度；角速度ω＝[p,q,r]T，p、q、r分别为滚转、俯仰和偏航角速度；步骤S312：飞艇空间运动的数学模型：η·=J(η)=J103×303×3J2V---(2)]]>MV·=N‾+G‾+τ---(3)]]>式中J1=cosψcosθcosψsinθsinφ-sinψcosφcosψsinθcosφ+sinψsinφsinψcosθsinψsinθsinφ+cosψcosφsinψsinθcosφ-cosψsinφ-sinθcosθsinφcosθcosφ---(4)]]>J2=0cosφ-sinφ0secθsinφsecθcosφ1tanθsinφtanθcosφ---(5)]]>M=m+m11000mzG-myG0m+m220-mzG0mxG00m+m33myG-mxG00-mzGmyGIx+I1100mzG0-mxG0Ix+I2200mxG0-Ixz0Ix+I33---(6)]]>G‾=(B-G)sinθ(G-B)cosθsinφ(G-B)cosθcosφyGGcosθcosφ-zGGcosθsinφ-xGGcosθcosφ-zGGsinθxGGcosθsinφ+yGGsinθ---(7)]]>τ=TcosμcosυTsinμTcosμsinυTsinυlyTcosυlz-TsinυlxTcosυlz-Tsinυlx---(8)]]>N‾=[Nu,Nv,Nw,Np,Nq,Nr]T---(9)]]>其中Nu=(m+m22)vr-(m+m33)wq+m[xG(p2+r2)-yGpq-zGpr]+QV2/3(-CXcosαcosβ+CYcosαsinβ+CZsinα)---(10)]]>Nv=(m+m33)wp-(m+m11)ur-m[xGpq-yG(p2+r2)+zGqr]+QV2/3(CXsinβ+CYcosβ)---(11)]]>Nw=(m+m22)vp-(m+m11)uq-m[xGpr+yGqr-zG(p2+q2)]+QV2/3(-CXsinαsinβ+CYsinαcosβ-CZcosα)---(12)]]>Np=[(Iy+I22)-(Iz+I33)]qr+Ixzpq-Ixypr-Iyz(r2-q2)+[mzG(ur-wp)+yG(uq-vp)]+QVCl---(13)]]>Nq=[(Iz+I33)-(Ix+I22)]pr+Ixyqr-Iyzpq-Ixz(p2-r2)+m[xG(vp-uq)-zG(wp-vr)]+QVCm---(14)]]>Nr=[(Iy+I22)-(Ix+I11)]pq-Ixzqr-Ixy(q2-p2)+Iyzpr+m[yG(wq-vr)-xG(ur-wp)]+QVCn---(15)]]>式中，m为飞艇质量，m11、m22、m33为附加质量,I11、I22、I33为附加惯量；Q为动压，α为迎角,β为侧滑角，CX、CY、CZ、Cl、Cm、Cn为气动系数；Ix、Iy、Iz分别为绕obxb、obyb、obzb的主惯量；Ixy、Ixz、Iyz分别为关于平面obxbyb、obxbzb、obybzb的惯量积；T为推力大小，μ为推力矢量与obxbzb面之间的夹角，规定其在obxbzb面之左为正，υ为推力矢量在obxbzb面的投影与obxb轴之间的夹角，规定其投影在obxb轴之下为正；lx、ly、lz表示推力作用点距原点ob的距离；步骤S313：将关于广义速度V的表达式式(3)，变换为关于广义坐标η的表达式：由式(1)可得：V=J-1(η)η·=R(η)η·=A03×303×3Bη·---(16)]]>式中J‑1(η)为J(η)的逆矩阵，A=cosψcosθsinψcosθ-sinθcosψsinθsinφ-sinψcosφsinψsinθsinφ+cosψcosφcosθsinφcosψsinθcosφ+sinψsinφsinψsinθcosφ-cosψsinφcosθcosφ---(17)]]>B=0-sinθ1cosφcosθsinφ0-sinφcosθcosφ0---(18)]]>对式(16)微分，可得V·=R·η·+Rη··---(19)]]>式中R·=A·03×303×3B·---(20)]]>式(19)左乘可得RTMV·=RTMR·η·+RTMRη··---(21)]]>由式(3)、式(19)以及式(21)可得，如式(22)所示的数学模型，即为被控对象采用反步控制方法设计航迹控制律：Mη(η)η··+Nη(η,η·)η·+Gη(η)=u---(22)]]>式中Mη(η)＝RTMR  (23)Nη(η,η·)=RTMR·---(24)]]>Gη(η)=-RT(N‾+G‾)---(25)]]>u＝RTτ  (26)其中，u＝[u1,u2,u3,u4,u5,u6]T，τ＝[τ1,τ2,τ3,τ4,τ5,τ6]T，飞艇执行机构的输入饱和特性：τi=ξ(τm)(τi-τi0-),(τi≤τi0-)0,(τi0-≤τi≤τi0+)ξ(τm)(τi-τi0+),(τi≥τi0+)---(27)]]>其中，和分别为执行机构输入的下阈值和上阈值，和死区的下阈值和上阈值，i＝1,2,…,6，函数ξ(τm)为ξ(τm)=τim+τi-τi0+,τi-τi0+>τim+1,τi-τi0-≥τim-orτi-τi0+≤τim+τim-τi-τi0-,τi-τi0->τim----(28);]]>步骤S320：航迹控制律设计虚拟控制量为：Γ=J-1(η)(η·d-ke)---(29)]]>其中，k＝diag(k1,k2,k3,k4,k5,k6)，diag(·)表示对角矩阵，k为正定矩阵，定义广义速度V与虚拟控制量Γ之间的误差：ε＝V‑Γ  (30)其中，ε＝[ε1,ε2,ε3,ε4,ε5,ε6]T，定义滑模面：s＝ce+ε  (31)其中，c＞0，s＝[s1,s2,s3,s4,s5,s6]T，定义向量函数：f(x)=-1λ(λJT(η)e-cMη(η)ke+cMη(η)J(η)ϵ-Nη(η,η·)-Gη(η)-Mη(η)Γ·)---(32)]]>其中，0＜λ＜1，f(x)＝[f1(x),f2(x),f3(x),f4(x),f5(x),f6(x)]T，设计输入饱和航迹控制律，航迹控制量为：ui=-(siρisi+|si|||fi(x)||)si|si|2+τi0+,si<00,si=0-(siρisi+|si|||fi(x)||)si|si|2+τi0-,si<0---(33)]]>其中，ρi＞0，si∈s，ui∈u，fi(x)∈f(x)，i＝1,2,…,6。