CN103148856A - Swing-by probe autonomous celestial navigation method based on adaptive scale change - Google Patents

Abstract

The invention relates to a swing-by probe autonomous celestial navigation method based on adaptive scale change. The method comprises the following steps: establishing a deep space probe state model and an autonomous celestial navigation system measurement model; acquiring measurement quantity of the autonomous celestial navigation system, and employing an adaptive scale change Unscented Kalman filtering method, so that the time scale suitable for the current model is obtained, and the position and speed of the detector in an inertial coordinate system which takes a target object as the center are obtained; applying the obtained time scale, position and speed to navigation at the next moment. The method belongs to the technical field of aerospace navigation, the time scale can be adaptively changed along with the time change according to the gravity acceleration stressed by the probe, the navigation system model error caused by the fixed time scale time update is reduced, and the method is suitable for a swing-by deep space probe.

Description

A kind of swing-by flight detector celestial self-navigation method that changes based on adaptive scale

Technical field

The present invention relates to when the deep space probe swing-by flight autonomous navigation method that changes based on the model trajectory adaptive scale, is a kind of autonomous navigation method that is highly suitable for the deep space probe swing-by flight stage.

Background technology

The survey of deep space technology has caused the very big concern of countries in the world as key character and the sign of a national overall national strength and scientific technological advance level.Prelude has been pulled open in the fight of new round survey of deep space.At the beginning of 21 century, each spacefaring nation focuses to sight one after another apart from 380,000 kilometers deep space universe in addition of the earth.The U.S., European Space Agency, Russia, Japan and India have all proposed following survey of deep space plan in the interior world main space flight group, carry out manned or based on the unmanned probing of robot to each major planet and satellite thereof.Along with the development of China's carrier rocket and other survey of deep space technology and the raising of economic strength, China has possessed the ability of surveying the even farther planets of the solar system of Mars.

Along with the expansion of investigative range and the increase of detection range, the required emitted energy of deep space probe also significantly increases, and this is the maximum predicament that the survey of deep space task faces.In seeking the process of head it off, the swing-by flight technology be owing to can effectively reduce emitted energy and the required speed increment of detection mission, and becomes one of technology that is most widely used in the survey of deep space task.Swing-by flight refers to utilize the second gravitation body to change the energy of detector relative center gravitation body, thereby change size or the direction of detector speed, to save emitted energy, with less initial velocity, in the situation that without any the acceleration of power consumption realization to detector, complete detection mission, even realize the detection mission that only depends on present carrier rocket to complete.

Although swing-by trajectory requires lower to the carrier rocket emitted energy, but owing to needing to borrow power to celestial body in the detector flight course, and during the power of borrowing, detector present position and travelling speed there is strict index request, therefore require the detector navigational system that in real time accurate navigation information must be able to be provided, if navigation information is untimely or not accurate enough, detector will miss the best opportunity that the navigation celestial body is borrowed power, can't carry out correct speed correction, even stray finally causes mission failure.Therefore the navigation accuracy in swing-by flight stage is the important guarantee that the swing-by flight Mission Success is implemented.

Be subjected to the impact of dynamics of orbits model accuracy in the navigation accuracy in deep space probe swing-by flight stage.In the independent navigation process, need to carry out integration to gravitational acceleration and obtain speed and position.Therefore one of them key factor that affects the dynamics of orbits model accuracy is exactly the integration step of dynamics of orbits.

Existing autonomous navigation system all adopts fixedly integration step to carry out dynamics of orbits model integration, and this method has the following disadvantages: the fixedly integration step method that existing autonomous navigation system adopts is thought of as normal value with the celestial body gravitation acceleration in an integrating range.But in the swing-by flight survey of deep space task of actual motion, due to along with detector and Mars move closer to, detector is subject to Mars gravitation and increases gradually, and the gravitational acceleration of Mars is not normal value, and temporal evolution is larger.The existing fixed step size integration method Mars gravitational acceleration that temporal evolution is larger is thought of as normal value in an integrating range, therefore will introduce the dynamics of orbits model error, and this error increases with the increase of integration step.When autonomous navigation system was set less integration step, in earlier stage because the detector motion temporal evolution is slow, this less integration step calculated amount was large, has wasted computational resource on the detector star at swing-by flight; When autonomous navigation system was set larger integration step, navigation accuracy was low when swing-by flight, and the gravitation that can't follow the tracks of central body changes.

Summary of the invention

The technical problem to be solved in the present invention is: overcome existing fixed step size integration method large in swing-by flight calculated amount in early stage, in the low problem of swing-by flight stage navigation accuracy, a kind of autonomous navigation method of high-efficiency high-accuracy is provided for the swing-by flight deep space probe.

The technical solution adopted for the present invention to solve the technical problems is: set up centered by target celestial body in inertial coordinates system the deep space probe state model based on the sun and the eight major planets of the solar system gravitation, obtain angle information measurement amount between target celestial body and satellite and fixed star by the optical guidance sensor, set up the measurement model of autonomous navigation system, and under existing time scale, use adaptive scale to change the Unscented kalman filter method, obtain on the one hand position and the speed parameter of the relative target celestial body of deep space probe; According to the result of Orbit simulation, return to suitable time scale on the other hand, adopt the time scale of gained to upgrade in the time renewal process.Realization is large at model trajectory temporal evolution hours yardstick, and the time yardstick is little when the model trajectory temporal evolution is fast, and the navigational parameters such as high precision position, speed finally are provided efficiently for the swing-by flight deep space probe.

Specifically comprise the following steps:

1. set up deep space probe based on the sun and the dynamic (dynamical) state model of the eight major planets of the solar system Attractive Orbit

Set up deep space probe in inertial coordinates system centered by the target celestial body barycenter based on the sun and the dynamic (dynamical) state model of the eight major planets of the solar system Attractive Orbit, i.e. the state model of autonomous astronomical navigation system;

X′(t)＝f ₁(X′(t),t)+w′(t) (5)

In formula, X ' (t)=[x ', y ', z ', v ' _x, v ' _y, v ' _z] ^TBe t state vector constantly, x ', y ', z ', v ' _x, v ' _y, v ' _zBe respectively position and the speed of detector three axles in target celestial body barycenter inertial coordinates system, f ₁(X ' (t), t) be the non-linear continuous state transfer function of system, w ' (t)=[0,0,0, w ' _x, w ' _y, w ' _z] ^TBe t state model error constantly, w ' _x, w ' _y, w ' _zIt is the model error of three axle speed differential.

2. set up the autonomous astronomical navigation system measurements model based on the starlight angular moment

The angle information θ of target celestial body and two satellites and three background fixed stars _1i, θ _2iAnd θ _3i(i=1,2,3) expression formula is:

