CN103984356B - 轨迹规划量测噪声抑制方法 - Google Patents

Abstract

轨迹规划量测噪声抑制方法，(1)在探月飞行器当前制导周期的初始时刻，量测得到地心距r_c、经度θ_c、纬度φ_c、速度V_c、航迹倾角γ_c和航向角ψ_c，作为当前状态量；利用探月飞行器的当前点和目标点计算飞行器的待飞航程s₀；(2)计算探月飞行器从当前点至目标点的状态值以及从当前点到目标点的航程s_f(k)；若s_f(k)-s₀>25km/R₀，并且k<10，则执行步骤(3)；如果s_f(k)-s₀≤25km/R₀，则执行(5)；如果s_f(k)-s₀>25km/R₀，并且k≥10，则取σ₀(k)＝σ₀(1)，执行(5)；(3)对步骤(2)得到的s_f(k)以及σ₀(k)进行微分得到和(4)利用迭代方法，得到制导倾侧角指令迭代新值σ₀(k+1)，进入下一迭代步，即令k的值自加1；将得到的新值σ₀(k+1)作为下一迭代步中的σ₀(k)，转步骤(2)循环执行；(5)当前制导周期的制导指令为σ₀(k)；进入下一制导周期，进入步骤(1)，直至制导结束。

Description

轨迹规划量测噪声抑制方法

技术领域

本发明属航空航天领域，涉及一种轨迹规划量测噪声抑制新方法。

背景技术

预测校正算法能够适用于探月返回飞行器的轨迹规划问题和闭环制导问题。该算法的重点是寻找从当前航程点飞至结束点应采用的倾侧角幅值指令σ₀，具体幅值由飞行器至着陆点的纵程要求决定。需要通过求解一个非线性、单变量求根问题得到。寻找σ₀的问题归为求解如下方程

s_f(cosσ₀)-s₀＝0

的根问题，其中s_f表示飞行器的航程，s₀表示待飞航程。一般采用割线法迭代求解上式，迭代表达式为：

$x_{n+1}=x_n-\frac{x_n-x_{n-1}}{s_f\left(x_n\right)-s_f\left(x_{n-1}\right)}(s_f\left(x_n\right)-s_0)$

其中x_n＝cosσ_n。在上述算法中，系数项

$\frac{x_n-x_{n-1}}{s_f\left(x_n\right)-s_f\left(x_{n-1}\right)}$

用来计算牛顿迭代法中的(参考文献：陆平，朱亮，敬忠良，胡士强，探月返回跳跃式再入制导，全国第十三届空间及运动体控制技术学术年会论文集)。

当存在量测噪声时，系数项对噪声具有放大作用。这里提出利用微分器的方法计算微分器是计算过程微分的有效方法，具有理想的微分效果和噪声抑制效果，从而在一定程度上解决了倾侧角迭代中的量测噪声抑制问题。

发明内容

本发明的技术解决问题是：提供了一种轨迹规划量测噪声抑制新方法，该方法相对现有技术能够较好地解决轨迹规划的量测噪声抑制问题。

本发明的技术解决方案是：轨迹规划量测噪声抑制方法，步骤如下：

(1)在探月飞行器当前制导周期的初始时刻，量测得到地心距r_c、经度θ_c、纬度φ_c、速度V_c、航迹倾角γ_c和航向角ψ_c，作为当前状态量；利用探月飞行器的当前点和目标点计算飞行器的待飞航程s₀；其中，定义k为迭代变量，σ₀(k)为制导倾侧角指令，k的初始值为1；σ₀(1)等于上一制导周期的制导倾侧角指令；

(2)根据步骤(1)的当前状态量和制导倾侧角指令σ₀(k)，计算探月飞行器从当前点至目标点的状态值：地心距r(k)、经度θ(k)、纬度φ(k)、速度V(k)、航迹倾角γ(k)和航向角ψ(k)，以及从当前点到目标点的航程s_f(k)；若s_f(k)-s₀>25km/R₀，并且k<10，则执行步骤(3)；如果s_f(k)-s₀≤25km/R₀，则执行步骤(5)；如果s_f(k)-s₀>25km/R₀，并且k≥10，则取σ₀(k)＝σ₀(1)，然后执行步骤(5)；

