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

本发明属于自动控制
技术领域：
，特别涉及一种航天航空领域的飞行控制方法，它为欠驱动飞艇提供一种航迹跟踪控制方法。
背景技术：
：飞艇是指一种依靠轻于空气的气体(如氦气、氢气等)产生静浮力升空，依靠自动飞行控制系统实现定点驻留和低速机动的飞行器，具有滞空时间长、能耗低、效费比高及定点驻留等优点，广泛应用于侦察监视、对地观测、环境监测、应急救灾、科学探测等领域，具有重要应用价值和广阔的应用前景，当前已成为航空领域的研究热点。航迹跟踪是指飞艇从给定的初始状态出发并跟踪给定的期望航迹。飞艇的空间运动具有非线性、通道耦合、不确定、易受外界扰动等特点，因此，航迹控制成为飞艇飞行控制的关键技术之一。众多研究人员针对全驱动飞艇的航迹跟踪问题，提出了一系列航迹控制方法，为飞艇航迹控制提供了可供参考借鉴的技术方案。但是，工程实际中，飞艇的控制输入量一般少于其自由度数，为典型的欠驱动运动体。因此，上述控制方法未能有效解决欠驱动飞艇的航迹跟踪问题。技术实现要素：为了解决欠驱动飞艇的航迹跟踪问题，本发明提供一种欠驱动飞艇航迹跟踪控制方法。本发明所提出的航迹控制器结构框图如图1所示。首先根据期望航迹和实际航迹计算位置误差量，然后采用串级设计方法解算期望速度，再根据期望速度和实际速度设计航迹控制律，计算航迹控制量。实际应用中，飞艇航迹由导航系统测量得到，将由该方法计算得到的控制量传输至执行机构即可实现航迹控制功能。由该方法控制的闭环系统能够高精度跟踪期望航迹，为欠驱动飞艇航迹跟踪控制的工程实现提供了有效方案。一种欠驱动飞艇航迹跟踪控制方法，包括以下步骤：S1：给定期望航迹：Pd＝[xd,yd,zd]T，其中，xd、yd和zd分别为期望x坐标、期望y坐标和期望z坐标，上标T表示向量或矩阵的转置；S2：计算期望航迹与实际航迹之间的航迹误差量Pe；S3：根据航迹误差量Pe求解期望速度Ud；S4：考虑飞艇的欠驱动特性，设计航迹跟踪控制律，计算航迹控制量τ。本发明步骤S2中，航迹误差量Pe的计算方法如下：Pe＝P-Pd＝[x-xd,y-yd,z-zd]T(1)P＝[x,y,z]T为实际航迹，x、y和z分别为x坐标、y坐标和z坐标。本发明步骤S3中，期望速度Ud的求解方法为：1)建立飞艇空间运动的数学模型飞艇空间运动的坐标系及运动参数定义如下：采用地面坐标系oexeyeze和体坐标系obxbybzb对飞艇的空间运动进行描述，CV为浮心，CG为重心，浮心到重心的矢量为rG＝[xG,yG,zG]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)飞艇空间运动的数学模型描述如下：η·=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)]]>τ=τu000τmτn=Tcosμcosυ000Tcosμcosυlz-TcosμsinυlxTsinμlx-Tcosμcosυly---(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+I11)]pr+Ixyqr-Iyzpq-Ixz(p2-r2)+m[xG(vp-uq)-zG(wq-vr)]+QVCm---(14)]]>Nr=[(Iy+I22)-(Ix+I11)]pq+Ixzqr-Ixy(p2-p2)+Iyzpr+m[yG(wq-uvrq)-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的距离。3)解算期望速度Ud将航迹误差量Pe变换为体坐标系中的误差ξe＝J-1Pe(16)式中，J-1为J的逆矩阵。定义ζe＝ξe-λ(17)式中，λ＝[ρ,0,0]T，ρ为一个正实数；对式(17)求导，可得：ζ·e=-ω×ζe-ω×λζe+v-J-1P·d=10000-ρ0ρ0uqr+0vw-ω×ζe-J-1-P·d---(18)]]>考虑到飞艇在v、w和p自由度上存在欠驱动，定义期望速度与实际速度之间的误差为：Ue＝U-Ud(19)式中，Ue＝[ue,qe,re]T，U＝[u,q,r]T，Ud＝[ud,qd,rd]T；由式(18)可得：Ud=udqdrd=10000-ρ0ρ0-1(-γ100010001ζe-0vw)e+J-1P·d---(20)]]>式中，γ为正实数。本发明步骤S4的方法为：定义以下误差量Ve＝V-Vd(21)式中，V＝[u,0,0,0,q,r]T，Vd＝＝[ud,0,0,0,qd,rd]T。对式(21)微分可得：V·e=V·-V·d---(21)]]>等号左右两侧同乘M，可得：MV·e=MV·-MV·d---(22)]]>将式(3)代入式(22)，可得MV·e=MV·-MV·d=N‾+G‾+τ-MV·d---(23)]]>根据式(23)可设计如下控制律：τ=MV·d+KV·e-N‾-G‾---(24)]]>式中，K＝diag(k1,0,0,0,k2,k3)，diag()表示对角矩阵。本发明针对飞艇的三维航迹跟踪问题，建立了飞艇的非线性动力学模型；以此为受控对象，考虑飞艇的欠驱动特性，采用串级设计方法，将非线性动力学模型分解为两个子系统，先根据位置误差解算出期望速度，再根据期望速度设计控制输入量。该方法的主要优点有：1)该方法考虑了飞艇的欠驱动特性，能够解决执行机构不足或执行机构发生故障情况下的航迹跟踪问题，提高了航迹跟踪控制系统的适应性。2)采用串级设计方法设计航迹控制律，将非线性动力学模型分解为两个子系统，先根据位置误差解算出期望速度，再根据期望速度设计控制输入量，由此简化了控制律设计难度。控制工程师在应用过程中可以根据实际飞行任务给定任意期望航迹，并将由该方法得到的控制量传输至执行机构实现航迹跟踪控制功能。附图说明图1为本发明所述飞艇航迹控制器结构图；图2为本发明所述飞艇航迹跟踪控制方法步骤流程图；图3为本发明所述飞艇坐标系及运动参数定义；图4为本发明所述飞艇航迹跟踪控制结果；图5为本发明所述飞艇航迹跟踪控制误差。图中符号说明如下：PdPd＝[xd,yd,zd]T为飞艇的期望航迹；PP＝[x,y,z]T为飞艇的实际航迹；PePe为飞艇的航迹误差；UdUd为期望速度；oexeyezeoexeyeze表示地面坐标系；obxbybzbobxbybzb表示飞艇体坐标系；xx为实际航迹的x坐标；yy为实际航迹的y坐标；zz为实际航迹的z坐标；θθ为俯仰角；ψψ为偏航角；φφ为滚转角；ττ为控制量；uu为体坐标系中的轴向速度；vv为体坐标系中的侧向速度；ww为体坐标系中的垂直方向的速度；pp为滚转角速度；qq俯仰角速度；rr为偏航角速度。具体实施方式下面结合具体实施例，对本发明作进一步的说明：一种欠驱动飞艇航迹跟踪控制方法，其具体步骤如下：步骤一：给定期望航迹Pd＝[40cos(0.02πt)m,40sin(0.02πt)m,(0.05t)m]T步骤二：航迹误差量计算计算期望航迹与实际航迹之间的误差量：Pe＝P-Pd＝[x-xd,y-yd,z-zd]T(1)其中，P＝[x,y,z]T为实际航迹，x、y和z分别为x坐标、y坐标和z坐标，为连续变化值。初始航迹为：P0＝[x0,y0,z0]T＝[10m,20m,0.02m]T。步骤三：期望速度解算1)建立飞艇空间运动的数学模型为便于描述，飞艇空间运动的坐标系及运动参数定义如下。如图3所示，采用地面坐标系oexeyeze和体坐标系obxbybzb对飞艇的空间运动进行描述，CV为浮心，CG为重心，浮心到重心的矢量为rG＝[xG,yG,zG]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自由度上存在欠驱动。飞艇空间运动的数学模型描述如下：η·=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)]]>τ=τu000τmτn=Tcosμcosυ000Tcosμcosυlz-TcosμsinυlxTsinμlx-Tcosμcosυly---(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+I11)]pr+Ixyqr-Iyzpq-Ixz(p2-r2)+m[xG(vp-uq)-zG(wq-vr)]+QVCm---(14)]]>Nr=[(Iy+I22)-(Ix+I11)]pq-Ixzqr-Ixy(p2-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的距离。本实施例中的飞艇参数见表1。表1飞艇参数2)解算期望速度Ud将航迹误差Pe变换为体坐标系中的误差ξe＝J-1Pe(16)式中，J-1为J的逆矩阵。定义ζe＝ξe-λ(17)式中，λ＝[0.5,0,0]T。对式(17)求导，可得：ζ·e=-ω×ζe-ω×λζe+v-J-1P·d=10000-ρ0ρ0uqr+0vw-ω×ζe-J-1P·d---(18)]]>考虑到飞艇在v、w和p自由度上存在欠驱动，定义期望速度与实际速度之间的速度误差为：Ue＝U-Ud(19)式中，Ue＝[ue,qe,re]T，U＝[u,q,r]T，Ud＝[ud,qd,rd]T。由式(18)可得：Ud=udqdrd=10000-ρ0ρ0-1(-γ100010001ζe-0vw)e+J-1P·d---(20)]]>式中，γ＝2。步骤四：航迹跟踪控制律设计定义以下误差量Ve＝V-Vd(21)式中，V＝[u,0,0,0,q,r]T，Vd＝＝[ud,0,0,0,qd,rd]T。对式(21)微分可得：V·e=V·-V·d---(21)]]>等号左右两侧同乘M，可得：MV·e=MV·-MV·d---(22)]]>将式(3)代入式(22)，可得MV·e=MV·-MV·d=N‾+G‾+τ-MV·d---(23)]]>根据式(23)可设计如下控制律：τ=MV·d+KV·e-N‾-G‾---(24)]]>式中，K＝diag(5,0,0,0,8,8)，diag()表示对角矩阵。实施例中的欠驱动飞艇航迹跟踪控制结果如图4、图5所示。图4给出了飞艇航迹跟踪控制结果，由图4可得：飞艇由初始位置出发，能够准确地跟踪期望航迹，验证了本发明所提出的航迹跟踪控制方法的有效性；图5给出了航迹跟踪控制误差，由图5可得，本发明所提出的航迹控制方法能够高精度地跟踪给定的期望航迹。当前第1页1 2 3