$\left\{\begin{array}{c}{\mathrm{\θ}}_{1i}=\arccos (-{\stackrel{\→}{l}}_{p1}^I{\stackrel{\→}{s}}_{1i}^I)\\ {\mathrm{\θ}}_{2i}=\arccos (-{\stackrel{\→}{l}}_{p2}^I\·{\stackrel{\→}{s}}_{2i}^I)\\ {\mathrm{\θ}}_{3i}=\arccos (-{\stackrel{\→}{l}}_{p3}^I\·{\stackrel{\→}{s}}_{3i}^I)\end{array}\right.---\left(6\right)$

In formula,

Be the unit vector of target celestial body in inertial coordinates system to detector, Unit vector for i fixed star in target celestial body image in inertial coordinates system;

Be first satellite of target celestial body in inertial coordinates system unit vector to detector,

Unit vector for i fixed star in first satellite image of target celestial body in inertial coordinates system;

Be the unit vector of second satellite of target celestial body in inertial coordinates system to detector,

Unit vector for i fixed star in second satellite image of target celestial body in inertial coordinates system.

The expression formula that can set up autonomous astronomical navigation system measurements model according to formula (6) is:

Z′(t)＝h ₁[X(t),t]+v ₁(t) (7)

In formula, h ₁[X ' (t), t] is the non-linear continuous measurement function of autonomous astronomical navigation system, Z ' (t)=[θ _11,θ ₁₂, θ ₁₃, θ ₂₁, θ ₂₂, θ ₂₃, θ ₃₁, θ ₃₂, θ ₃₃] ^TFor t autonomous astronomical navigation system quantities is constantly measured, $v_1={[v_{{\mathrm{\θ}}_{11}},v_{{\mathrm{\θ}}_{12}},v_{{\mathrm{\θ}}_{13}},v_{{\mathrm{\θ}}_{21}},v_{{\mathrm{\θ}}_{22}},v_{{\mathrm{\θ}}_{23}},v_{{\mathrm{\θ}}_{31}},v_{{\mathrm{\θ}}_{31}},v_{{\mathrm{\θ}}_{32}},v_{{\mathrm{\θ}}_{33}}]}^T$ Be autonomous astronomical navigation system measurements noise,

Be respectively and measure θ ₁₁, θ ₁₂, θ ₁₃, θ ₂₁, θ ₂₂, θ ₂₃, θ ₃₁, θ ₃₂, θ ₃₃Observational error.

3. the state model in step 1 and step 2 and measurement model are carried out discretize

X′(k)＝F ₁(X′(k-1),k-1)+W′(k) (8)

Z′(k)＝H ₁(X′(k),k)+V ₁(k) (9)

In formula, k=1,2 ..., F ₁(X ' (k-1), k-1) be f ₁(X ' (t) is carved into k nonlinear state transfer function constantly, H during from k-1 after t) discrete ₁(X ' (k), k) be h ₁(X ' (t), t) discrete after k non-linear measurement function constantly, W ' (k), V ₁(k) be respectively w ' (t), v ₁(t) k equivalent noise constantly after discrete, and W ' is (k) and V ₁(k) uncorrelated mutually.

4. obtain the measurement amount by the autonomous astronomical navigation sensor

Obtained the image of target celestial body by the autonomous astronomical navigation sensor, utilize image processing techniques, determine that the pixel of celestial body barycenter is as line coordinates; Through being tied to the two-dimensional imaging plane coordinate system, being tied to from the two-dimensional imaging planimetric coordinates three conversions that sensor is measured coordinate system as line coordinates from pixel, determine celestial body and the unit vector of background fixed star in the sensor coordinate system thereof; Calculate at last the starlight angular distance between celestial body unit vector and background fixed star unit vector.

5. the autonomous astronomical navigation system is carried out adaptive scale and change the Unscented Kalman filtering

According to state model and the measurement model of the autonomous astronomical navigation system discrete form that is obtained by step 1～step 3 and obtained the measurement amount that obtained by the celestial navigation sensor by step 4, carry out autonomous astronomical navigation system self-adaption dimensional variation Unscented Kalman filtering.Concrete steps are as follows:

A. the state vector X ' of initialization the n+1 time filtering autonomous astronomical navigation system (n) and the time scale h of the n+1 time filtering autonomous astronomical navigation system _n, the corresponding moment of the n time filtering

N=0,1,2 ..., wherein X ' (n)=

When n=0,

${\hat{x}}_0^{\′}=E\left[x_0^{\′}\right],P_0^{\′}=E\left[(x_0^{\′}-{\hat{x}}_0^{\′}){(x_0^{\′}-{\hat{x}}_0^{\′})}^T\right],h_n=h_0---\left(10\right)$ In formula,

Be three shaft positions and the speed initial value of initial time detector in target celestial body barycenter inertial coordinates system, x ' ₀Be three shaft positions of initial time detector in target celestial body barycenter inertial coordinates system and the estimated value of speed, P ' ₀Be the initial square error battle array of state vector, h _nBe the time scale that the n+1 time filtering is used, h ₀Time scale for initial time filtering.When n ≠ 0,

${\hat{x}}_n^{\′}=E\left[x_n^{\′}\right],P_n^{\′}=E\left[(x_n^{\′}-{\hat{x}}_n^{\′}){(x_n^{\′}-{\hat{x}}_n^{\′})}^T\right],h_n=h_{n-1}---\left(11\right)$

In formula,

Be three shaft positions of the n+1 time filtering detector in target celestial body barycenter inertial coordinates system and the initial value of speed, x ' _nBe three shaft positions and the velocity estimation value of the n time filtering detector in target celestial body barycenter inertial coordinates system, P ' _nThe initial square error battle array of state vector when being the n+1 time filtering, h _nBe the time scale that the n+1 time filtering is used, h _n-1Be the time scale that the n time filtering is used, the corresponding moment of the n time filtering

B. utilize state vector and time scale after the steps A initialization to carry out the adjustment of time scale self-adaptation

A) the right function K of 12 interlude points in dimension self-adaption adjustment process computing time _Stepi

K _step1＝F ₁(X′(n),n) (12)

$K_{\mathrm{stepi}}=F_1(X^{\′}\left({n+a}_{\mathrm{stepi}}h\right)+h\underset{\mathrm{stepj}=0}{\overset{\mathrm{stepi}-1}{\mathrm{\Σ}}}{b}_{\mathrm{stepi},\mathrm{stepj}}{K}_{\mathrm{stepj}},{n+a}_{\mathrm{stpi}}h,),---\left(13\right)$

In formula, X ' (n) by the n+1 time filtering correspondence initial state vector constantly, F ₁(X ' (n) is n) that nonlinear state from the n time filtering to the n+1 time filtering shifts discrete function, stepi=2,3..., 12, b wherein _{Stepi, stepj}Nonzero value be: b _1,0=2/27, b _2,0=1/36, b _2,1=1/12, b _3,0=1/24, b _3,2=1/8, b _4,0=5/12, b _4,2=-25/16, b _4,3=25/16, b _5,0=1/20, b _5,3=1/4, b _5,4=1/5, b _6,0=-25/108, b _6,3=125/108, b _6,4=-65/27, b _6,5=125/54, b _7,0=31/300, b _7,4=61/225, b _7,5=-2/9, b _7,6=13/900, b _8,0=2b _8,3=-53/6, b _8,4=704/45, b _8,5=-107/9, b _8,6=67/90, b _9,0=-91/108, b _9,3=23/108, b _9,4=-976/135, b _9,5=311/54, b _9,6=-19/60, b _9,7=17/6, b _9,8=-1/12, b _10,0=2383/4100, b _10,3=-341/164, b _10,4=4496/1024, b _10,5=-301/82, b _10,6=2133/4100, b _10,7=45/82, b _10,8=45/162, b _10,9=18/41, b _11,0=3/205, b _11,5=-6/41, b _11,6=-3/205, b _11,7=-3/41, b _11,8=3/41, b _11,9=6/41, b _12,0=-1777/4100, b _12,5=-289/82, b _12,6=2193/4100, b _12,7=51/82, b _12,8=33/164, b _12,9=12/41, b _12,11=1

, a _Step0=0, a _Step1=2/27, a _Step2=1/9, a _Step3=1/6, a _Step4=5/12, a _Step5=1/2, a _Step6=5/6, a _Step7=1/6, a _Step8=2/3, a _Step9=1/3, a _Step10=1, a _Step11=0, a _Step12=1, X ' (n)=

, h=h _n

B) 7 rank in dimension self-adaption adjustment process computing time and 8 rank predicted state vectors

$X_{n+1}^{\′}=X_n^{\′}+h\underset{\mathrm{stepi}=0}{\overset{10}{\mathrm{\Σ}}}{c}_{\mathrm{stepi}}{K}_{\mathrm{stepi}}---\left(14\right)$

${\tilde{X}}_{n+1}^{\′}=X_n^{\′}+h\underset{\mathrm{stepi}=0}{\overset{12}{\mathrm{\Σ}}}{\tilde{c}}_{\mathrm{stepi}}{K}_{\mathrm{stepi}}---\left(15\right)$

In formula, X ' _n+1Be 7 rank predicted state vectors in time scale self-adaptation adjustment process,

Be 8 rank predicted state vectors in time scale self-adaptation adjustment process, c _Step0=41/840, c _Step1=0, c _Step2=0, c _Step3=0, c _Step4=0, c _Step5=34/105, c _Step6=9/35, c _Step7=9/35, c _Step8=9/280, c _Step9=9/280, c _Step10=41/840, ${\tilde{c}}_{\mathrm{step}0}=0,{\tilde{c}}_{\mathrm{step}1}=0,{\tilde{c}}_{\mathrm{step}2}=0,{\tilde{c}}_{\mathrm{step}3}=0,{\tilde{c}}_{\mathrm{step}4}=0,{\tilde{c}}_{\mathrm{step}5}=034/105,{\tilde{c}}_{\mathrm{step}6}=9/35,$ ${\tilde{c}}_{\mathrm{step}7}=9/35,{\tilde{c}}_{\mathrm{step}8}=9/280,{\tilde{c}}_{\mathrm{step}9}=9/280,{\tilde{c}}_{\mathrm{step}10}=0,{\tilde{c}}_{\mathrm{step}11}=41/840,{\tilde{c}}_{\mathrm{step}12}=41/840;$