(3)对步骤(2)得到的s_f(k)以及σ₀(k)进行微分得到和其中x(k)＝cosσ₀(k)；

(4)根据步骤(2)得到的s_f(k)、σ₀(k)，以及步骤(3)得到的和利用迭代方法，得到制导倾侧角指令迭代新值σ₀(k+1)，进入下一迭代步，即令k的值自加1；将得到的新值σ₀(k+1)作为下一迭代步中的σ₀(k)，转步骤(2)循环执行；

(5)当前制导周期的制导指令为σ₀(k)；进入下一制导周期，即将下一制导周期作为当前制导周期进入步骤(1)，直至制导结束。

所述步骤(4)中的迭代计算公式如下： ${\mathrm{\&sigma;}}_0(k+1)={\mathrm{\&sigma;}}_0\left(k\right)-\mathrm{\&lambda;}{s}_f\left(k\right)\stackrel{\&CenterDot;}{x}\left(k\right)/{\stackrel{\&CenterDot;}{s}}_f\left(k\right),\mathrm{\&lambda;}>0.$

本发明与现有技术相比有益效果为：

(1)本发明方法提出了一种新型探月返回飞行器数值预测校正制导方法，通过引入微分器对倾侧角进行求解，有效减少了量测噪声的影响。

(2)本发明利用微分方法对制导指令进行求解，不同于文献中的割线法。本方法所采用的微分器具有抑制噪声的作用。

(3)利用探月返回飞行器作为仿真对象，设计了轨迹规划量测噪声抑制新方法，有效减少了量测噪声的影响。

附图说明

图1为轨迹规划中的倾侧角剖面设计图；

图2为本发明流程图。

具体实施方式

下面结合附图对本发明做详细说明，图2为本发明方法的流程框图，本发明包括如下5个步骤：

步骤(1)在探月飞行器的每个制导周期，通过迭代计算制导指令σ₀。在当前制导周期的初始时刻(下文称当前点)，通过量测得到当前点的状态量：地心距r_c、经度θ_c、纬度φ_c、速度V_c、航迹倾角γ_c和航向角ψ_c，作为当前状态量。利用当前点和目标点(着陆降落伞展开位置)求出待飞航程s₀。定义迭代变量k。定义迭代初值σ₀(1)等于上一制导周期的制导律。初始值k＝1。然后执行步骤(2)。

本发明中，待飞航程s₀由下式求出：

s₀＝arccos(sinφ_csinφ_s+cosφ_ccosφ_scos(θ_c-θ_s))

其中，φ_s,θ_s为目标点的经度和纬度。

步骤(2)针对探月飞行器纵向再入动力学方程，以步骤(1)给出的当前状态量和σ₀(k)作为输入，利用数值积分计算出从当前点至目标点的状态值：地心距r(k)、经度θ(k)、纬度φ(k)、速度V(k)、航迹倾角γ(k)和航向角ψ(k)，和从当前点至目标点的航程s_f(k)。

本发明中，考虑地球自转的探月飞行器纵向再入动力学方程如下：

$\stackrel{\&CenterDot;}{r}=V\sin \mathrm{\&gamma;}---\left(1\right)$

$\stackrel{\&CenterDot;}{\mathrm{\&theta;}}=\frac{V\cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}}{r\cos \mathrm{\&phi;}}---\left(2\right)$

$\stackrel{\&CenterDot;}{\mathrm{\&phi;}}=\frac{V\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}}{r}---\left(3\right)$

$\stackrel{\&CenterDot;}{V}=-D-\frac{\sin \mathrm{\&gamma;}}{r^2}+{\mathrm{\&Omega;}}^{2}r\cos \mathrm{\&phi;}(\sin \mathrm{\&gamma;}\cos \mathrm{\&phi;}-\cos \mathrm{\&gamma;}\sin \mathrm{\&phi;}\cos \mathrm{\&psi;})---\left(4\right)$

