CN103017773A - Surrounding road navigation method based on celestial body surface feature and natural satellite road sign - Google Patents

Abstract

The invention discloses a surrounding road navigation method based on a celestial body surface feature and a natural satellite road sign. The surrounding road navigation method comprises the following steps of: in a navigation process, performing inertial attitude estimation by a gyro and a star sensor; extrapolating a position and speed of a detector by a kinetic equation according to measurement of an accelerometer and an inertial attitude estimated value; then shooting a plurality of road signs by a navigation camera according to a certain distance and a sequence, and identifying image coordinates of road sign points, wherein known landscapes of the surface of a deep celestial body and natural satellites of the celestial body are in the selectable range of the road sign points, and making an observation sequence according to different rail heights and illumination conditions; estimating and correcting the position and the speed of the detector by expanded Kalman filtering by taking the image coordinates of the road sign points as the measurement based on the extrapolated position and the speed of the detector; and repeating the steps, thus finishing navigation calculation. By the surrounding road navigation method, the range of selecting the road sign points of optical navigation of a surrounding road is expanded; and the flexibility and the precision of a navigation system are improved.

Description

A kind of based on celestial body surface characteristics and natural satellite road sign around the section air navigation aid

Technical field

The present invention relates to a kind of based on celestial body surface characteristics and natural satellite road sign around the section air navigation aid, belong to survey of deep space independent navigation field.

Background technology

Deep sky object is important means long-term to target celestial body, that approach and observe comprehensively around detection.Be subject to carrying the restriction of propellant total amount, the deep sky object detector can not directly be braked and enter the circuit orbit around target celestial body, but operate on the highly elliptic orbit after arriving at target celestial body through interplanetary flight.Because highly elliptic orbit is subjected to the impact of astronomical perturbation comparatively complicated, so the track of detector determines that (navigation) becomes deep space around an important technology of surveying.

So far, the external large celestial body of deep space determines that around the track of detector also main ground observing and controlling net or the Deep Space Network of relying on carries out.The cost of this method and cost are very large, and the global layout of tracking telemetry and command station, measuring accuracy etc. have been proposed very high requirement, and real-time also is difficult for guaranteeing.Independent navigation then is a kind of good technological means that remedies these defectives.But the independent navigation of survey of deep space is also only verified in transfer leg or landing phase at present.

But on the method aspect, both at home and abroad the researchist has still carried out a large amount of research to deep space around the autonomous navigation technology of section.Comprise the method that relies on target celestial body apparent radius and direction of visual lines to measure, and the method take celestial body surface characteristics landform as road sign etc.But all there are various defectives in these methods, are difficult to engineering and use.For example, the method that based target sky stereoscopic radius and direction of visual lines are measured depends on the optical sensor of large visual field, and it can only be applied on the higher circular surround orbit of orbit altitude, and is subject to easily the impact of target celestial body illumination condition.And be applicable to the very near surround orbit segmental arc of distance objective celestial body based on the method for celestial body surface road sign point, but it depends on a large amount of, detailed target celestial body partly graphic data or image data.And this exactly the deep space target approach and lack before surveying.In addition, because the restriction of detector flight track and the impact of target celestial body weather condition are added in the relative solar motion of target celestial body, alternative celestial body surface road sign point can further reduce, and this has increased the difficulty that the method realizes greatly.

Summary of the invention

Technology of the present invention is dealt with problems: overcome the deficiencies in the prior art, often run on the characteristics of highly elliptic orbit for deep space around detector, the new method that provides the feature landform on a kind of natural satellite with target celestial body and its surface to be used as road sign.When orbit altitude is higher with the large-scale and obvious landforms in natural satellite and target celestial body surface as road sign, when orbit altitude is hanged down with celestial body part landform as road sign.Enlarge the range of choice of road sign, improved quantity of information, increased the Observable segmental arc, improved adaptive faculty and the precision of independent navigation.

The technical solution adopted in the present invention is: a kind of survey of deep space based on celestial body surface characteristics and natural satellite road sign is around the section autonomous navigation method, detector runs on the highly elliptic orbit around the target deep sky object, is equipped with IMU (comprising gyro and accelerometer), star sensor and optical navigation camera on the detector.Performing step is as follows:

(1) the quick inertia attitude of gyro and star is estimated