C) the local truncation error Δ of computing mode vector

${\mathrm{\Δ}~X}_{n+1}-{\tilde{X}}_{n+1}~\frac{41}{810}(K_0+K_{10}-K_{11}{K}_{12})h---\left(16\right)$

D) judge whether the step-length that adaptive scale changes the Unscented Kalman filtering needs to adjust

If Δ〉1e-16, adjust step-length h=h/2, return to step a); Otherwise carry out step C;

C. calculate the sampled point that adaptive scale changes the Unscented Kalman filtering

Constantly corresponding in the n+1 time filtering institute of autonomous astronomical navigation system

State vector

Near choose a series of sample points, the average of these sample points and square error battle array are respectively

With If state vector is 6 * 1 dimension, 13 autonomous astronomical navigation systematic sample point χ ' so _{0, n}..., χ ' _{12, n}And weights W ₀' ... W ' ₁₂As follows respectively:

${\mathrm{\χ}}_{0,n}^{\′}={\hat{x}}_n^{\′},W_0^{\′}=-1$

${\mathrm{\χ}}_{j,n}^{\′}={\hat{x}}_n^{\′}+\sqrt{3}{\left(\sqrt{P_n^{\′}}\right)}_j,W_j^{\′}=1/6---\left(17\right)$

${\mathrm{\χ}}_{j+6,n}^{\′}={\hat{x}}_n^{\′}-\sqrt{3}{\left(\sqrt{P_n^{\′}}\right)}_j,W_{j+6}^{\′}=1/6$

In formula, as P ' _n=A ' ^TA ' time,

The j that gets A ' is capable, as P ' _n=A ' A ' ^TThe time,

Get the j row of A ', sampled point χ ' when getting the n+1 time filtering _nUniform expression be:

D. the time upgrades

According to the step-length h of step B gained, to all sampled point χ ' of step C gained _nThe time of carrying out upgrades, and obtains the one-step prediction of autonomous astronomical navigation system state vector

Estimate square error battle array one-step prediction

Measure estimate vector

K _step1＝F ₁(χ′ _n,n) (19)

$K_{\mathrm{stepi}}=F_1({\mathrm{\χ}}_{n+a_{\mathrm{stepi}}h}^{\′}+h\underset{\mathrm{stepj}=0}{\overset{\mathrm{stepi}-1}{\mathrm{\Σ}}}{b}_{\mathrm{stepi},\mathrm{stepj}}{K}_{\mathrm{stepj}},n+a_{\mathrm{stepi}}h,)---\left(20\right)$

${\mathrm{\χ}}_{n+1|n}^{\′}={\mathrm{\χ}}_n^{\′}+h\underset{\mathrm{stepi}=0}{\overset{10}{\mathrm{\Σ}}}{c}_{\mathrm{stepi}}{K}_{\mathrm{stepi}}---\left(21\right)$

χ′ _n+1|n＝F ₁(χ′ _n,n) (22)

Result after the one-step prediction weighting of all sampled point state vectors of autonomous astronomical navigation system

For:

${\hat{x}}_{n+1}^{\′-}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j^{\′}{{\mathrm{\χ}}^{\′}}_{j,n+1|n}---\left(23\right)$

In formula, W _j' be the weights of j sampled point;

The estimation square error battle array one-step prediction of autonomous astronomical navigation system state vector

For:

$P_{n+1}^{\′-}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j^{\′}[{\mathrm{\χ}}_{j,n+1|n}^{\′}-{\hat{x}}_{n+1}^{\′-}]{[{\mathrm{\χ}}_{j,n+1|n}^{\′}-{\hat{x}}_{n+1}^{\′-}]}^T+Q_{n+1}^{\′}---\left(24\right)$

In formula, Q ' _n+1The state model error covariance matrix of autonomous astronomical navigation system when being the n+1 time filtering;

The measurement estimate vector Z ' that autonomous astronomical navigation systematic sampling point is corresponding _N+1|n

Z′ _n+1|n＝H ₁(χ′ _n+1|n,n+1) (25)

All sampled points of autonomous astronomical navigation system measure estimates weighing vector

${\hat{z}}_{n+1}^{\′-}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j^{\′}{Z}_{j,n+1|n}^{\′}---\left(26\right)$

E. measure and upgrade

Measure according to step D gained and estimate weighing vector

Result after the one-step prediction weighting of state vector Measurement square error battle array, state vector measurement amount square error battle array, filter gain, estimated state vector, estimation square error battle array are upgraded:

Autonomous astronomical navigation system measurements square error battle array

For:

$P_{{\hat{z}}_{n+1}{\hat{z}}_{n+1}}^{\′}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j^{\′}[Z_{j,n+1|n}^{\′}-{\hat{z}}_{n+1}^{\′-}]{[Z_{j,n+1|n}^{\′}-{\hat{z}}_{n+1}^{\′-}]}^T+R_{n+1}^{\′}---\left(27\right)$

In formula, R ' _n+1The measurement noise covariance matrix of autonomous astronomical navigation system when being the n+1 time filtering;

Autonomous astronomical navigation system state vector quantity is measured the square error battle array

:

$P_{{\hat{x}}_{n+1}{\hat{z}}_{n+1}}^{\′}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j^{\′}[{\mathrm{\χ}}_{j,n+1|n}^{\′}-{\hat{x}}_{n+1}^{\′-}]{[Z_{j,n+1|n}^{\′}-{\hat{z}}_{n+1}^{\′-}]}^T---\left(28\right)$

Autonomous astronomical navigation system filter gain K ' _n+1For:

$K_{n+1}^{\′}=P_{{\hat{x}}_{n+1}{\hat{z}}_{n+1}}^{\′}{P}_{{\hat{z}}_{n+1}{\hat{z}}_{n+1}}^{\′-1}---\left(29\right)$

The estimated state vector of autonomous astronomical different boat system

With estimation square error battle array P ' _n+1:

${\hat{x}}_{n+1}^{\′}={\hat{x}}_{n+1}^{\′-}+K_{n+1}^{\′}(Z_{n+1}^{\′}-{\hat{z}}_{n+1}^{\′-})---\left(30\right)$

$P_{n+1}^{\′}=P_{n+1}^{\′-}-K_{n+1}^{\′}{P}_{{\hat{z}}_{n+1}{\hat{z}}_{n+1}}^{\′}{K}_{n+1}^{\′T}---\left(31\right)$

The most at last formula (30) and formula (31) obtain the n+1 time filtering the time vectorial at the estimated state of target celestial body barycenter inertial coordinates system

With estimation square error battle array P ' _n+1Output, the estimated state vector

Be illustrated in position, the velocity information of detector in target celestial body barycenter inertial coordinates system, the estimation square error battle array P ' of output _n+1Represented the performance that filtering is estimated, and these navigation informations have been returned to respectively in the autonomous astronomical navigation system position, speed navigation information when being used for the n+2 time filtering.

Principle of the present invention is: at first according to the dynamics of orbits of the sun and the eight major planets of the solar system gravitation, set up System State Model in the inertial coordinates system centered by target celestial body; Then, set up the measurement model of autonomous astronomical navigation system, afterwards according to the principle of work of celestial navigation sensor, the measurement amount that the celestial navigation sensor that obtains and process is measured; After this, because state model and the measurement model of detector all presents nonlinear characteristic, and system noise is non-Gaussian noise, therefore adopts the Unscented kalman filter method, and the detector navigation information of two sub-systems is estimated; Secondly, because System State Model is fast in detector swing-by flight phase change.If adopt larger set time yardstick solving state error in equation large, be difficult to realize the high precision swing-by flight, if adopt less set time yardstick to find the solution, swing-by flight assesses the cost too large in earlier stage, has wasted the resource on the star.Therefore adopt the Unscented kalman filter method of auto-adaptive time dimensional variation, obtain on the one hand high precision position and the speed parameter of the relative target celestial body of deep space probe; According to the result of Orbit simulation, return to suitable time scale on the other hand, adopt the time scale of gained to upgrade in the time renewal process.Realization is large at model trajectory temporal evolution hours yardstick, and the time yardstick is little when the model trajectory temporal evolution is fast, and the navigational parameters such as high precision position, speed finally are provided efficiently for the swing-by flight deep space probe.

