CN100476359C - Deep space probe UPF celestial self-navigation method based on starlight angle - Google Patents

Abstract

The invention relates to an UPF automatic astrogation based on the star angular distance. It starts with precise modeling of the UPF track dynamics, using the star angular distance as the measuring unit, and optimizing the estimation based on the UPF (unscented particle filter) for the guide parameter. It applies to UPF guide location on the track transfer for the guide parameter determination.

Description

A kind of deep space probe UPF celestial self-navigation method based on the starlight angular distance

Technical field

The present invention relates to the aerospace navigation technical field, can be applicable to determining of transfer orbit section deep space probe navigational parameter, be particularly related to a kind of UPF (UnscentedParticle Filter) celestial self-navigation method of the deep space probe based on the starlight angular distance, be applicable to the navigator fix of the deep space probe on the transfer orbit.

Background technology

At present, survey of deep space is the focus of international space industry research, one of China has also developed survey of deep space as space flight in the recent period target.Because the deep space probe long-play is in transfer orbit, and be that distance to go is far away, speed is fast in one of characteristics of transfer orbit motion, the electromagnetic wave round-trip delay is long, and it is very difficult with observing and controlling to be taken remote measurement by land station; The 2nd, the free-running operation time is long, and under microgravity environment, can't navigate with inertial navigation technology; The 3rd, the accuracy requirement height, awing the way must position and error be proofreaied and correct; The 4th, the power consumption that the requirement deep space probe carries equipment is little, equipment is light, reliability is high.

Present stage, the navigate mode of deep space probe mainly was to adopt land station's observing and controlling, though this method can provide accurate navigation information, when the detector distance earth is too far away, was difficult to its real-time of assurance.And celestial navigation is a kind of traditional complete autonomous air navigation aid, and it has following characteristics: 1. not needing to carry out any message exchange with the external world, is a kind of autonomous air navigation aid fully; 2. can provide position, speed and attitude information simultaneously; 3. only need utilize detector attitude sensing unit star sensor and Horizon sensor, and not need additionally to increase other hardware device; 4. do not need any priori.By the characteristics of celestial navigation as can be known, celestial navigation has overcome the shortcoming that land station's observing and controlling is difficult to keep real-time, is a kind of autonomous navigate mode fully, has become one of major programme of present stage survey of deep space independent navigation.

Deep space probe celestial self-navigation method commonly used is based on the filtering method of dynamics of orbits equation, promptly utilizes astronomical measurement information in conjunction with the dynamics of orbits equation, obtains the navigation information of deep space probe by the method for optimal estimation.At present, following several main type is arranged in the selection of the astronomical measurement information of deep space probe:

1) sun, earth direction vector: utilize detector to the direction vector of the sun, the earth as measurement information.

2) angle between the sun, the earth, the planet direction vector: utilize solar vector direction, earth direction vector, the angle between the planet direction vector is as measurement information.

3) starlight angular distance: i.e. angle between the direction vector of the direction vector of the navigation fixed star starlight that observes from detector and the center celestial body centre of sphere.

Use direction vector as measurement information, can obtain reasonable filtering accuracy and convergence effect, but there are coupled relation in direction vector and detector attitude, the measuring accuracy of direction vector is subjected to the influence of attitude accuracy, therefore, there is low, the uppity defective of navigation accuracy in existing deep space probe celestial self-navigation method.

Summary of the invention

Technology of the present invention is dealt with problems and is: overcome the deficiency of land station's observing and controlling aspect the deep space probe navigation, adopt the navigator fix that carries out deep space probe with the starlight angular distance between the sun, the earth, planet and the fixed star as the UPF astronomical navigation method of measurement amount.

Technical solution of the present invention is: a kind of UPF celestial self-navigation method of the deep space probe based on the starlight angular distance, at first accurately set up the state equation (be many track body kinetic model) of deep space probe on transfer orbit, set up the measurement equation of system then as the measurement amount with the starlight angular distance between the sun, the earth, planet and the fixed star, adopt advanced UPF filtering algorithm to obtain high-precision position, velocity estimation at last, concrete steps are as follows:

(1) setting up the state equation of deep space probe on transfer orbit is:

${\stackrel{\·\·}{r}}_{\mathrm{ps}}=-{\mathrm{\μ}}_s\frac{r_{\mathrm{ps}}}{r_{\mathrm{ps}}^3}-{\mathrm{\μ}}_m[\frac{r_{\mathrm{pm}}}{r_{\mathrm{pm}}^3}-\frac{r_{\mathrm{sm}}}{r_{\mathrm{sm}}^3}]-{\mathrm{\μ}}_e[\frac{r_{\mathrm{pe}}}{r_{\mathrm{pe}}^3}-\frac{r_{\mathrm{se}}}{r_{\mathrm{se}}^3}]$

In the formula, r _PsFor day the heart to the radius vector of detector; Be r _PsSecond derivative, r _PmBe the radius vector of planet to detector; r _PeBe the radius vector of the earth to detector; r _SeBe the radius vector of the earth's core to day heart; r _SmBe the radius vector of planet center to day heart; μ _s, μ _m, μ _eBe respectively solar gravitation constant, planetary gravitation constant and terrestrial gravitation constant.Suppose that planet makes uniform circular motion around the sun, radius is the mean distance r of the sun to planet _SmThe earth is made uniform circular motion around the sun, and radius is 1 astronomical long measure AU.

(2) set up the measurement equation of system as the measurement amount with three starlight angular distances of detector and the sun, the earth, planet and fixed star, promptly the direction vector of the navigation fixed star starlight that observes from deep space probe respectively and the angle between the direction vector of the sun, the earth and the Mars centre of sphere, expression formula is as follows:

$\left\{\begin{array}{c}{\mathrm{\θ}}_s=\arccos (-\frac{r_{\mathrm{ps}}\·s_1}{r_{\mathrm{ps}}})\\ {\mathrm{\θ}}_e=\arccos (-\frac{r_{\mathrm{pe}}\·s_2}{r_{\mathrm{pe}}})\\ {\mathrm{\θ}}_m=\arccos (-\frac{r_{\mathrm{pm}}\·s_3}{r_{\mathrm{pm}}})\end{array}\right.$

In the formula, s ₁, s ₂, s ₃Be respectively the unit vector of fixed star starlight direction, discern by star sensor.

(3) adopt the UPF filtering algorithm to improve the navigation accuracy of deep space probe on the transfer orbit, the UPF algorithm utilizes UKF (Unscented Kalman Filter) to obtain the important density function of particle, at first determine one group of sigma point according to previous moment particle and variance thereof, the position of this point set and weights are well-determined by the expectation of particle and variance, can catch the feature of particle probability distribution more exactly.Then with these some difference substitution state equations, obtain a new point set, with the weighted sum of these point sets as expectation, with the weighted sum of its variance as variance, and then the expectation and the variance of having tried to achieve are revised with measuring equation, and with expectation and the variance of revised value, produce the particle of a current time as Gaussian distribution.Because it takes into full account current measurement to the influence that posterior probability distributes, improved the utilization ratio of particle.

Principle of the present invention is: because natural celestial body is always by its intrinsic regular movement, they sometime relatively the position vector of preferred coordinate system can accurately obtain, therefore those are convenient to just constitute with the natural celestial body that satellite borne equipment is observed the beacon of celestial navigation in the deep space probe flight course, handle by the data that beacon observation station is obtained, just can obtain the position at deep space probe place.Concrete grammar is at first accurately to set up the state equation (many track body kinetic model) of deep space probe on transfer orbit, set up the measurement equation of system then as the measurement amount with three starlight angular distances of detector and the sun, the earth, planet and fixed star, and adopt the UPF filtering algorithm, finally improve the navigation accuracy of deep space probe.

The present invention's advantage compared with prior art is: overcome the shortcoming that when the detector distance earth is too far away land station's observing and controlling is difficult to guarantee its real-time, made up a kind of deep space probe complete autonomous that is used on the transfer orbit, high-precision astronomical navigation method, it has the following advantages: (1) adopts the astronomical navigation method of starlight angular distance as the measurement amount, only need utilize deep space probe attitude sensing unit star sensor and infrared horizon, and do not need additionally to increase other hardware device, not needing to carry out any message exchange with the external world, is a kind of autonomous air navigation aid fully; (2) adopt the UPF filtering algorithm, the particle filter that has overcome standard does not consider that up-to-date measurement information and UKF can only be applied to the deficiency that noise is a Gaussian distribution, can obtain than the particle filter of standard and the filtering accuracy of UKF filtering convergence faster and Geng Gao.

Description of drawings

Fig. 1 is the process flow diagram of a kind of embodiment of celestial self-navigation method of the present invention;

Fig. 2 is measurement information---the starlight angular distance synoptic diagram among the present invention.

Embodiment

As depicted in figs. 1 and 2, the deep space probe that present embodiment adopts is a kind of Mars probes, and concrete grammar of the present invention is as follows:

1, set up the state equation (be many track body kinetic model) of Mars probes on transfer orbit, the state model among the present invention is thought of as circular restricted limbs model.Circular restricted limbs model need be considered the interaction between detector, sun center gravitation, Mars center gravitation and ground ball center gravitation, and the influence of other perturbations is not then considered.

Suppose that Mars makes uniform circular motion around the sun, radius is a day fiery mean distance r _SmThe earth is made uniform circular motion around the sun, and radius is 1 astronomical long measure AU.In the general restricted limbs model, the Mars probes state equation can be expressed as formula (1).

${\stackrel{\·\·}{r}}_{\mathrm{ps}}=-{\mathrm{\μ}}_s\frac{r_{\mathrm{ps}}}{r_{\mathrm{ps}}^3}-{\mathrm{\μ}}_m[\frac{r_{\mathrm{pm}}}{r_{\mathrm{pm}}^3}-\frac{r_{\mathrm{sm}}}{r_{\mathrm{sm}}^3}]-{\mathrm{\μ}}_e[\frac{r_{\mathrm{pe}}}{r_{\mathrm{pe}}^3}-\frac{r_{\mathrm{se}}}{r_{\mathrm{se}}^3}]---\left(1\right)$

In the formula, r _PsFor day the heart to the radius vector of detector;

Be r _PsSecond derivative; r _PmBe the radius vector of Mars to detector; r _PeBe the radius vector of the earth to detector; r _SeBe the radius vector of the earth's core to day heart; r _SmBe the radius vector of the fiery heart to day heart; μ _s, μ _m, μ _eBe respectively solar gravitation constant, Mars gravitational constant and terrestrial gravitation constant.In the actual computation, vector form is changed into the rectangular coordinate form, choose the earth's core ecliptic inertial coordinates system, can get $\left\{\begin{array}{c}\stackrel{\·}{x}=v_x\\ \stackrel{\·}{y}=v_y\\ \stackrel{\·}{z}=v_z\\ {\stackrel{\·}{v}}_x=-{\mathrm{\μ}}_s\frac{x}{r_{\mathrm{ps}}^3}-{\mathrm{\μ}}_m[\frac{x-x_1}{r_{\mathrm{pm}}^3}-\frac{x_1}{r_{\mathrm{sm}}^3}]-{\mathrm{\μ}}_e[\frac{x-x_2}{r_{\mathrm{pe}}^3}-\frac{x_2}{r_{\mathrm{se}}^3}]\\ {\stackrel{\·}{v}}_y=-{\mathrm{\μ}}_s\frac{y}{r_{\mathrm{ps}}^3}-{\mathrm{\μ}}_m[\frac{y-y_1}{r_{\mathrm{pm}}^3}-\frac{y_1}{r_{\mathrm{sm}}^3}]-{\mathrm{\μ}}_e[\frac{y-y_2}{r_{\mathrm{pe}}^3}-\frac{y_2}{r_{\mathrm{se}}^3}]\\ {\stackrel{\·}{v}}_z=-{\mathrm{\μ}}_s\frac{z}{r_{\mathrm{ps}}^3}-{\mathrm{\μ}}_m[\frac{z-z_1}{r_{\mathrm{pm}}^3}-\frac{z_1}{r_{\mathrm{sm}}^3}]-{\mathrm{\μ}}_e[\frac{z-z_2}{r_{\mathrm{pe}}^3}-\frac{z_2}{r_{\mathrm{se}}^3}]\end{array}\right.---\left(2\right)$

In the formula, (x ₁, y ₁, z ₁) be the Mars coordinate, (x ₂, y ₂, z ₂) be terrestrial coordinates, (x, y z) are the Mars probes coordinate, and wherein Mars and terrestrial coordinates are the function of time, can be tried to achieve by ephemeris.

Formula (2) can be abbreviated as $\stackrel{\·}{X}\left(t\right)=f(X\left(t\right),w\left(t\right))---\left(3\right)$

Wherein, state vector X=[x y z v _xv _yv _z] ^TX, y, z, v _x, v _y, v _zBe respectively detector at X, Y, the position and the speed of three directions of Z; $(\stackrel{\·}{x},\stackrel{\·}{y},\stackrel{\·}{z},{\stackrel{\·}{v}}_x,{\stackrel{\·}{v}}_y,{\stackrel{\·}{v}}_z)$ Be (x, y, z, v _x, v _y, v _z) first order derivative; W is the system model noise.

2, set up the measurement equation of system as the measurement amount with the starlight angular distance between the sun, the earth, planet and the fixed star.The starlight angular distance is a kind of observed quantity commonly used in the celestial navigation, refers to the angle between the direction vector of the direction vector of the navigation fixed star starlight that observes from deep space probe and the celestial body centre of sphere.

Figure 2 shows that the starlight angular distance of the detector and the sun, the earth, Mars and three fixed stars, the expression formula that can obtain the starlight angular distance is as follows.

$\left\{\begin{array}{c}{\mathrm{\θ}}_s=\arccos (-\frac{r_{\mathrm{ps}}\·s_1}{r_{\mathrm{ps}}})\\ {\mathrm{\θ}}_e=\arccos (-\frac{r_{\mathrm{pe}}\·s_2}{r_{\mathrm{pe}}})\\ {\mathrm{\θ}}_m=\arccos (-\frac{r_{\mathrm{pm}}\·s_3}{r_{\mathrm{pm}}})\end{array}\right.---\left(4\right)$

In the formula, s ₁, s ₂, s ₃Be respectively the unit vector of three fixed star starlight directions, discern by star sensor.

Make Z=[θ _sθ _eθ _m] ^T, measurement noise $v={\left[\begin{array}{ccc}{v}_{{\mathrm{\θ}}_S}& v_{{\mathrm{\θ}}_e}& v_{{\mathrm{\θ}}_m}\end{array}\right]}^T,$ The measurement equation that can be got system by formula (4) is:

Z(t)＝H(X(t)，v(t)) (5)

3, adopt the UPF filtering algorithm to improve the navigation accuracy of deep space probe.

Particle filter (PF) is the another kind of way of realization of recursion Bayes wave filter, its basic thought is to describe probability distribution with random sample, these samples are called as " particle ", then on based measurement, the size by regulating each particle weights and the position of sample, be similar to actual probability distribution, and with the average of sample estimated value, can be used for the state estimation of non-linear arbitrarily non-gaussian random system in principle as system.But the shortcoming of this method is to have degradation phenomena, and eliminating degenerates can be dependent on suitably chooses important density function.

The UPF algorithm is exactly to utilize UKF to obtain a kind of particle filter method of the important density function of particle, owing to comprised up-to-date measurement information in this important density function, therefore has more performance.That the deep space probe navigational system is actually is non-linear, the non-Gauss of noise divides other system, so the UPF method is a kind of filtering method that relatively is suitable for.At first determine one group of sigma point according to previous moment particle and variance thereof, the position of this point set and weights are well-determined by the expectation of particle and variance, can catch the feature of particle probability distribution more exactly.Then with these some difference substitution state equations, obtain a new point set, with the weighted sum of these point sets as expectation, with the weighted sum of its variance as variance, and then the expectation and the variance of having tried to achieve are revised with measuring equation, and with expectation and the variance of revised value, produce the particle of a current time as Gaussian distribution.Because it takes into full account current measurement to the influence that posterior probability distributes, improved the utilization ratio of particle.Concrete steps are as follows:

The state equation of system and measurement equation can be obtained by formula (2) and formula (4), and $x_k=f(x_{k-1},w_{k-1})\↔p(x_k,x_{k-1}),$ $z_k=f(x_{k-1},v_k)\↔p(z_k,x_k)$

1. initialization

During T=0, for i=1 ..., N generates and obeys prior distribution q (x ₀) N sampling particle { x ₀ ¹, x ₀ ², L, x ₀ ⁿ, and with the initial weight ω of each sample ₀ ⁱAll be made as 1/N.

${\stackrel{\‾}{x}}_0^i=E\left[x_0^i\right]$

$P_0^i=E\left[(x_0^i-{\stackrel{\‾}{x}}_0^i){(x_0^i-{\stackrel{\‾}{x}}_0^i)}^T\right]$

2. for T=1,2 ..., the filtering during T=k is as follows:

(a) important sampling

Upgrade with the UKF method respectively for N particle, by { x _K-1 ⁱ, P _K-1 ⁱObtain { x _k ⁱ, P _k ⁱ, and gather a new sampled point Make ${\hat{x}}_k^i~q\left(x_k^i\right|x_{k-1}^i,Z_k)=N({\stackrel{\‾}{x}}_k^i,{\stackrel{\‾}{P}}_k^i).$

According to the similarity degree refreshing weight of sample, make ${\stackrel{\‾}{\mathrm{\ω}}}_k^i={\mathrm{\ω}}_{k-1}^i\frac{p\left(Z_k\right|{\hat{x}}_k^i)p\left({\hat{x}}_k^i\right|x_{k-1}^i)}{q\left({\hat{x}}_k^i\right|x_{k-1}^i,Z_{k-1})}.$

Weights to each particle carry out normalization then, even ${\mathrm{\ω}}_k^i=\frac{1}{{\mathrm{\Σ}}_{j=1}^N{\stackrel{\‾}{\mathrm{\ω}}}_k^i}.$

Calculate the size N of effective particle at last _Eff, $N_{\mathrm{eff}}=\frac{1}{{\mathrm{\Σ}}_{i=1}^N{\left({\mathrm{\ω}}_k^i\right)}^2}.$ If N _EffLess than threshold value N _Th, illustrate that the particle diversity reduces, and need change step (b) over to and resample, otherwise change the filtering result that step (c) is exported this time over to.

(b) resample

The purpose that resamples is to eliminate the less particles of weights, increases the bigger particle of weights, makes the distribution of the sample set after the resampling meet posterior density p (x _k| Z _k).The weights ω of each sample during resampling _k ⁱBe re-set as 1/N.

(c) result's output

Use weights omega _k ⁱMultiply by particle (x respectively _k ⁱ, P _k ⁱ) obtain N random particles sequence.

The state estimation value is $x_k=\underset{i}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}_k^i{x}_k^i.$

State estimation error battle array is $P_k=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}_k^i{P}_k^i=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}_k^i(x_k^i-{\hat{x}}_k^i){(x_k^i-{\hat{x}}_k^i)}^{\′}.$

The UPF algorithm utilizes UKF to obtain the importance sampling density function of particle filter, thereby the particle filter that has overcome standard does not consider that up-to-date measurement information and UKF can only be applied to the deficiency that noise is a Gaussian distribution.Simulation result shows that this method can obtain the filtering accuracy than the particle filter of standard and UKF filtering convergence faster and Geng Gao.

The content that is not described in detail in the instructions of the present invention belongs to this area professional and technical personnel's known prior art.

Claims (3)

1, a kind of deep space probe UPF celestial self-navigation method based on the starlight angular distance is characterized in that may further comprise the steps:

(1) set up the state equation of deep space probe on transfer orbit, i.e. many track bodies kinetics equation,

${\stackrel{\·\·}{r}}_{\mathrm{ps}}=-{\mathrm{\μ}}_s\frac{r_{\mathrm{ps}}}{r_{\mathrm{ps}}^3}-{\mathrm{\μ}}_m[\frac{r_{\mathrm{pm}}}{r_{\mathrm{pm}}^3}-\frac{r_{\mathrm{sm}}}{r_{\mathrm{sm}}^3}]-{\mathrm{\μ}}_e[\frac{r_{\mathrm{pe}}}{r_{\mathrm{pe}}^3}-\frac{r_{\mathrm{se}}}{r_{\mathrm{se}}^3}]$

In the formula, r _PsFor day the heart to the radius vector of detector;

Be r _PsSecond derivative; r _PmBe the radius vector of planet to detector; r _PeBe the radius vector of the earth to detector; r _SeBe the radius vector of the earth's core to day heart; r _SmBe the radius vector of planet center to day heart; μ _s, μ _m, μ _eBe respectively solar gravitation constant, planetary gravitation constant and terrestrial gravitation constant;

(2) set up the measurement equation of system as the measurement amount with the starlight angular distance between the sun, the earth, planet and the fixed star;

(3) adopt the UPF filtering algorithm to improve the navigation accuracy of deep space probe.

2, the UPF celestial self-navigation method of a kind of deep space probe based on the starlight angular distance according to claim 1, it is characterized in that: the measurement equation of setting up system as the measurement amount with the starlight angular distance described in the step (2) for the direction vector of three navigation fixed star starlights observing from deep space probe respectively and the angle between the direction vector of the sun, the earth and the planet centre of sphere, expression formula is as follows:

$\left\{\begin{array}{c}{\mathrm{\θ}}_s=\arccos (-\frac{r_{\mathrm{ps}}\·s_1}{r_{\mathrm{ps}}})\\ {\mathrm{\θ}}_e=\arccos (-\frac{r_{\mathrm{pe}}\·s_2}{r_{\mathrm{pe}}})\\ {\mathrm{\θ}}_m=\arccos (-\frac{r_{\mathrm{pm}}\·s_3}{r_{\mathrm{pm}}})\end{array}\right.$

In the formula, s ₁, s ₂, s ₃Be respectively the unit vector of fixed star starlight direction, discern by star sensor.

3, the UPF celestial self-navigation method of a kind of deep space probe based on the starlight angular distance according to claim 1, it is characterized in that: the method that the employing UPF filtering algorithm described in the step (3) improves the navigation accuracy of deep space probe is: utilize UKF to obtain the important density function of particle, determine one group of sigma point according to previous moment particle and variance thereof, the position of this point set and weights are unique definite by the expectation and the variance of particle; With described sigma point difference substitution state equation, obtain a new point set, with the weighted sum of these point sets as expectation, with the weighted sum of its variance as variance, with measuring equation the expectation and the variance of having tried to achieve are revised again, and with expectation and the variance of revised value, produce the particle of a current time as Gaussian distribution.

Images