Utilize the angular velocity of gyro to measure in the previous sampling period that the detector inertia attitude of current time is extrapolated; Then utilize the optical axis inertia of the quick acquisition of star to point to measurement, use Kalman filtering that the error of gyro extrapolation inertia attitude is estimated; The inertia attitude of estimating with the estimated value correction gyro of error at last obtains final inertia attitude and estimates;

(2) utilize the position of accelerometer measures, speed to extrapolate

In gyro to measure, use accelerometer to obtain the measured value of the non-gravitational acceleration of detector, utilize acquired inertia attitude estimated value that it is transformed in the inertial coordinates system; Then according to the dynamics of orbits model, position and the speed of detector are extrapolated;

(3) the road sign dot information obtains

Use the optical navigation camera of installing on the detector according to certain shooting sequence possible road sign point to be taken pictures, the selection of road sign point has comprised the characteristic feature of known location on celestial body surface and the natural satellite of celestial body; Road sign point is selected in advance according to orbit altitude, illumination condition, and lists observation sequence in, selects natural satellite and the large-scale characteristic feature of target celestial body when in general track is higher, selects the local landform of celestial body when track is low; At last, extract the pixel coordinate of road sign point from navigation picture, pass to Navigation Filter;

(4) based on the detector position of road sign, the estimation of speed

Forecast based on the satisfy the need image coordinate of punctuate of detector position, the speed of extrapolation in the step (2); Road sign point pixel coordinate and predicted value that reality obtains are compared, consist of and measure new breath; Then use EKF that detector position, the speed of prediction are revised, obtain final detector position, velocity estimation value.

(5) to the step of each control cycle repetition (1)～(4), finish navigation calculation.

Described method simultaneously with celestial body surface characteristics and natural satellite all as the navigation road sign, when orbit altitude is higher than the natural satellite track take the large-scale landforms in natural satellite and celestial body surface as road sign, take the local landform of celestial body as road sign, it is variable that Navigation resolves in the process quantity of road sign point when track is lower than the natural satellite track.

The present invention's advantage compared with prior art is: the present invention also expands to the natural satellite of deep sky object the road sign point of independent navigation, thereby increased the kind of alternative road sign point, reduce the demand to target celestial body surface local feature landform priori, reduced the difficulty that road sign point was selected and determined on ground.And for deep space around detector on the section highly elliptic orbit apart from the very large characteristics of target celestial body height change, near the near point of surround orbit, mainly use a day body characteristics landform to be road sign, near around the far point of section, natural satellite is combined with the obvious landforms in celestial body surface, has improved the adaptive faculty of air navigation aid to the large oval surround orbit of deep space.

Description of drawings

Fig. 1 is that deep space is around the autonomous navigation system block diagram of segment base in road sign;

Fig. 2 be deep space take Mars as representative around section with celestial body surface characteristics and the natural satellite air navigation aid principle as road sign;

Fig. 3 is that deep space is around the selected road sign type of section different time points;

Fig. 4 is that deep space is around section navigation position error change curve;

Fig. 5 is that deep space is around section navigation speed error change curve.

Embodiment

Detector runs on the highly elliptic orbit around the target deep sky object, is equipped with IMU (comprising gyro and accelerometer), star sensor and optical navigation camera on the detector.

As shown in Figure 1, the concrete computation process of the present invention is as follows:

The first step is carried out the quick detector attitude of gyro+star and is estimated

(1) gyro to measure

If current time is t _k, last sampling instant is t _K-1, gyro can obtain at [t so _K-1, t _k] interior probe angle increment Delta g of time period ^b, subscript b represents the body coordinate system (O of detector _b-X _bY _bZ _b), this coordinate system connects firmly on detector, and initial point overlaps with the detector barycenter, and three axles are parallel to the detector principal axis of inertia.

Angle step just can obtain the detector mean angular velocity ω of this section in the time divided by sampling time interval ^bMeasured value.

${\widetilde{\mathrm{\ω}}}_k^b=\frac{{\mathrm{\Δg}}^b}{t_k-t_{k-1}}---\left(1\right)$

Subscript "～" expression measured value.

(2) attitude motion is learned extrapolation