The present invention's advantage compared with prior art is: the Unscented kalman filter method that adopts adaptive scale to change, in the time renewal process, according to the result of Orbit simulation, return to suitable time scale.Realization is large at model trajectory temporal evolution hours yardstick, and the time yardstick is little when the model trajectory temporal evolution is fast, and the navigational parameters such as high precision position, speed finally are provided efficiently for the swing-by flight deep space probe

Description of drawings

Fig. 1 the present invention is based on the autonomous astronomical navigation process flow diagram that adaptive scale changes the Unscented Kalman filtering;

Fig. 2 is the imaging schematic diagram of autonomous astronomical navigation system of the present invention celestial navigation sensor used;

Fig. 3 is the process flow diagram that in the present invention, the time scale self-adaptation is adjusted.

Embodiment

As shown in Figure 1, in the aforementioned techniques scheme, the related power celestial body of borrowing can be the interplanetary planets such as Mars, Venus, Jupiter, Saturn, below with Mars as embodiment, specific implementation process of the present invention is described:

1. set up deep space probe based on the sun and the dynamic (dynamical) state model of the eight major planets of the solar system Attractive Orbit

At first initialized location, speed, establish X '=[x ' y ' z ' v _x' v _y' v _z'] ^TBe the state vector in fiery heart inertial coordinates system, x ', y ', z ', v ' _x, v ' _y, v ' _zBe respectively position and the speed of detector three axles in fiery heart inertial coordinates system, above-mentioned variable is all the function relevant with t, and according to the Track desigh of detector, position and the speed initial value of choosing detector are X ' (0).

Set up deep space probe based on the sun and the dynamic (dynamical) state model of the eight major planets of the solar system Attractive Orbit in fiery heart inertial coordinates system, consider that the sun such as the sun and Mars, the earth and the eight major planets of the solar system are to the graviational interaction of detector, choose fiery heart inertial coordinates system, can get the state model of deep space probe in fiery heart inertial coordinates system and be:

$\left\{\begin{array}{c}{\stackrel{\·}{x}}^{\′}=v_x^{\′}\\ {\stackrel{\·}{y}}^{\′}=v_y^{\′}\\ {\stackrel{\·}{z}}^{\′}=v_z^{\′}\\ {\stackrel{\·}{v}}_x^{\′}=-{\mathrm{\μ}}_m\frac{x^{\′}}{r_{\mathrm{pm}}^{\′3}}-{\mathrm{\μ}}_s[\frac{x^{\′}-x_s^{\′}}{r_{\mathrm{ps}}^{\′3}}+\frac{x_s^{\′}}{r_{\mathrm{ms}}^{\′3}}]-\underset{i_c}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\μ}}_{i_c}[\frac{x^{\′}-x_{i_c}^{\′}}{r_{{\mathrm{pi}}_c}^{\′3}}+\frac{x_{i_c}^{\′}}{r_{{\mathrm{mi}}_c}^3}]+w_x^{\′}\\ {\stackrel{\·}{v}}_y^{\′}=-{\mathrm{\μ}}_m\frac{y^{\′}}{r_{\mathrm{pm}}^{\′3}}-{\mathrm{\μ}}_s[\frac{y^{\′}-y_s^{\′}}{r_{\mathrm{ps}}^{\′3}}+\frac{y_s^{\′}}{r_{\mathrm{ms}}^{\′3}}]-\underset{i_c}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\μ}}_{i_c}[\frac{y^{\′}-y_{i_c}^{\′}}{r_{{\mathrm{pi}}_c}^{\′3}}+\frac{y_{i_c}^{\′}}{r_{{\mathrm{mi}}_c}^3}]+w_y^{\′}\\ {\stackrel{\·}{v}}_z^{\′}=-{\mathrm{\μ}}_m\frac{z^{\′}}{r_{\mathrm{pm}}^{\′3}}-{\mathrm{\μ}}_s[\frac{z^{\′}-z_s^{\′}}{r_{\mathrm{ps}}^{\′3}}+\frac{z_s^{\′}}{r_{\mathrm{ms}}^{\′3}}]-\underset{i_c}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\μ}}_{i_c}[\frac{z^{\′}-z_{i_c}^{\′}}{r_{{\mathrm{pi}}_c}^{\′3}}+\frac{z_{i_c}^{\′}}{r_{{\mathrm{mi}}_c}^3}]+w_z^{\′}\end{array}\right.---\left(1\right)$

In formula, x ', y ', z ' are detector three shaft positions in fiery heart inertial coordinates system, v ' _x, v ' _y, v ' _zBe detector three axle speed in fiery heart inertial coordinates system,

Be the differential of detector three shaft positions in fiery heart inertial coordinates system,

Be the differential of detector three axle speed in fiery heart inertial coordinates system, μ _s, μ _mWith

Be respectively the sun, Mars and i _cThe gravitational constant of planet; r′ _psFor day the heart to the distance of detector; r′ _pmBe the distance of Mars to detector; r′ _msFor day the heart to the distance of the fiery heart;

Be i _cPlanet is to the distance of detector;

Be i _cPlanet barycenter is to the distance of the fiery heart; (x ' _s, y ' _s, z ' _s),

Be respectively the sun, i _cThe three shaft position coordinates of planet in fiery heart inertial coordinates system can be obtained according to the time w by planet ephemerides _x', w _y', w _z' be respectively the state model error of detector three axle speed differential in state model; i _cRepresent i from the inside to the outside in the sun and the eight major planets of the solar system _cPlanet, i _c=1,2,3..., N (i _c≠ 4), N=8 is due to i _c=4 expression the 4th planets (Mars) therefore are not included in the summation expression formula.

Each variable in formula (1) is all the variable relevant with time t, can be abbreviated as

${\stackrel{\·}{X}}^{\′}\left(t\right)=f_1(X^{\′}\left(t\right),t)+w^{\′}\left(t\right)---\left(2\right)$

In formula, X ' (t)=[x ', y ', z ', v ' _x, v ' _y, v ' _z] ^TBe the state vector of t autonomous astronomical navigation System State Model constantly,

(t) be X ' differential (t), f ₁(X ' (t), t) be the non-linear continuous state transfer function of system of state model, w ' (t)=[0,0,0, w ' _x, w ' _y, w ' _z] ^TBe t state model error constantly, w ' _x, w ' _y, w ' _zIt is the model error of three axle speed differential.

2. set up the autonomous astronomical navigation system measurements model based on the starlight angular distance;

Angle information θ between Mars and i background fixed star _1iSize to measure coordinate system at the Mars sensor consistent with expression formula in fiery heart inertial coordinates system, can be expressed as:

${\mathrm{\θ}}_{1i}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pm}}^S\·{\stackrel{\→}{s}}_{1i}^S)=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pm}}^I\·{\stackrel{\→}{s}}_{1i}^I)---\left(3\right)$

In formula,

Be the unit vector from Mars to detector in Mars sensor measurement coordinate system,

Be the unit vector of Mars in inertial coordinates system to detector, can be expressed as:

${\stackrel{\→}{l}}_{\mathrm{pm}}^I=\frac{1}{\sqrt{x^{\′2}+y^{\′2}+z^{\′2}}}\left[\begin{array}{c}{x}^{\′}\\ y^{\′}\\ z^{\′}\end{array}\right]---\left(4\right)$

In formula, (x ', y ', z ') be detector three shaft position coordinates in fiery heart inertial coordinates system,

Be the unit vector of i background fixed star in Mars sensor measurement coordinate system in the Mars image,

Be the unit vector of i background fixed star starlight in inertial coordinates system, i=1,2,3, can directly be obtained by fixed star almanac data storehouse,

In like manner can get angle information θ between phobos and Deimos and its i background fixed star _2iAnd θ _3iExpression formula be:

${\mathrm{\θ}}_{2i}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pp}}^I\·{\stackrel{\→}{s}}_{2i}^I)---\left(5\right)$

${\mathrm{\θ}}_{3i}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pd}}^I\·{\stackrel{\→}{s}}_{3i}^I)---\left(6\right)$

The expression formula that can set up the autonomous astronomical navigation measurement model according to formula (4)～formula (6) is

Z′(t)＝h ₁[X′(t),t]+v ₁(t) (7)