$V\stackrel{\&CenterDot;}{\mathrm{\&gamma;}}=L\cos \mathrm{\&sigma;}+(V^2-\frac{1}{r})\frac{\cos \mathrm{\&gamma;}}{r}+2\mathrm{\&Omega;}V\cos \mathrm{\&phi;}\sin \mathrm{\&psi;}+{\mathrm{\&Omega;}}^{2}r\cos \mathrm{\&phi;}(\cos \mathrm{\&gamma;}\cos \mathrm{\&phi;}+\sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}\sin \mathrm{\&phi;})---\left(5\right)$

$V\stackrel{\&CenterDot;}{\mathrm{\&psi;}}=\frac{L\sin \mathrm{\&sigma;}}{\cos \mathrm{\&gamma;}}+\frac{V^2\cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}\tan \mathrm{\&phi;}}{r}-2\mathrm{\&Omega;V}(\tan \mathrm{\&gamma;}\cos \mathrm{\&psi;}\cos \mathrm{\&phi;}-\sin \mathrm{\&phi;})+\frac{{\mathrm{\&Omega;}}^{2}r\sin \mathrm{\&psi;}\sin \mathrm{\&phi;}\cos \mathrm{\&phi;}}{\cos \mathrm{\&gamma;}}---\left(6\right)$

其中

$L=\frac{\mathrm{\&rho;}{\left(V_{s}V\right)}^2{S}_{\mathrm{ref}}{C}_L}{2{\mathrm{mg}}_0},D=\frac{\mathrm{\&rho;}{\left(V_{s}V\right)}^2{S}_{\mathrm{ref}}{C}_D}{2{\mathrm{mg}}_0}---\left(7\right)$

当地球转动和正对目标点的航向偏移量可以忽略时，航程变量s可以由下式得到：

$\stackrel{\&CenterDot;}{s}=V\cos \mathrm{\&gamma;}/r---\left(8\right)$

方程(1)-(8)求导针对的是无量纲时间R₀和g₀分别表示地球平均半径和地球表面重力加速度，R₀＝6378136.3m，g₀＝9.81m/s²。方程(1)-(8)中，状态量为地心距r、经度θ、纬度φ、速度V、航迹倾角γ和航向角ψ；倾侧角σ为输入，如图1所示，s₁＝2200km/R₀，当飞行器在跳跃段和开普勒段时，倾侧角σ＝σ₀(k)；在最终段σ＝σ_f；

各符号说明见附录。

针对方程(1)-(8)，以倾侧角σ和步骤(1)得到的当前状态量作为输入，利用数值积分计算出从当前点至目标点的状态值，和从当前点至目标点的航程s_f(k)。

如果s_f(k)-s₀>25km/R₀，并且k<10，则执行步骤(3)。如果s_f(k)-s₀≤25km/R₀，则执行步骤(5)。如果s_f(k)-s₀>25km/R₀，并且k≥10，则取σ₀(k)＝σ₀(1)，然后执行步骤(5)。

步骤(3)以步骤(2)给出的s_f(k)和σ₀(k)作为输入，利用微分器得到和其中x(k)＝cosσ₀(k)。然后执行步骤(4)。

本发明中，微分器采用跟踪微分器(TD)：(z₁,z₂)＝TD(z₁,z₂,z,r,h):