Use hypercomplex number q=[q ₁, q ₂, q ₃, q ₄] ^TThe attitude of expression detector relative inertness system, subscript represents transposition, inertial system (O _I-X _IY _IZ _I) initial point at the target celestial body barycenter, three coordinate axis are pointed to specific direction (when describing attitude motion, this coordinate origin being moved to the detector barycenter from the target celestial body barycenter) in inertial space.Attitude motion equation is so

$\stackrel{\·}{q}=\frac{1}{2}E\left(q\right){\mathrm{\ω}}^b---\left(2\right)$

Wherein,

$E\left(q\right)=\left[\begin{array}{ccc}{q}_4& {-q}_3& q_2\\ q_3& q_4& -q_1\\ -q_2& q_1& q_4\\ -q_1& -q_2& -q_3\end{array}\right]---\left(3\right)$

Owing to contain constant value drift b in the gyro to measure ^b, suppose that the estimated value of constant value drift is And use

The attitude quaternion estimated value that the expression navigational system provides then utilizes formula (2) can carry out the extrapolation that attitude motion is learned

${\hat{q}}_{k-1}\≈{\hat{q}}_{k-1}+E\left({\hat{q}}_{k-1}\right)({\widetilde{\mathrm{\ω}}}_k^b-{\hat{b}}_k^b)(t_k-t_{k-1})---\left(4\right)$

(3) star sensor is measured

When utilizing the gyro data extrapolation, star sensor is also measured according to certain intervals.Star sensor is output as optical axis in the sensing of inertial space With

(subscript i represents that this vector is described under the inertial system, and subscript star represents star sensor), the attitude quaternion that utilizes step (2) to estimate

They are transformed in the body series of detector,