In formula, Z ' (t)=[θ ₁₁, θ ₁₂, θ ₁₃, θ ₂₁, θ ₂₂, θ ₂₃, θ ₃₁, θ ₃₂, θ ₃₃] ^TFor t autonomous astronomical navigation system quantities is constantly measured, θ ₁₁, θ ₁₂, θ ₁₃, θ ₂₁, θ ₂₂, θ ₂₃, θ ₃₁, θ ₃₂, θ ₃₃Be respectively the angle information of target celestial body and two satellites and three background fixed stars, h ₁[X ' (t), t] be the non-linear continuous measurement function of autonomous astronomical navigation,

Be autonomous astronomical navigation system measurements noise,

Be respectively and measure θ ₁₁, θ ₁₂, θ ₁₃, θ ₂₁, θ ₂₂, θ ₂₃, θ ₃₁, θ ₃₂, θ ₃₃Observational error.

3. the state model in step 1 and step 2 and measurement model are carried out discretize

X′(k)＝F ₁(X′(k-1),k-1)+W′(k) (8)

Z′(k)＝H ₁(X′(k),k)+V ₁(k) (9)

In formula, k=1,2 ..., F ₁(X ' (k-1), k-1) be f ₁(X ' (t) is carved into k nonlinear state transfer function constantly, H during from k-1 after t) discrete ₁(X ' (k), k) be h ₁(X ' (t), t) discrete after k non-linear measurement function constantly, W ' (k), V ₁(k) be w ' (t), v ₁(t) k equivalent noise constantly after discrete, and W ' is (k) and V ₁(k) uncorrelated mutually.

4. obtain the measurement amount by the autonomous astronomical navigation sensor

Fig. 2 has provided the imaging schematic diagram of autonomous astronomical navigation system Mars sensor used, and phobos sensor, Deimos sensor imaging process are similar with it.The Mars sensor mainly is comprised of optical lens and two-dimensional imaging face battle array, measures coordinate system O ' X at sensor _cY _cZ _cMiddle direction vector along Mars to detector

Mars sunlight reflection directive Mars sensor, at this moment, the coordinate that Mars is measured in coordinate system at the Mars sensor is (x _c, y _c, z _c); The optical lens of Mars sensor is imaged on two-dimensional imaging face battle array after with the light refraction of focal distance f with Mars, and the image brightness signal that two-dimensional imaging face battle array will impinge upon on each image-generating unit stores.According to the image-forming principle of sensor, the processing procedure that the autonomous astronomical navigation system quantities is measured is as follows:

A. the processing of celestial image

Because the image of Mars on two-dimensional imaging face battle array is not a point, but a circle determines that by image processing techniquess such as barycenter identifications the Mars image centroid is O at pixel as line coordinates _plX _plY _plIn pixel as line (p, l).

B. center-of-mass coordinate is the conversion that is converted to the two-dimensional imaging plane coordinate system as line coordinates from pixel

As the relation between line coordinates system and two-dimensional imaging plane coordinate system, be to be converted to two-dimensional imaging plane coordinate system from pixel as line coordinates with the Mars center-of-mass coordinate according to pixel:

$\left[\begin{array}{c}{x}_{2d}\\ y_{2d}\end{array}\right]=K^{-1}(\left[\begin{array}{c}p\\ l\end{array}\right]-\left[\begin{array}{c}{p}_0\\ l_0\end{array}\right])---\left(10\right)$

In formula, (x _2d, y _2d) be that the Mars barycenter is at two-dimensional imaging plane OX _2dY _2dIn coordinate, p and l are respectively Mars at the pixel on Mars sensor two-dimensional imaging plane and picture line, $K=\left[\begin{array}{cc}{K}_x& K_{\mathrm{yx}}\\ K_{\mathrm{xy}}& K_y\end{array}\right]$ The millimeter of serving as reasons transfers the sensor transition matrix of pixel, p to ₀And l ₀Being respectively Mars sensor center is OX at pixel as line coordinates _plY _plIn pixel and the picture line.

C. center-of-mass coordinate is converted to from the two-dimensional imaging plane coordinate system conversion that sensor is measured coordinate system

According to the lens imaging principle, the Mars center-of-mass coordinate is converted to sensor from the two-dimensional imaging plane coordinate system measures coordinate system:

${\stackrel{\→}{l}}_{\mathrm{pm}}^S=\begin{array}{c}\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]\end{array}=\frac{1}{\sqrt{x_{2d}^2+y_{2d}^2+f^2}}\left[\begin{array}{c}{x}_{2d}\\ y_{2d}\\ -f\end{array}\right]---(11)$

In formula, (x _c, y _c, z _c) be that the Mars barycenter is measured coordinate system O ' X at the Mars sensor _cY _cZ _cIn coordinate, f is the focal length of Mars sensor,

For measure the unit vector from Mars to detector in coordinate system at the Mars sensor.

In like manner can obtain the unit vector of i background fixed star in Mars sensor measurement coordinate system in the Mars image

I=1,2,3.

D. calculate the starlight angular distance

According to measuring in coordinate system Mars at the Mars sensor to the unit vector of detector

Unit vector with i background fixed star in the Mars image

Calculate the starlight angular distance θ between two vectors _1i

${\mathrm{\θ}}_{1i}=\arccos [(-{\stackrel{\→}{l}}_{\mathrm{pm}}^S)\·{\stackrel{\→}{s}}_{1i}^S]---\left(12\right)$

In like manner can obtain the starlight angular distance θ between phobos and its background fixed star, Deimos and its background fixed star _2i, θ _3i

5. the autonomous astronomical navigation system is carried out adaptive scale and change the Unscented Kalman filtering

According to state model formula (8), autonomous astronomical navigation system measurements modular form (9), by the measurement amount formula (12) that the celestial navigation sensor obtains, carry out autonomous astronomical navigation system self-adaption dimensional variation Unscented Kalman filtering.Concrete steps are as follows:

A. the state vector X ' of initialization the n+1 time filtering autonomous astronomical navigation system (n) and the time scale h of the n+1 time filtering autonomous astronomical navigation system _n, the corresponding moment of the n time filtering

Wherein

When n=0,

${\hat{x}}_0^{\′}=E\left[x_0^{\′}\right],P_0^{\′}=E\left[(x_0^{\′}-{\hat{x}}_0^{\′}){(x_0^{\′}-{\hat{x}}_0^{\′})}^T\right],h_n=h_0---\left(13\right)$

In formula,

Be three shaft positions and the speed initial value of initial time detector in fiery heart inertial coordinates system, x ' ₀Be three shaft positions and the speed actual value of initial time detector in fiery heart inertial coordinates system, P ₀' be the initial square error battle array of state vector, h _nBe the time scale that filtering is used, h ₀Time scale for initial time filtering.

When n ≠ 0,

${\hat{x}}_n^{\′}=E\left[x_n^{\′}\right],P_n^{\′}=E\left[(x_n^{\′}-{\hat{x}}_n^{\′}){(x_n^{\′}-{\hat{x}}_n^{\′})}^T\right],h_n=h_{n-1}---\left(14\right)$

In formula,

Be three shaft positions of the n+1 time filtering detector in fiery heart inertial coordinates system and the initial value of speed, x ' _nBe three shaft positions and the velocity estimation value of the n time filtering detector in fiery heart inertial coordinates system, P ' _nThe initial square error battle array of state vector when being the n+1 time filtering, h _nBe the time scale that the n+1 time filtering is used, h _n-1Be the time scale that the n time filtering is used, the corresponding moment of the n time filtering

B. utilize state vector and time scale after the steps A initialization to carry out the adjustment of time scale self-adaptation, process flow diagram as shown in Figure 3;

A) the right function K of beans-and bullets shooter in the middle of 12 in dimension self-adaption adjustment process computing time _Stepi

K _step1＝F ₁(X′(n),n) (15)

$K_{\mathrm{stepi}}=F_1(X^{\′}(n+a_{\mathrm{stepi}}h)+h\underset{\mathrm{stepj}=1}{\overset{\mathrm{stepi}-1}{\mathrm{\Σ}}}{b}_{\mathrm{stepi},\mathrm{stepj}}{K}_{\mathrm{stepj}},n+a_{\mathrm{stepi}}h,)---\left(16\right)$