$\left\{\begin{array}{c}{z}_1(k+1)=z_1\left(k\right)+h{z}_2\left(k\right)\\ z_2(k+1)=z_2\left(k\right)+\mathrm{hfhan}(z_1\left(k\right)-z\left(k\right),z_2\left(k\right),r,h)\end{array}\right.,z_1\left(1\right)=0,z_2\left(1\right)=1$

其中，r为设计变量，h为采样周期，函数fhan(z₁,z₂,r,h)的具体表达式为：

$\left\{\begin{array}{c}d=\mathrm{rh}\\ d_0=\mathrm{hd}\\ y=z_1+h{z}_2\\ a_0=\sqrt{d^2+8r\left|y\right|}\\ a=\left\{\begin{array}{cc}{x}_2+\frac{(a_0-d)}{2}\mathrm{sign}\left(y\right),& \left|y\right|>d_0\\ x_2+\frac{y}{h},& \left|y\right|\&le;d_0\end{array}\right.\\ \mathrm{fhan}=-\left\{\begin{array}{cc}\mathrm{rsign}\left(a\right),& \left|a\right|>d\\ r\frac{a}{d},& \left|a\right|\&le;d\end{array}\right.\end{array}\right.$

以步骤(2)得到的s_f(k)和步骤(2)中定义的σ₀(k)作为输入，利用上述TD可得 $\stackrel{\&CenterDot;}{x}\left(k\right)=x_2(k+1),{\stackrel{\&CenterDot;}{s}}_f\left(k\right)=y_2(k+1),$ 其中，

(x₁(k+1),x₂(k+1))＝TD(x₁(k),x₂(k),σ₀(k),r,h)

(y₁(k+1),y₂(k+1)＝TD(y₁(k),y₂(k),s_f(k),r,h)

步骤(4)以步骤(3)给出的和作为输入，利用迭代计算公式，给出倾侧角迭代新值σ₀(k+1)。令k＝k+1，然后执行步骤(2)。

本发明中，迭代计算公式为：

${\mathrm{\&sigma;}}_0(k+1)={\mathrm{\&sigma;}}_0\left(k\right)-\mathrm{\&lambda;}{s}_f\left(k\right)\stackrel{\&CenterDot;}{x}\left(k\right)/{\stackrel{\&CenterDot;}{s}}_f\left(k\right),\mathrm{\&lambda;}>0$

步骤(5)方程

s_f(cosσ₀)-s₀＝0

的根为σ₀(k)，从而得到新的制导指令。进入下一制导周期，即将下一制导周期作为当前制导周期进入步骤(1)，直至制导结束。

附录：符号说明

本发明未详细说明部分属于本领域技术人员公知常识。

Claims (1)

1.轨迹规划量测噪声抑制方法，其特征在于步骤如下：

(1)在探月飞行器当前制导周期的初始时刻，量测得到地心距r_c、经度θ_c、纬度φ_c、速度V_c、航迹倾角γ_c和航向角ψ_c，作为当前状态量；利用探月飞行器的当前点和目标点计算飞行器的待飞航程s₀；其中，定义k为迭代变量，σ₀(k)为制导倾侧角指令，k的初始值为1；σ₀(1)等于上一制导周期的制导倾侧角指令；

(2)根据步骤(1)的当前状态量和制导倾侧角指令σ₀(k)，计算探月飞行器从当前点至目标点的状态值：地心距r(k)、经度θ(k)、纬度φ(k)、速度V(k)、航迹倾角γ(k)和航向角ψ(k)，以及从当前点到目标点的航程s_f(k)；若s_f(k)-s₀>25km/R₀，并且k<10，则执行步骤(3)；如果s_f(k)-s₀≤25km/R₀，则执行步骤(5)；如果s_f(k)-s₀>25km/R₀，并且k≥10，则取σ₀(k)＝σ₀(1)，然后执行步骤(5)；其中，R₀表示地球平均半径；

(3)对步骤(2)得到的s_f(k)以及σ₀(k)进行微分得到和其中x(k)＝cosσ₀(k)；

(4)根据步骤(2)得到的s_f(k)、σ₀(k)，以及步骤(3)得到的和利用迭代方法，得到制导倾侧角指令迭代新值σ₀(k+1)，进入下一迭代步，即令k的值自加1；将得到的新值σ₀(k+1)作为下一迭代步中的σ₀(k)，转步骤(2)循环执行；其中，迭代计算公式： ${\mathrm{\&sigma;}}_0(k+1)={\mathrm{\&sigma;}}_0\left(k\right)-{\mathrm{\&lambda;s}}_f\left(k\right)\stackrel{\&CenterDot;}{x}\left(k\right)/{\stackrel{\&CenterDot;}{s}}_f\left(k\right),\mathrm{\&lambda;}>0;$

(5)当前制导周期的制导指令为σ₀(k)；进入下一制导周期，即将下一制导周期作为当前制导周期进入步骤(1)，直至制导结束。