$\left\{\begin{array}{c}{\hat{X}}_{\mathrm{star}}^i=A\left({\hat{q}}_k\right){\tilde{X}}_{\mathrm{star}}^i\\ {\hat{Y}}_{\mathrm{star}}^i=A\left({\hat{q}}_k\right){\tilde{Y}}_{\mathrm{star}}^i\\ {\hat{Z}}_{\mathrm{star}}^i=A\left({\hat{q}}_k\right){\tilde{Z}}_{\mathrm{star}}^i\end{array}\right.---\left(5\right)$

Wherein A (q) expression is converted to attitude matrix with attitude quaternion, has

$A\left(q\right)=\left[\begin{array}{ccc}{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HSA00000817707000021.png,HSA00000817707000031.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HSA00000817707000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-{\mathrm{<figure-callout\; id="3"\; label="q"\; filenames="HSA00000817707000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_3^2+q_4^2& 2(q_1{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HSA00000817707000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_3{q}_4)& 2(q_1{q}_3-q_2{q}_4)\\ 2(q_1{q}_2-q_3{q}_4)& -{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HSA00000817707000021.png,HSA00000817707000031.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2+{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HSA00000817707000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-{\mathrm{<figure-callout\; id="3"\; label="q"\; filenames="HSA00000817707000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_3^2+q_4^2& 2(q_2{\mathrm{<figure-callout\; id="3"\; label="q"\; filenames="HSA00000817707000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_3+q_1{q}_4)\\ 2(q_1{\mathrm{<figure-callout\; id="3"\; label="q"\; filenames="HSA00000817707000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_3+q_2{q}_4)& 2(q_2{q}_3-q_1{q}_4)& -{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HSA00000817707000021.png,HSA00000817707000031.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HSA00000817707000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2+{\mathrm{<figure-callout\; id="3"\; label="q"\; filenames="HSA00000817707000021.png"\; state="\{\{state\}\}">q</figure-callout>}}_3^2+q_4^2\end{array}\right]---\left(6\right)$

Utilize

With

Point to star sensor three axles that determined by the installation site of star sensor

With

Obtain the error quaternion of attitude prediction

${\mathrm{\&Delta;}\tilde{q}}_k=\left[\begin{array}{c}\mathrm{\&delta;}\tilde{q}\\ 1\end{array}\right]=\left[\begin{array}{c}\frac{1}{2}(X_{\mathrm{star}}^b\&times;{\hat{X}}_{\mathrm{star}}^b+Y_{\mathrm{star}}^b\&times;{\hat{Y}}_{\mathrm{star}}^b+Z_{\mathrm{star}}^b\&times;{\hat{Z}}_{\mathrm{star}}^b)\\ 1\end{array}\right]---\left(7\right)$

(4) Kalman filtering correction

Because the attitude prediction error quaternion is

To its both sides differentiate and with formula (2) substitution wherein, the error propagation equation that then can derive scalar part in the attitude quaternion extrapolation process is

$\mathrm{\&delta;}\stackrel{\&CenterDot;}{q}=-{\widehat{\mathrm{\&omega;}}}^b\&times;\mathrm{\&delta;q}-\frac{1}{2}{\mathrm{\&Delta;b}}^b-\frac{1}{2}{\mathrm{\&eta;}}_1^b---\left(8\right)$

In the formula

It is the random white noise of gyro to measure.And the constant value drift evaluated error of gyro can be modeled as

${\mathrm{\&Delta;}\stackrel{\&CenterDot;}{b}}^b={\mathrm{\&eta;}}_2^b---\left(9\right)$

It also is random white noise.The state equation that wave filter can be constructed in simultaneous formula (8), (9) is

$\left[\begin{array}{c}\mathrm{\&delta;}\stackrel{\&CenterDot;}{q}\\ {\mathrm{\&Delta;}\stackrel{\&CenterDot;}{b}}^b\end{array}\right]=\left[\begin{array}{cc}-[{\widehat{\mathrm{\&omega;}}}^b\&times;]& -\frac{1}{2}{\mathrm{<figure-callout\; id="3"\; label="I"\; filenames="HSA00000817707000021.png"\; state="\{\{state\}\}">I</figure-callout>}}_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right]\left[\begin{array}{c}\mathrm{\&delta;q}\\ {\mathrm{\&Delta;b}}^b\end{array}\right]+\left[\begin{array}{cc}\frac{1}{2}{\mathrm{<figure-callout\; id="3"\; label="I"\; filenames="HSA00000817707000021.png"\; state="\{\{state\}\}">I</figure-callout>}}_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& {\mathrm{<figure-callout\; id="3"\; label="I"\; filenames="HSA00000817707000021.png"\; state="\{\{state\}\}">I</figure-callout>}}_{3\&times;3}\end{array}\right]\left[\begin{array}{c}{\mathrm{\&eta;}}_1^b\\ {\mathrm{\&eta;}}_2^b\end{array}\right]---\left(10\right)$

Wherein Expression

The skew symmetry battle array, namely

$\left[{\widehat{\mathrm{\&omega;}}}^b\right]=\left[\begin{array}{ccc}0& -{\widehat{\mathrm{\&omega;}}}_z^b& {\widehat{\mathrm{\&omega;}}}_y^b\\ {\widehat{\mathrm{\&omega;}}}_z^b& 0& -{\widehat{\mathrm{\&omega;}}}_x^b\\ -{\widehat{\mathrm{\&omega;}}}_y^b& {\widehat{\mathrm{\&omega;}}}_x^b& 0\end{array}\right]$ (subscript xyz represents respectively Component in three coordinate axis of body series)

The measurement equation of wave filter is

$\mathrm{\&delta;}\tilde{q}=\left[\begin{array}{cc}{\mathrm{<figure-callout\; id="3"\; label="I"\; filenames="HSA00000817707000021.png"\; state="\{\{state\}\}">I</figure-callout>}}_{3\&times;3}& 0_{3\&times;3}\end{array}\right]\left[\begin{array}{c}\mathrm{\&delta;q}\\ {\mathrm{\&Delta;b}}^b\end{array}\right]+v_{\mathrm{star}}---\left(11\right)$

Wherein, V _StarThe measurement noise of expression star sensor, I _{3 * 3}3 * 3 unit matrix, 0 _{3 * 3}It is 3 * 3 null matrix.System for being made of formula (10) and (11) can use Kalman Filter Estimation to go out attitude error and gyroscope constant value drift error.

Make system state

$x=\left[\begin{array}{c}\mathrm{\&delta;q}\\ {\mathrm{\&Delta;b}}^b\end{array}\right]---\left(12\right)$

According to formula (10) state-transition matrix be

${\mathrm{\&Phi;}}_{k|k-1}\&ap;I+\mathrm{\&Delta;t}\left[\begin{array}{cc}-[{\widehat{\mathrm{\&omega;}}}_{k-1}^b\&times;]& -\frac{1}{2}{I}_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right]---\left(13\right)$

According to formula (11) observing matrix be

H＝[I _3×3 0 _3×3] (14)

System noise

$w=\left[\begin{array}{c}{\mathrm{\&eta;}}_1^b\\ {\mathrm{\&eta;}}_2^b\end{array}\right]---\left(15\right)$

Its variance is Q.And order

$G=\left[\begin{array}{cc}\frac{1}{2}{I}_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& I_{3\&times;3}\end{array}\right]---\left(16\right)$

Make measuring amount be

$z=\mathrm{\&delta;}\tilde{q}---\left(17\right)$

The measurement noise is

v＝v _star (18)

Its variance is R, then can use Kalman filtering to carry out state estimation, and method is as follows

x _k|k-1＝Φ _k|k-1x _k-1 (19)

$P_{k|k-1}={\mathrm{\&Phi;}}_{k|k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k|k-1}^T+G_k{Q}_k{G}_k^T---\left(20\right)$

$K_k=\left(P_{k|k-1}{H}_k^T\right){(H_k{P}_{k|k-1}{H}_k^T+R_k)}^{-1}---\left(21\right)$

x _k＝x _k|k-1+K _k(z _k-H _kx _k|k-1) (22)

$P_k=(I-K_k{H}_k)P_{k|k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T---\left(23\right)$

Obtaining state estimation

After, according to the definition of formula (12), just can obtain the estimated value of correction With

Utilizing these two estimated values just can revise attitude quaternion and gyroscopic drift estimates

${\hat{q}}_k\&LeftArrow;{\hat{q}}_k+E\left({\hat{q}}_k\right)\mathrm{\&delta;}{\hat{q}}_k---\left(1\right)$

${\hat{b}}_k^b\&LeftArrow;{\hat{b}}_k^b+{\mathrm{\&Delta;}\hat{b}}_k^b---\left(25\right)$

And, can upgrade attitude matrix according to revised attitude quaternion

$C_i^b=A\left({\hat{q}}_k\right)---\left(26\right)$

Second step carries out position, velocity estimation based on road sign point

(1) accelerometer measures

Accelerometer can obtain at [t _K-1, t _k] the speed increment Δ V of detector in the time period ^bSpeed increment just can obtain the average specific force f of the detector of this section in the time divided by sampling time interval ^bMeasured value.

${\tilde{f}}_k^b=\frac{{\mathrm{\&Delta;v}}^b}{t_k-t_{k-1}}---\left(27\right)$

(2) dynamics of orbits extrapolation

Utilization is carried out estimating of position, speed with the accelerometer measures in the time period and attitude estimated value.Represent that with r detector position, v represent detector speed, then inertial system (O _I-X _IY _IZ _I) descend the detector kinetics equation to be

${\stackrel{\&CenterDot;}{r}}^i=v^i$

${\stackrel{\&CenterDot;}{v}}^i=-\frac{{\mathrm{\&mu;}}_m}{{\left|\right|r^i\left|\right|}^3}{r}^i+a_{\mathrm{RCS}}^i+g_{\mathrm{dis}}^i---\left(28\right)$

Wherein, a _RCSBe the acceleration that propulsion system produces, g _DisBe Gravitational perturbation.a _RCSCan be by the measurement of accelerometer, and calculate by attitude information, namely

${\tilde{a}}_{\mathrm{RCS}}^i={\left(C_i^b\right)}^T{\tilde{f}}^b---\left(29\right)$

Getting quantity of state is

$X=\left[\begin{array}{c}{r}^i\\ v^i\end{array}\right]---\left(30\right)$

And order

(28) formula can be written as so

$\stackrel{\&CenterDot;}{X}=f(X,u)+w---\left(31\right)$

Wherein

$f(X,u)=\left[\begin{array}{c}{v}^i\\ -\frac{{\mathrm{\&mu;}}_m}{{\left|\right|r^i\left|\right|}^3}{r}^i+a_{\mathrm{RCS}}^i\end{array}\right]---\left(32\right)$

Here

And it simply is modeled as average is

, variance is

White noise.

Can utilize so formula (31) by t _K-1State estimation constantly

Estimate t _kState constantly

${\hat{X}}_k=F({\hat{X}}_{k-1},u_{k-1})$

$\&ap;{\hat{X}}_{k-1}+(t_k-t_{k-1})\&CenterDot;f({\hat{X}}_{k-1},u_{k-1})+\frac{{(t_k-t_{k-1})}^2}{2!}\left[\frac{\&PartialD;f}{\&PartialD;X}f\right]{|}_{X={\hat{X}}_{k-1}}---\left(33\right)$

Simultaneously can carry out the renewal of state estimation variance

$P_k={\mathrm{\&Phi;}}_{k|k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k|k-1}^T+Q_{k-1}---\left(34\right)$

Wherein

${\mathrm{\&Phi;}}_{k|k-1}=\frac{\&PartialD;F}{\&PartialD;X}{|}_{X={\hat{X}}_{k-1}}\&ap;I+\frac{\&PartialD;f}{\&PartialD;X}{|}_{X={\hat{X}}_{k-1}}\&CenterDot;(t_k-t_{k-1})---\left(35\right)$

(3) estimate road sign point position

Suppose at t _kConstantly, can in navigation picture, observe the individual road sign of m (m 〉=0) according to observation sequence.Take wherein j road sign as example, its position under inertial system can be calculated according to the almanac of celestial body or satellite.With t _kThe position of j road sign is designated as constantly

(omitting the subscript k of representative time here), then the direction of visual lines of its relative detector can be used formula (36) forecast

${\hat{p}}_{m,j}^i=\frac{r_{m,j}^i-{\hat{r}}^i}{\left|\right|r_{m,j}^i-{\hat{r}}^i\left|\right|}---\left(36\right)$

Write as the component form, made r ⁱ=[xyz] ^T, $r_{m,j}^i={\left[\begin{array}{ccc}{x}_{m,j}& y_{m,j}& z_{m,j}\end{array}\right]}^T,$ Then have

${\hat{p}}_{m,j}^i=\frac{1}{\sqrt{{(x_{m,j}-\hat{x})}^2+{(y_{m,j}-\hat{y})}^2+{(z_{m,j}-\hat{z})}^2}}\left[\begin{array}{c}{x}_{m,j}-\hat{x}\\ y_{m,j}-\hat{y}\\ z_{m,j}-\hat{z}\end{array}\right]---\left(37\right)$

The optical guidance sensor is measured coordinate system (O _c-X _cY _cZ _c, wherein initial point is at optical guidance sensor lens center, X _cAnd Y _cBe parallel to the focal plane, Z _cAlong optical axis direction) relatively the transformational relation of detector body system installed by the optical guidance sensor and determine, be designated as

The attitude matrix of detector relative inertness system is (being provided by formula (26)) then can be transformed into the prediction direction of visual lines of road sign the optical guidance sensor according to coordinate transform and measure under the coordinate system

${\hat{p}}_{m,j}^c=C_b^c\&CenterDot;C_i^b\&CenterDot;{\hat{p}}_{m,j}^i---\left(38\right)$

If the focal length of camera is f, the picpointed coordinate of road sign point imaging is x _{Cm, j}And y _{Cm, j}, then according to image-forming principle, the coordinate that can dope road sign point picture point with formula (39) and (40) is

${\hat{x}}_{\mathrm{cm},j}=-f\frac{C_{11}(x_{m,j}-\hat{x})+C_{12}(y_{m,j}-\hat{y})+C_{13}(z_{m,j}-\hat{z})}{C_{31}(x_{m,j}-\hat{x})+C_{32}(y_{m,j}-\hat{y})+C_{33}(z_{m,j}-\hat{z})}---\left(39\right)$

${\hat{y}}_{\mathrm{cm},j}=-f\frac{C_{21}(x_{m,j}-\hat{x})+C_{22}(y_{m,j}-\hat{y})+C_{23}(z_{m,j}-\hat{z})}{C_{31}(x_{m,j}-\hat{x})+C_{32}(y_{m,j}-\hat{y})+C_{33}(z_{m,j}-\hat{z})}---\left(40\right)$

Wherein,

$C_i^c=C_b^c\&CenterDot;C_i^b$

$=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]$

The imaging of optical guidance sensor, actual after image is processed what provide is the pixel coordinate (p, l) of road sign point.Ignore pattern distortion, then can further predict the pixel coordinate of j road sign according to formula (41) and (42)

${\hat{p}}_{m,j}=-K_{x}f\frac{C_{11}(x_{m,j}-\hat{x})+C_{12}(y_{m,j}-\hat{y})+C_{13}(z_{m,j}-\hat{z})}{C_{31}(x_{m,j}-\hat{x})+C_{32}(y_{m,j}-\hat{y})+C_{33}(z_{m,j}-\hat{z})}+p_0---\left(41\right)$

${\hat{l}}_{m,j}=-K_{y}f\frac{C_{21}(x_{m,j}-\hat{x})+C_{22}(y_{m,j}-\hat{y})+C_{23}(z_{m,j}-\hat{z})}{C_{31}(x_{m,j}-\hat{x})+C_{32}(y_{m,j}-\hat{y})+C_{33}(z_{m,j}-\hat{z})}+l_0---\left(42\right)$

Wherein, K _xAnd K _yBe the imaging parameters of camera, it has reflected the proportionate relationship between image point position and the pixel coordinate.

(4) obtain road sign point position by the navigation camera

At t _kConstantly, use the imaging of optical guidance sensor, from the navigation picture of actual photographed, extract the pixel coordinate of road sign point by image processing techniques.If comprised m road sign in the current navigation picture, wherein the actual pixels coordinate of j road sign is (p _{M, j}, l _{M, j}), satisfy imaging relations between it and the real detector position

$p_{m,j}=-K_{x}f\frac{C_{11}(x_{m,j}-x)+C_{12}(y_{m,j}-y)+C_{13}(z_{m,j}-z)}{C_{31}(x_{m,j}-x)+C_{32}(y_{m,j}-y)+C_{33}(z_{m,j}-z)}+{\mathrm{<figure-callout\; id="0"\; label="p"\; filenames="HSA00000817707000021.png,HSA00000817707000031.png"\; state="\{\{state\}\}">p</figure-callout>}}_0+v_{p,j}---\left(43\right)$

$l_{m,j}=-K_{x}f\frac{C_{21}(x_{m,j}-x)+C_{22}(y_{m,j}-y)+C_{23}(z_{m,j}-z)}{C_{31}(x_{m,j}-x)+C_{32}(y_{m,j}-y)+C_{33}(z_{m,j}-z)}+{\mathrm{<figure-callout\; id="0"\; label="l"\; filenames="HSA00000817707000021.png,HSA00000817707000031.png"\; state="\{\{state\}\}">l</figure-callout>}}_0+v_{l,j}---\left(44\right)$

Wherein, v _{P, j}And v _{L, j}It is the imaging noise.

(5) EKF

Make Z _j=[p _{M, j}, l _{M, j}] ^T, then formula (43) and (44) can consist of the measurement equation

Z _j＝h _j(X)+v _j (45)

Wherein,

$h_j\left(X\right)=\left[\begin{array}{c}-K_{x}f\frac{C_{11}(x_{m,j}-x)+C_{12}(y_{m,j}-y)+C_{13}(z_{m,j}-z)}{C_{31}(x_{m,j}-x)+C_{32}(y_{m,j}-y)+C_{33}(z_{m,j}-z)}+p_0\\ -K_{y}f\frac{C_{21}(x_{m,j}-x)+C_{22}(y_{m,j}-y)+C_{23}(z_{m,j}-z)}{C_{31}(x_{m,j}-x)+C_{32}(y_{m,j}-y)+C_{33}(z_{m,j}-z)}+l_0\end{array}\right]---\left(46\right)$

$v_j=\left[\begin{array}{c}{v}_{p,j}\\ v_{l,j}\end{array}\right]---\left(47\right)$

Equation is measured in the filtering of finishing that m the road sign that is obtained simultaneously by current time so consists of

Z＝h(X)+v (48)

Wherein,

$Z=\left[\begin{array}{c}{\mathrm{<figure-callout\; id="1"\; label="Z"\; filenames="HSA00000817707000021.png,HSA00000817707000031.png"\; state="\{\{state\}\}">Z</figure-callout>}}_1\\ .\\ .\\ .\\ Z_m\end{array}\right]---\left(49\right)$

$h\left(X\right)=\left[\begin{array}{c}{h}_1\left(X\right)\\ .\\ .\\ .\\ h_m\left(X\right)\end{array}\right]---\left(50\right)$

$\mathrm{<figure-callout\; id="1"\; label="v\; v"\; filenames="HSA00000817707000021.png,HSA00000817707000031.png"\; state="\{\{state\}\}">v</figure-callout>}\left[\begin{array}{c}{v}_{}\end{array}\right]\left[\begin{array}{c}{}_1\\ .\\ .\\ .\\ v_m\end{array}\right]---\left(51\right)$

And the variance of establishing v is R.

So, just can use EKF to carry out state estimation.

At first, the state variance battle array calculation of filtered gain battle array of predicting according to formula (34)

$K_k=P_k{H}_k^T{(H_k{P}_k{H}_k^T+R_k)}^{-1}---\left(52\right)$

And have

$H_k=\frac{\&PartialD;h\left(X\right)}{\&PartialD;\left(X\right)}{|}_{X={\hat{X}}_{k-1}}---\left(53\right)$

Then utilize actual measurement that the state of estimating is revised

${\hat{X}}_k={\hat{X}}_k+K_k[Z_k-h\left({\hat{X}}_k\right)]---\left(54\right)$

At last the state variance battle array of prediction is revised

$P_k=(I-K_k{H}_k)P_k{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T---\left(55\right)$

Along with the recursion of time, repeat above-mentioned steps, just finished based on road sign around the section navigation calculation.

Simulation analysis.The air navigation aid that the present invention proposes is verified as example around the detector of Mars flight.If the periareon height of detector track is 353km, apoareon height 56600km.Detector is selected phobos, Deimos or areographic feature landform according to the observability segmentation of illumination condition, flying height and target in the circumaviate process be that road sign carries out independent navigation, as shown in Figure 2.The navigation viewing field of camera that uses is 2 °, and focal length is 0.1m, and pixel resolution is 1024 * 1024.Road sign point pixel extraction precision is 0.1 pixel (1 σ), and the cycle that navigation is revised is 1 minute.

Begin a navigation Selecting landmarks in the orbital period as shown in Figure 3 from the periareon.Its priority is according to observability and illumination condition, take first phobos, then Deimos, last martian surface feature landform be as order.The change curve of corresponding navigation position error and velocity error as shown in Figure 4 and Figure 5.From scheming as seen, omnidistance navigation position error is less than 2km, and velocity error is less than 3m/s.

The non-elaborated part of the present invention belongs to techniques well known.

Claims (2)

1. One kind based on celestial body surface characteristics and natural satellite road sign around the section air navigation aid, it is characterized in that performing step is as follows:

   (1) the inertia attitude of gyro and star sensor is estimated

   Utilize the angular velocity of gyro to measure in the previous sampling period that the detector inertia attitude of current time is extrapolated; Then the optical axis inertia that utilizes star sensor to obtain points to be measured, and uses Kalman filtering that the error of gyro extrapolation inertia attitude is estimated; The inertia attitude of estimating with the estimated value correction gyro of error at last obtains final inertia attitude and estimates;

   (2) utilize the position of accelerometer measures, speed to extrapolate

   In gyro to measure, use accelerometer to obtain the measured value of the non-gravitational acceleration of detector, utilize acquired inertia attitude estimated value that it is transformed in the inertial coordinates system; Then according to the dynamics of orbits model, position and the speed of detector are extrapolated;

   (3) the road sign dot information obtains

   Use the optical navigation camera of installing on the detector according to certain shooting sequence possible road sign point to be taken pictures, the selection of road sign point has comprised the characteristic feature of known location on celestial body surface and the natural satellite of celestial body; Road sign point is selected in advance according to orbit altitude, illumination condition, and lists observation sequence in; At last, extract the pixel coordinate of road sign point from navigation picture, pass to Navigation Filter;

   (4) based on the detector position of road sign, the estimation of speed

   Forecast based on the satisfy the need image coordinate of punctuate of detector position, the speed of extrapolation in the step (2); Road sign point pixel coordinate and predicted value that reality obtains are compared, consist of and measure new breath; Then use EKF that detector position, the speed of prediction are revised, obtain final detector position, velocity estimation value;

   (5) to the step of each control cycle repetition (1)～(4), finish navigation calculation.
2. According to claim 1 based on celestial body surface characteristics and natural satellite road sign around the section air navigation aid, it is characterized in that: described method simultaneously with celestial body surface characteristics and natural satellite all as the navigation road sign, when orbit altitude is higher than the natural satellite track take the large-scale landforms in natural satellite and celestial body surface as road sign, take the local landform of celestial body as road sign, it is variable that Navigation resolves in the process quantity of road sign point when track is lower than the natural satellite track.

Images