In formula, X ' (n) by the n+1 time filtering correspondence initial state vector constantly, F ₁(X ' (n) is n) that nonlinear state from the n time filtering to the n+1 time filtering shifts discrete function, stepi=2,3..., 12, b wherein _{Stepi, stepj}Nonzero value be: b _1,0=2/27, b _2,0=1/36, b _2,1=1/12, b _3,0=1/24, b _3,2=1/8, b _4,0=5/12, b _4,2=-25/16, b _4,3=25/16, b _5,0=1/20, b _5,3=1/4, b _5,4=1/5, b _6,0=-25/108, b _6,3=125/108, b _6,4=-65/27, b _6,5=125/54, b _7,0=31/300, b _7,4=61/225, b _7,5=-2/9, b _7,6=13/900, b _8,0=2b _8,3=-53/6, b _8,4=704/45, b _8,5=-107/9, b _8,6=67/90, b _9,0=-91/108, b _9,3=23/108, b _9,4=-976/135, b _9,5=311/54, b _9,6=-19/60, b _9,7=17/6, b _9,8=-1/12, b _10,0=2383/4100, b _10,3=-341/164, b _10,4=4496/1024, b _10,5=-301/82, b _10,6=2133/4100, b _10,7=45/82, b _10,8=45/162, b _10,9=18/41, b _11,0=3/205, b _11,5=-6/41, b _11,6=-3/205, b _11,7=-3/41, b _11,8=3/41, b _11,9=6/41, b _12,0=-1777/4100, b _12,5=-289/82, b _12,6=2193/4100, b _12,7=51/82, b _12,8=33/164, b _12,9=12/41, b _12,11=1

a _Step0=0, a _Step1=2/27, a _Step2=1/9, a _Step3=1/6, a _Step4=5/12, a _Step5=1/2, a _Step6=5/6, a _Step7=1/6, a _Step8=2/3, a _Step9=1/3, a _Step10=1, a _Step11=0, a _Step12=1,

h=h _n

B) 7 rank in dimension self-adaption adjustment process computing time and 8 rank predicted state vectors

$X_{n+1}^{\′}=X_n^{\′}+h\underset{\mathrm{stepi}=0}{\overset{10}{\mathrm{\Σ}}}{c}_{\mathrm{stepi}}{K}_{\mathrm{stepi}}---\left(17\right)$

${\tilde{X}}_{n+1}^{\′}=X_n^{\′}+h\underset{\mathrm{stepi}=0}{\overset{12}{\mathrm{\Σ}}}{\tilde{c}}_{\mathrm{stepi}}{K}_{\mathrm{stepi}}---\left(18\right)$

In formula, X ' _n+1Be 7 rank predicted state vectors in time scale self-adaptation adjustment process,

Be 8 rank predicted state vectors in time scale self-adaptation adjustment process, c _Step0=41/840, c _Step1=0, c _Step2=0, c _Step3=0, c _Step4=0, c _Step5=34/105, c _Step6=9/35, c _Step7=9/35, c _Step8=9/280, c _Step9=9/280, c _Step10=41/840, ${\tilde{c}}_{\mathrm{step}0}=0,{\tilde{c}}_{\mathrm{step}1}=0,{\tilde{c}}_{\mathrm{step}2}=0,{\tilde{c}}_{\mathrm{step}3}=0,{\tilde{c}}_{\mathrm{step}4}=0,{\tilde{c}}_{\mathrm{step}5}=34/105,{\tilde{c}}_{\mathrm{step}6}=9/35,$ ${\tilde{c}}_{\mathrm{step}7}=9/35,{\tilde{c}}_{\mathrm{step}8}=9/280,{\tilde{c}}_{\mathrm{step}9}=9/280,{\tilde{c}}_{\mathrm{step}10}=0,{\tilde{c}}_{\mathrm{step}11}=41/840,{\tilde{c}}_{\mathrm{step}12}=41/840;$

C) the local truncation error Δ of computing mode vector

$\mathrm{\Δ}~X_{n+1}-{\tilde{X}}_{n+1}~\frac{41}{810}(K_0+K_{10}-K_{11}-K_{12})h---\left(19\right)$

D) judge whether the step-length that adaptive scale changes the Unscented Kalman filtering needs to adjust

If Δ〉1e-16, adjust step-length h=h/2, return to step a); Otherwise carry out step C

C. calculate the sampled point that adaptive scale changes the Unscented Kalman filtering

Constantly corresponding in the n+1 time filtering institute of autonomous astronomical navigation system

State vector

Near choose a series of sample points, the average of these sample points and square error battle array are respectively

And P ' _nIf state vector is 6 * 1 dimension, 13 autonomous astronomical navigation systematic sample point χ ' so _{0, n}..., χ ' _{12, n}And weights W ' ₀W ' ₁₂As follows respectively:

${\mathrm{\χ}}_{0,n}^{\′}={\hat{x}}_n^{\′},W_0^{\′}=-1$

${\mathrm{\χ}}_{j,n}^{\′}={\hat{x}}_n^{\′}+\sqrt{3}{\left(\sqrt{P_n^{\′}}\right)}_j,W_j^{\′}=1/6---\left(20\right)$

${\mathrm{\χ}}_{j+6,n}^{\′}={\hat{x}}_n^{\′}-\sqrt{3}{\left(\sqrt{P_n^{\′}}\right)}_j,W_{j+6}^{\′}=1/6$

In formula, as P ' _n=A ' ^TA ' time, The j that gets A ' is capable, as P ' _n=A ' A ' ^TThe time,

Get the j row of A ', sampled point χ ' when getting the n+1 time filtering _nUniform expression be:

D. the time upgrades

According to the step-length h of step B gained, to all sampled point χ ' of step C gained _nThe time of carrying out upgrades, and obtains the one-step prediction of autonomous astronomical navigation system state vector Estimate square error battle array one-step prediction

Measure estimate vector

$K_{\mathrm{step}1}=F_1({\mathrm{\χ}}_n^{\′},n)---\left(22\right)$

$K_{\mathrm{stepi}}=F_1({\mathrm{\χ}}_{n+a_{\mathrm{stepi}}h}^{\′}+h\underset{\mathrm{stepj}=0}{\overset{\mathrm{stepi}-1}{\mathrm{\Σ}}}{b}_{\mathrm{stepi},\mathrm{stepj}}{K}_{\mathrm{stepj}},n+a_{\mathrm{stepi}}h,),---\left(23\right)$

${\mathrm{\χ}}_{n+1|n}^{\′}={\mathrm{\χ}}_n^{\′}+h\underset{\mathrm{stepi}=0}{\overset{10}{\mathrm{\Σ}}}{c}_{\mathrm{stepi}}{K}_{\mathrm{stepi}}---\left(24\right)$

χ′ _n+1|n＝F ₁(χ′ _n,n) (25)

Result after the one-step prediction weighting of all sampled point state vectors of autonomous astronomical navigation system

For:

${\hat{x}}_{n+1}^{\′-}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j^{\′}{\mathrm{\χ}}_{j,n+1|n}^{\′}---\left(26\right)$

In formula, W _j' be the weights of j sampled point;

The estimation square error battle array one-step prediction of autonomous astronomical navigation system state vector

For:

$P_{n+1}^{\′-}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j^{\′}[{\mathrm{\χ}}_{j,n+1|n}^{\′}-{\widehat{\mathrm{\χ}}}_{n+1}^{\′-}]{[{\mathrm{\χ}}_{j,n+1|n}^{\′}-{\hat{x}}_{n+1}^{\′-}]}^T+Q_{n+1}^{\′}---\left(27\right)$

In formula, Q ' _n+1The state model error covariance matrix of autonomous astronomical navigation system when being the n+1 time filtering;

The measurement estimate vector Z ' that autonomous astronomical navigation systematic sampling point is corresponding _N+1|n

Z′ _n+1|n＝H ₁(χ′ _n+1|n,n+1) (28)

All sampled points of autonomous astronomical navigation system measure estimates weighing vector

${\hat{z}}_{n+1}^{\′-}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j^{\′}{Z}_{j,n+1|n}^{\′}---\left(29\right)$

E. measure and upgrade

Measure according to step D gained and estimate weighing vector

, state vector the one-step prediction weighting after result

Measurement square error battle array, state vector measurement amount square error battle array, filter gain, estimated state vector, estimation square error battle array are upgraded:

Autonomous astronomical navigation system measurements square error battle array

For:

$P_{{\hat{z}}_{n+1}{\hat{z}}_{n+1}}^{\′}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j^{\′}[Z_{j,n+1|n}^{\′}-{\hat{z}}_{n+1}^{\′-}]{[Z_{j,n+1|n}^{\′}-{\hat{z}}_{n+1}^{\′-}]}^T+R_{n+1}^{\′}---\left(30\right)$

In formula, R ' _n+1The measurement noise covariance matrix of autonomous astronomical different course system when being the n+1 subwave;

Autonomous astronomical navigation system state vector quantity is measured the square error battle array

$P_{{\hat{x}}_{n+1}{\hat{z}}_{n+1}}^{\′}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j^{\′}[{\mathrm{\χ}}_{j,n+1|n}^{\′}-{\hat{x}}_{n+1}^{\′-}]{[Z_{j,n+1|n}^{\′}-{\hat{z}}_{n+1}^{\′-}]}^T---\left(31\right)$

Autonomous astronomical navigation system filter gain K ' _n+1For:

$K_{n+1}^{\′}=P_{{\hat{x}}_{n+1}{\hat{z}}_{n+1}}^{\′}{P}_{{\hat{z}}_{n+1}{\hat{z}}_{n+1}}^{\′-1}---\left(32\right)$

The estimated state vector of autonomous astronomical navigation system

With estimation square error battle array P ' _n+1For

${\hat{x}}_{n+1}^{\′}={\hat{x}}_{n+1}^{\′-}+K_{n+1}^{\′}(Z_{n+1}^{\′}-{\hat{z}}_{n+1}^{\′-})---\left(33\right)$

$P_{n+1}^{\′}=P_{n+1}^{\′-}-K_{n+1}^{\′}{P}_{{\hat{z}}_{n+1}{\hat{z}}_{n+1}}^{\′}{K}_{n+1}^{\′T}---\left(34\right)$

The most at last formula (33) and formula (34) obtain the n+1 time filtering the time estimated state in fiery heart inertial coordinates system vectorial

With estimation square error battle array P ' _n+1Output, the estimated state vector Be illustrated in position, the velocity information of detector in fiery heart inertial coordinates system, the estimation square error battle array P ' of output _n+1Represented the performance that filtering is estimated, and these navigation informations have been returned to respectively in the autonomous astronomical navigation system position, speed navigation information when being used for the n+2 time filtering.

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

Claims (1)

1. swing-by flight detector celestial self-navigation method that changes based on adaptive scale, it is characterized in that: the state model of model deep space probe and measurement model, utilize the measurement amount of the relative target celestial body of autonomous astronomical navigation system acquisition, change Unscented filtering by adaptive scale and estimate to obtain the detector position in inertial coordinates system and speed centered by target celestial body, specifically comprise the following steps:

1. set up deep space probe based on the sun and the eight major planets of the solar system Attractive Orbit dynamics state model;

The state model of deep space probe in target celestial body barycenter inertial coordinates system is:

X′(t)＝f ₁(X′(t),t)+w′(t)

In formula, X ' (t)=[x ', y ', z ', v ' _x, v ' _y, v ' _z] ^TBe t autonomous astronomical navigation system state vector constantly,

(t) be X ' differential (t), x ', y ', z ', v ' _x, v ' _y, v ' _zBe respectively detector at position and the speed of three axles in target celestial body barycenter inertial coordinates system, f ₁(X ' (t), t) be the non-linear continuous state transfer function of system, w ' (t)=[0,0,0, w ' _x, w ' _y, w ' _z] ^TBe t state model error constantly, w ' _x, w ' _y, w ' _zIt is the model error of three axle speed differential;

2. set up the autonomous astronomical navigation system measurements model based on the starlight angular moment;

Z′(t)＝h ₁[X(t),t]+v ₁(t) (1)

In formula, Z ' (t)=[θ ₁₁, θ ₁₂, θ ₁₃, θ ₂₁, θ ₂₂, θ ₂₃, θ ₃₁, θ ₃₂, θ ₃₃] ^TFor t autonomous astronomical navigation system quantities is constantly measured, θ ₁₁, θ ₁₂, θ ₁₃, θ ₂₁, θ ₂₂, θ ₂₃, θ ₃₁, θ ₃₂, θ ₃₃Be respectively the angle information of target celestial body and two satellites and three background fixed stars, h ₁[X ' (t), t] be the non-linear continuous measurement function of autonomous astronomical navigation,

Be autonomous astronomical navigation system measurements noise,

Be respectively and measure θ ₁₁, θ ₁₂, θ ₁₃, θ ₂₁, θ ₂₂, θ ₂₃, θ ₃₁, θ ₃₂, θ ₃₃Observational error;

3. 1. state model and the measurement model in 2. carries out discretize with step to step, obtains state model and the measurement model of autonomous astronomical navigation system discrete form;

X′(k)＝F ₁(X′(k-1),k-1)+W′(k)

Z′(k)＝H ₁(X′(k),k)+V ₁(k)

In formula, k=1,2 ..., F ₁(X ' (k-1), k-1) be f ₁(X ' (t) is carved into k nonlinear state transfer function constantly, H during from k-1 after t) discrete ₁(X ' (k), k) be h ₁(X ' (t), t) discrete after k non-linear measurement function constantly, W ' (k), V ₁(k) be respectively w ' (t), v ₁(t) k equivalent noise constantly after discrete, and W ' is (k) and V ₁(k) uncorrelated mutually;

4. obtain the measurement amount by the autonomous astronomical navigation sensor;

5. according to state model and the measurement model of the autonomous astronomical navigation system discrete form that is 3. obtained by step and 4. obtained the measurement amount that obtained by the celestial navigation sensor by step, carry out autonomous astronomical navigation system self-adaption dimensional variation Unscented Kalman filtering, concrete steps are as follows:

A. the state vector of initialization the n+1 time filtering autonomous astronomical navigation system

Time scale h with the n+1 time filtering _n, the corresponding moment of the n time filtering

N=0,1,2 ..., wherein X ' (n)=

B. utilize state vector and time scale after the steps A initialization to carry out the adjustment of time scale self-adaptation;

A) the right function K of 12 interlude points in dimension self-adaption adjustment process computing time _Stepi

K _step1＝F ₁(X′(n),n)

$K_{\mathrm{stepi}}=F_1(X^{\′}(n+a_{\mathrm{stepi}}h)+h\underset{\mathrm{stepj}=0}{\overset{\mathrm{stepi}-1}{\mathrm{\Σ}}}{b}_{\mathrm{stepi},\mathrm{stepj}}{K}_{\mathrm{stepj}},n+a_{\mathrm{stepi}}h,)$

In formula, X ' (n) by the n+1 time filtering correspondence initial state vector constantly, F ₁(X ' (n) is n) that nonlinear state from the n time filtering to the n+1 time filtering shifts discrete function, stepi=2,3..., 12, b wherein _{Stepi, stepj}Nonzero value be: b _1,0=2/27, b _2,0=1/36, b _2,1=1/12, b _3,0=1/24, b _3,2=1/8, b _4,0=5/12, b _4,2=-25/16, b _4,3=25/16, b _5,0=1/20, b _5,3=1/4, b _5,4=1/5, b _6,0=-25/108, b _6,3=125/108, b _6,4=-65/27, b _6,5=125/54, b _7,0=31/300, b _7,4=61/225, b _7,5=-2/9, b _7,6=13/900, b _8,0=2b _8,3=-53/6, b _8,4=704/45, b _8,5=-107/9, b _8,6=67/90, b _9,0=-91/108, b _9,3=23/108, b _9,4=-976/135, b _9,5=311/54, b _9,6=-19/60, b _9,7=17/6, b _9,8=-1/12, b _10,0=2383/4100, b _10,3=-341/164, b _10,4=4496/1024, b _10,5=-301/82, b _10,6=2133/4100, b _10,7=45/82, b _10,8=45/162, b _10,9=18/41, b _11,0=3/205, b _11,5=-6/41, b _11,6=-3/205, b _11,7=-3/41, b _11,8=3/41, b _11,9=6/41, b _12,0=-1777/4100, b _12,5=-289/82, b _12,6=2193/4100, b _12,7=51/82, b _12,8=33/164, b _12,9=12/41, b _12,11=1

a _Step0=0, a _Step1=2/27, a _Step2=1/9, a _Step3=1/6, a _Step4=5/12, a _Step5=1/2, a _Step6=5/6, a _Step7=1/6, a _Step8=2/3, a _Step9=1/3, a _Step10=1, a _Step11=0, a _Step12=1, X ' (n)=

', h=h _n

B) 7 rank in dimension self-adaption adjustment process computing time and 8 rank predicted state vectors

$X_{n+1}^{\′}=X_n^{\′}+h\underset{\mathrm{stepi}=0}{\overset{10}{\mathrm{\Σ}}}{c}_{\mathrm{stepi}}{K}_{\mathrm{stepi}}---\left(2\right)$

${\tilde{X}}_{n+1}^{\′}=X_n^{\′}+h\underset{\mathrm{stepi}=0}{\overset{12}{\mathrm{\Σ}}}{\tilde{c}}_{\mathrm{stepi}}{K}_{\mathrm{stepi}}---\left(3\right)$

In formula, X ' _n+1Be 7 rank predicted state vectors in time scale self-adaptation adjustment process,

Be 8 rank predicted state vector c in time scale self-adaptation adjustment process _Step0=41/840, c _Step1=0, c _Step2=0, c _Step3=0, c _Step4=0, c _Step5=34/105, c _Step6=9/35, c _Step7=9/35, c _Step8=9/280, c _Step9=9/280, c _Step10=41/840, ${\tilde{c}}_{\mathrm{step}0}=0,{\tilde{c}}_{\mathrm{step}1}={0,\tilde{c}}_{\mathrm{step}2}=0,{\tilde{c}}_{\mathrm{step}3}=0,{\tilde{c}}_{\mathrm{step}4}=0,{\tilde{c}}_{\mathrm{step}5}=34/105,{\tilde{c}}_{\mathrm{step}6}=9/35,$ ${\tilde{c}}_{\mathrm{step}7}=9/35,{\tilde{c}}_{\mathrm{step}8}=9/280,{\tilde{c}}_{\mathrm{step}9}=9/280,{\tilde{c}}_{\mathrm{step}10}=0,{\tilde{c}}_{\mathrm{step}11}=41/840,{\tilde{c}}_{\mathrm{step}12}=41/840;$

C) the local truncation error Δ of computing mode vector

$\mathrm{\Δ}~X_{n+1}-{\tilde{X}}_{n+1}~\frac{41}{810}(K_0+K_{10}-K_{11}-K_{12})h---\left(4\right)$

D) judge whether the step-length that adaptive scale changes the Unscented Kalman filtering needs to adjust

If Δ〉1e-16, adjust step-length h=h/2, return to step a); Otherwise carry out step C;

C. calculate the sampled point that adaptive scale changes the Unscented Kalman filtering;

Constantly corresponding in the n+1 time filtering institute of autonomous astronomical navigation system

State vector

Near choose a series of sample points, the average of these sample points and square error battle array are respectively

And P ' _nIf state vector is 6 * 1 dimension, 13 autonomous astronomical navigation systematic sample point χ ' so _{0, n}..., χ ' _{12, n}And weights W ' ₀W ' ₁₂As follows respectively:

${\mathrm{\χ}}_{0,n}^{\′}={\hat{x}}_n^{\′},W_0^{\′}=-1$

${\mathrm{\χ}}_{j,n}^{\′}={\hat{x}}_n^{\′}+\sqrt{3}{\left(\sqrt{P_n^{\′}}\right)}_j,W_j^{\′}=1/6$

${\mathrm{\χ}}_{j+6,n}^{\′}={\hat{x}}_n^{\′}+\sqrt{3}{\left(\sqrt{P_n^{\′}}\right)}_j,W_{j+6}^{\′}=1/6$

In formula, as P ' _n=A ' ^TA ' time,

The j that gets A ' is capable, as P ' _n=A ' A ' ^TThe time,

Get the j row of A ', sampled point χ ' when getting the n+1 time filtering _nUniform expression be:

D. the time upgrades;

According to the step-length h of step B gained, to all sampled point χ ' of step C gained _nThe time of carrying out upgrades, and obtains the one-step prediction of autonomous astronomical navigation system state vector Estimate square error battle array one-step prediction

Measure estimate vector

K _step1＝F ₁(χ′ _n,n)

$K_{\mathrm{stepi}}=F_1({\mathrm{\χ}}_{{n+a}_{\mathrm{stepi}}h}^{\′}+h\underset{\mathrm{stepj}=0}{\overset{\mathrm{stepi}-1}{\mathrm{\Σ}}}{b}_{\mathrm{stepi},\mathrm{stepj}}{K}_{\mathrm{stepj}},n+a_{\mathrm{stepi}}h,)$

${\mathrm{\χ}}_{n+1|n}^{\′}={\mathrm{\χ}}_n^{\′}+h\underset{\mathrm{stepi}=0}{\overset{10}{\mathrm{\Σ}}}{c}_{\mathrm{stepi}}{K}_{\mathrm{stepi}}$

χ′ _n+1|n＝F ₁(χ′ _n,n)

Result after the one-step prediction weighting of all sampled point state vectors of autonomous astronomical navigation system

For:

${\hat{x}}_{n+1}^{\′-}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j^{\′}{\mathrm{\χ}}_{j,n+1|n}^{\′}$

In formula, W _j' be the weights of j sampled point;

The estimation square error battle array one-step prediction of autonomous astronomical navigation system state vector For:

$P_{n+1}^{\′-}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j^{\′}[{\mathrm{\χ}}_{j,n+1|n}^{\′}-{\hat{x}}_{n+1}^{\′-}]{[{\mathrm{\χ}}_{j,n+1|n}^{\′}-{\hat{x}}_{n+1}^{\′-}]}^T+Q_{n+1}^{\′}$

In formula, Q ' _n+1The state model error covariance matrix of autonomous astronomical navigation system when being the n+1 time filtering;

The measurement estimate vector Z ' that autonomous astronomical navigation systematic sampling point is corresponding _N+1|n

Z′ _n+1|n＝H ₁(χ′ _n+1|n,n+1)

All sampled points of autonomous astronomical navigation system measure estimates weighing vector

${\hat{z}}_{n+1}^{\′-}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j^{\′}{Z}_{j,n+1|n}^{\′}$

E. measure and upgrade;

Measure according to step D gained and estimate weighing vector

Result after the one-step prediction weighting of state vector

Measurement square error battle array, state vector measurement amount square error battle array, filter gain, estimated state vector, estimation square error battle array are upgraded;

Autonomous astronomical navigation system measurements square error battle array

For:

$P_{{\hat{z}}_{n+1}{\hat{z}}_{n+1}}^{\′}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j^{\′}[Z_{j,n+1|n}^{\′}-{\hat{z}}_{n+1}^{\′-}]{[Z_{j,n+1|n}^{\′}-{\hat{z}}_{n+1}^{\′-}]}^T{R}_{n+1}^{\′}$

In formula, R ' _n+1The measurement noise covariance matrix of autonomous astronomical navigation system when being the n+1 time filtering;

Autonomous astronomical navigation system state vector quantity is measured the square error battle array

$P_{{\hat{x}}_{n+1}{\hat{z}}_{n+1}}^{\′}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j^{\′}[{\mathrm{\χ}}_{j,n+1|n}^{\′}-{\hat{x}}_{n+1}^{\′-}]{[Z_{j,n+1|n}^{\′}-{\hat{z}}_{n+1}^{\′-}]}^T$

Autonomous astronomical navigation system filter gain K ' _n+1For:

$K_{n+1}^{\′}=P_{{\hat{x}}_{n+1}{\hat{z}}_{n+1}}^{\′}{P}_{{\hat{z}}_{n+1}{\hat{z}}_{n+1}}^{\′-1}$

The estimated state vector of autonomous astronomical navigation system

With estimation square error battle array P ' _n+1For

${\hat{x}}_{n+1}^{\′}={\hat{x}}_{n+1}^{\′-}+K_{n+1}^{\′}(Z_{n+1}^{\′}-{\hat{z}}_{n+1}^{\′-})$

$P_{n+1}^{\′}=P_{n+1}^{\′-}-K_{n+1}^{\′}{P}_{{\hat{z}}_{n+1}{\hat{z}}_{n+1}}^{\′}{K}_{n+1}^{\′T}$

Estimated state vector during the final the n+1 time filtering that obtains in fiery heart inertial coordinates system

With estimation square error battle array P ' _n+1Output, the estimated state vector

Be illustrated in position, the velocity information of detector in fiery heart inertial coordinates system, the estimation square error battle array P ' of output _n+1Represented the performance that filtering is estimated, and these navigation informations have been returned to respectively in the autonomous astronomical navigation system position, speed navigation information when being used for the n+2 time filtering.

Images