CN103674034A - Robust navigation method capable of realizing multi-beam velocity and distance measurement correction - Google Patents

Abstract

The robust navigation method for multi-beam speed measurement and ranging correction first performs inertial navigation calculation, then performs ranging correction, and then performs speed correction. The information source used for ranging correction is the ranging sensor; the signal source of speed correction is divided into There are two types, one is the distance measurement signal, and the other is the speed measurement signal: when the distance measurement signal is valid and the flight height is in line with the introduction range of the speed measurement correction, the height calculated by the two consecutive distance measurement signals is differentiated to obtain the gravity direction. Speed: when the speed measurement signal is valid and the flight altitude is in line with the introduction range of the speed measurement correction, the speed along the beam direction of the speed measurement sensor can be obtained directly. Then use the speed of the first two cycles of the inertial navigation to calculate and calculate the inertial speed of the lander at the time corresponding to the ranging or speed measurement signal, and then convert it to the speed relative to the lunar surface and project it to the direction of altitude or the direction of the speed measurement beam. The invention is simple and practical, has high reliability and ensures the high precision of the whole navigation process.

Description

A Robust Navigation Method for Multi-beam Velocity and Ranging Correction

technical field

The invention relates to a robust navigation method for multi-beam speed measurement and range correction, which belongs to the field of autonomous navigation for lunar and deep space exploration soft landing missions.

Background technique

The soft landing of extraterrestrial objects is a brand new task for our country. For the GNC system, obtaining accurate navigation data is a prerequisite to ensure a safe and soft landing of the lander. Due to the short time and high autonomy of the soft landing process, there are similar missions abroad, including the moon landing missions implemented by the US Apollo, Surveyor and the former Soviet Union's Luna series, as well as the lunar landing missions planned by the European Space Agency and Japan. All use inertial navigation as the main navigation method.

Inertial navigation technology relies on the gyroscope to directly measure the angular velocity and integrate it to obtain the attitude angle of the detector or use the gyroscope to establish a stable platform and obtain the attitude through the frame angle; rely on the accelerometer to measure the specific force and integrate to obtain the velocity and position. This navigation method has the characteristic that the running process does not depend on external information. Since inertial navigation is an extrapolation algorithm, the accuracy of inertial navigation depends on the initial value of navigation and the error of inertial devices, and it shows the characteristics of continuous growth with time.

For soft landing missions, the initial value of inertial navigation is given by ground measurement and control and star sensors. For extraterrestrial celestial bodies, the accuracy of ground measurement and control is limited, and the position error of the detector reaches the order of kilometers. Using this as an initial value for navigation is dangerous for a soft landing. In addition, the terrain fluctuations on the surface of celestial bodies are also uncertain. Therefore, it is not feasible to rely solely on inertial navigation to complete the soft landing navigation task.

For this reason, foreign extraterrestrial celestial body soft landing missions are equipped with microwave or laser ranging or speed measuring sensors to correct inertial navigation. However, due to differences in sensor technology levels, detector scales, and soft landing guidance algorithms in various countries, the specific methods for correcting inertial navigation using ranging or velocity information are also different. For example, the Apollo lunar module in the United States used a microwave velocity and ranging sensor, which has three velocity measuring beams and one ranging beam. The three velocity measuring beams work separately, and then the velocity of the lunar module to the lunar surface is calculated geometrically. In addition, the sensor has two different gears, which can ensure that the ranging beam points to the lunar surface as much as possible according to the attitude of the lunar module, so as to obtain the altitude information of the lunar module. See "Bernard A. Kriegsman, Radar-updated inertial navigation of a continuously-powered space vehicle, IEEE transaction on Aerospace and Electronic Systems, Vol. AES-2, No. 4, July, 1966, 549-565" and "Floyd V. Bennett, Apollo lunar descent and ascent trajectories, NASA TMX58040, March 1970". The filtering correction method adopts the simplified Kalman filtering method. The disadvantages of the above method are: (1) three beams are required to be effective at the same time, and the fault tolerance is not good; (2) there is only one ranging beam, in order to adapt to the adjustment of large attitude, moving parts are required, which reduces the reliability of the system.

The navigation sensors used in the soft landing of the second phase of my country's lunar exploration are different from foreign related tasks. In addition to the IMU used for inertial navigation, it is also equipped with microwave ranging, speed measuring sensors and laser ranging sensors for correcting inertial navigation. In order to avoid damage to the landing process caused by the failure of the sensor's rotating mechanism, my country's microwave ranging and speed measuring sensors are different from Apollo's approach and are fixed installations. However, in order to ensure that the speed measurement and distance measurement information can be obtained within the 90° attitude rotation range during the landing process, the equipped microwave speed measurement and distance measurement sensor has three speed measurement beams with different directions and two distance measurement beams with different directions. In addition, the laser ranging sensor also has two ranging beams with different directions, so the number of beams that can be used for speed measurement and ranging correction in my country's soft landing mission is much larger than that of Apollo. These beam measurements are not synchronized, and the accuracy is different. How to effectively and reasonably use this information and take into account the needs of system fault tolerance has become a difficult problem that the navigation algorithm needs to solve.

Contents of the invention

The technical problem of the present invention is to overcome the deficiencies of the prior art, provide a simple and practical multi-beam speed measurement and range correction robust navigation method, and have high reliability, ensuring the accuracy of the whole navigation.

The technical scheme adopted in the present invention is: a robust navigation method for multi-beam speed measurement and ranging correction, and the implementation steps are as follows

(1) Inertial navigation solution

Use the angle increment measured by the gyro in the previous sampling period to calculate the inertial attitude of the lander at the current moment, and then use the velocity increment measured by the accelerometer to accumulate and calculate the speed of the lander at the current moment, and integrate to obtain the lander in the inertial coordinate system s position;

(2) Ranging correction

If the effective data from the ranging sensor is received, and the flight height is in line with the introduction range of the ranging correction, then the data will be transformed to obtain the height data through the installation matrix and the lander attitude matrix; at the same time, the inertial navigation solution of the previous two cycles Calculate, calculate the height of the lander at the time point corresponding to the range sensor data, make the difference between the heights calculated by the inertial navigation and the range finder, and correct the position error in the height direction according to the height correction coefficient; the height correction coefficient varies with The altitude of the lander changes linearly, the higher the altitude of the lander, the smaller the altitude correction factor, and the lower the altitude, the larger the altitude correction factor;

(3) Speed correction

There are two types of signal sources for speed correction, one is the distance measurement signal and the other is the speed measurement signal; when the distance measurement signal is valid and the flight altitude is within the range of the speed measurement correction introduction, the altitude calculated by two consecutive distance measurement signals Make a difference to obtain the speed in the direction of gravity. At the same time, use the speed of the first two cycles in the inertial navigation solution to calculate the inertial speed of the lander at the time corresponding to the ranging signal, and convert it to the speed relative to the lunar surface. The speed obtained by the ranging signal Make a difference with the speed obtained by the inertial navigation, and correct the speed in the altitude direction according to the speed correction coefficient. The speed correction coefficient changes linearly with the speed of the lander. The faster the lander, the smaller the speed correction coefficient, and the smaller the speed, the speed correction coefficient The larger the value is; it is effective for the speed measurement signal, and the inertial speed of the lander at the time corresponding to the speed measurement signal is calculated by using the speed of the first two cycles in the inertial navigation solution, and converted into the relative lunar speed, and the speed obtained by the speed measurement signal is compared with the speed obtained by the inertial navigation. The speed of the lander is different, and the speed in the direction of the beam is corrected according to the speed correction coefficient. The speed correction coefficient changes linearly with the speed of the lander. The faster the lander is, the smaller the speed correction coefficient is, and the smaller the speed is, the larger the speed correction coefficient is;

(4) Repeat the steps (1) to (3) for each control cycle to complete the navigation solution.

The height correction coefficient and the speed correction coefficient generally vary between 0 and 1.

Compared with the prior art, the present invention has the advantages that: the present invention does not require ranging and speed measuring beams to have movable parts, which increases the reliability of the system; Correction of the results of the calculation is conducive to improving the fault tolerance of the system and reducing the complexity of the fault diagnosis algorithm; the filtering process uses a linearly changing correction coefficient, which is simpler to calculate than the standard Kalman filter, and can be used for sensors at different stages. Different accuracy automatically adjusts the filter correction parameters to ensure the high precision of the whole navigation process.

Description of drawings

Fig. 1 is the functional block diagram of the robust navigation method of multi-beam speed measurement and ranging correction of the present invention;

Figure 2 is the definition of the reference coordinate system;

Figure 3 is the curve of the actual lander position change during the lunar soft landing process;

Fig. 4 is the position variation curve given by the navigation system during the soft landing on the moon;

Figure 5 is the curve of the actual lander speed change during the lunar soft landing process;

Fig. 6 The speed change curve given by the navigation system during the lunar soft landing.

Detailed ways

As shown in Figure 1, the specific calculation process of the robust navigation method for multi-beam speed measurement and ranging correction in the present invention is as follows:

The first step is to perform inertial navigation calculation

(1) Establishment of reference coordinate system and initial value of navigation system (initialization)

This system (O _B -X _B Y _B Z _B ) is fixedly connected to the lander, the origin coincides with the center of mass, and the three axes are parallel to the main axis of inertia of the lander.

The navigation system selects the landing inertial coordinate system (O _m -X _I Y _I Z _I ) as the reference coordinate system: the origin is the center of the moon O _m , at the nominal landing time, the X _{and I} axes point to the nominal landing point, and the Z _{and I} axes are at the planned The plane of the landing orbit points to the direction of motion, and the Y _I axis forms a right-handed orthogonal coordinate system, as shown in Figure 2. If the lander lands at a nominal landing point with a nominal attitude at a nominal time, then the three axes of the landing inertial coordinate system and the body coordinate system are parallel.

The initial values of the navigation system include position, velocity and attitude. The position and velocity are given by ground measurement and control, while the attitude is obtained by star sensor and gyroscope measurement. It should be noted that the coordinate system used for ground measurement and control and star sensitivity measurement may not be the landing inertial coordinate system, such as the moon-centered J2000 coordinate system. However, since the nominal landing point and nominal landing time are known, these states can be conveniently transferred to the landing inertial coordinate system according to the lunar ephemeris. Let the position vector be r ^I , the velocity vector be v ^I , and the attitude matrix of the body relative to the landing inertial system be C _bi (the corresponding attitude quaternion is q).

(2) Attitude determination algorithm

The purpose of the attitude determination algorithm is to update the attitude matrix C _bi , which can be obtained recursively from the gyroscopic measurements. Assuming that the angular velocity measured by the gyro is ω ^b , the attitude motion equation can be described by quaternion as:

$\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}\mathrm{<span\; class="notranslate">\; Eq</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; q</span>}<span\; class="notranslate">\; )</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}^{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>$

in

$\mathrm{<span\; class="notranslate">\; Eq</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; q</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}& {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}& {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}\\ <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}& {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}& {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}\\ <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>$

Integrating the formula (1) can complete the update of the quaternion, and further solve the attitude matrix:

${\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; bi</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="q"\; filenames="HDA0000447199320000021.png,HDA0000447199320000032.png"\; state="\{\{state\}\}">q</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="4"\; label="q"\; filenames="HDA0000447199320000021.png"\; state="\{\{state\}\}">q</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}& \mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="q"\; filenames="HDA0000447199320000021.png,HDA0000447199320000032.png"\; state="\{\{state\}\}">q</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}<span\; class="notranslate">\; )</span>& \mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}<span\; class="notranslate">\; )</span>\\ \mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}<span\; class="notranslate">\; )</span>& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="q"\; filenames="HDA0000447199320000021.png,HDA0000447199320000032.png"\; state="\{\{state\}\}">q</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="4"\; label="q"\; filenames="HDA0000447199320000021.png"\; state="\{\{state\}\}">q</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}& \mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}<span\; class="notranslate">\; )</span>\\ \mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}<span\; class="notranslate">\; )</span>& \mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}<span\; class="notranslate">\; )</span>& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="q"\; filenames="HDA0000447199320000021.png,HDA0000447199320000032.png"\; state="\{\{state\}\}">q</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="4"\; label="q"\; filenames="HDA0000447199320000021.png"\; state="\{\{state\}\}">q</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; )</span>$

(3) Position and velocity recursion, that is, use the velocity increment measured by the accelerometer to accumulate and calculate the velocity of the lander at the current moment, and integrate to obtain the position of the lander in the inertial coordinate system

The basic equation of inertial navigation described in the landing inertial coordinate system is

${\stackrel{<span\; class="notranslate">\; \&CenterDot;</span><span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; r</span>}}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; g</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; f</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 4</span>}<span\; class="notranslate">\; )</span>$

Among them, g ^I is the gravitational acceleration of the moon, under the condition of ignoring the aspheric gravitational perturbation of the moon, we have

${\mathrm{<span\; class="notranslate">\; g</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&mu;</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}}\frac{{\mathrm{<span\; class="notranslate">\; r</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}}{{\mathrm{<span\; class="notranslate">\; r</span>}}^{\mathrm{<span\; class="notranslate">\; 3</span>}}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; )</span>$

Among them, μ _m is the gravitational constant of the moon. f ^I is the non-gravitational acceleration (specific force) expressed in the inertial system, which can be measured by the accelerometer. However, since the accelerometer measurement is expressed in the native system, namely f ^b , it needs to be converted according to the attitude matrix.

${\mathrm{<span\; class="notranslate">\; f</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; ib</span>}}{\mathrm{<span\; class="notranslate">\; f</span>}}^{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; bi</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}{\mathrm{<span\; class="notranslate">\; f</span>}}^{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 6</span>}<span\; class="notranslate">\; )</span>$

因此对(4)式进行两重积分就可以分别获得着陆器的速度矢量v^I和位置矢量r^I。because

Therefore, the speed vector v ^I and the position vector r ^I of the lander can be obtained respectively by performing double integration on formula (4).

After obtaining the position, the height relative to the lunar surface can also be calculated

h=||r ^I ||-r _M (7)

r _M is the reference lunar surface altitude corresponding to r ^I.

The second step is to perform ranging and speed correction (autonomous navigation filtering)

The position and speed of the inertial navigation recursion can be corrected by using the speed measuring and ranging radar. The ranging sensor provides the line-of-sight distance, so the line-of-sight distance can be converted into the height of the lander relative to the lunar surface by using the attitude information and the beam direction of the sensor (line-of-sight direction). The velocity sensor obtains the beam direction component of the velocity of the lander relative to the lunar surface. Therefore, the inertia of the lander can be obtained by using the attitude information and the installation of the velocity sensor, and compensating the ground velocity caused by the rotation of the upper moon according to the position of the probe. speed. Comparing the information obtained by the ranging and velocity sensors with the position and velocity information calculated by the inertial navigation system to obtain new information, the position and velocity errors of the inertial navigation system can be corrected by Kalman filtering.

In actual use, due to the inconsistent measurement time of the speed measurement and distance measurement sensors, and the limitation of the calculation amount of the on-board software, the extended Kalman filter in the navigation algorithm is realized by fitting and optimizing the filter correction coefficient, and the speed measurement, measurement Corrections are performed separately.

(1) Ranging correction

According to the height of the lander relative to the reference surface of the lunar surface at the measurement time and the height converted from the ranging sensor data, the measurement residual is calculated; the position is corrected according to the optimized correction coefficient, and the current gravitational acceleration is calculated using the corrected position. The specific algorithm is:

a. Navigation data extrapolation

则利用惯导预测的t_u时刻着陆器位置为Since the time of the ranging information and the time of the inertial navigation solution are often not synchronized, it is necessary to use navigation data extrapolation to synchronize the time points. Suppose the time corresponding to the ranging information is t _u , the time of the first two steps of inertial navigation calculation closest to it is t _n-2 and t _n-1 , and the corresponding position calculation value is and

Then the position of the lander at time t _u predicted by inertial navigation is

${\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; u</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}}{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; u</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\text{<span class="notranslate"> 8</span>}<span\; class="notranslate">\; )</span>$

b. Error calculation

The position information calculated by the inertial navigation can obtain the height relative to the lunar surface

${\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; u</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; ins</span>}}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; |</span><span\; class="notranslate">\; |</span>{\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; u</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; |</span><span\; class="notranslate">\; |</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 9</span>}<span\; class="notranslate">\; )</span>$

对应的参考月面高度。r _M is

The corresponding reference lunar surface altitude.

则利用姿态信息可以将它转到着陆惯性坐标系中Suppose the installation direction of the ranging sensor beam in this system is

Then use the attitude information to transfer it to the landing inertial coordinate system

${\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; R</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; ib</span>}}{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; R</span>}}^{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 10</span>}<span\; class="notranslate">\; )</span>$

Assuming that the measurement of the ranging sensor is ρ _u , the height can be calculated as

${\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; u</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; radar</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; u</span>}}\frac{<span\; class="notranslate">\; <</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; R</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; u</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; ></span>}{<span\; class="notranslate">\; |</span><span\; class="notranslate">\; |</span>{\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; u</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; |</span><span\; class="notranslate">\; |</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 11</span>}<span\; class="notranslate">\; )</span>$

Thus, the error of height calculation can be obtained as

δh _u =h _u,radar -h _u,ins (12)

c. Filter correction

Take the height correction coefficient w _h to increase gradually as the height decreases, that is,

${\mathrm{<span\; class="notranslate">\; w</span>}}_{\mathrm{<span\; class="notranslate">\; h</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; w</span>}}_{\mathrm{<span\; class="notranslate">\; h</span>}}^{<span\; class="notranslate">\; *</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; /</span>{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; h</span>}}^{<span\; class="notranslate">\; *</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 13</span>}<span\; class="notranslate">\; )</span>$

和

为给定参数，可以根据不同的任务特点进行调整一般

的取值在0～1之间，因此 $0\&le;w_h\&le;w_h^{*}.$

and

For the given parameters, it can be adjusted according to different task characteristics.

The value of is between 0 and 1, so $0\&le;w_h\&le;w_h^{*}.$

Then make an altitude correction to the inertial navigation

r ^I =r ^I +w _h ·δh _u ·r ^I /||r ^I || (14)

(2) Speed correction

I. Algorithm for Correcting Gravity Direction Velocity Using Altitude Information

Use the slant distance information measured by the ranging beam at two different times to calculate the height of the lander relative to the lunar surface, and obtain the velocity component in the direction of gravity of the lander by difference, calculate the residual error of the velocity measurement, and perform velocity correction according to the optimized correction coefficient . The specific algorithm is:

a. Navigation data extrapolation

和

则As in the processing of ranging data, it is necessary to extrapolate the speed at the time t _u measured by the speed sensor based on the data of the first two steps of inertial navigation. Let the calculated speed at time t _n-2 and t _n-1 be

and

but

${\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; u</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}}{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; u</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 15</span>}<span\; class="notranslate">\; )</span>$

b. Error calculation

The speed calculated by the inertial navigation system is the speed relative to the inertial system, and then converted to the speed relative to the lunar surface.

${\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; g</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; ins</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; u</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; \&times;</span>{\mathrm{<span\; class="notranslate">\; r</span>}}_{\mathrm{<span\; class="notranslate">\; u</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 16</span>}<span\; class="notranslate">\; )</span>$

Its component in the direction of gravity is

${\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; ins</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\frac{<span\; class="notranslate">\; <</span>{\mathrm{<span\; class="notranslate">\; r</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; \&Center\; Dot;</span>{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; g</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; ins</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; ></span>}{<span\; class="notranslate">\; |</span><span\; class="notranslate">\; |</span>{\mathrm{<span\; class="notranslate">\; r</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; |</span><span\; class="notranslate">\; |</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 17</span>}<span\; class="notranslate">\; )</span>$

Let the slant range information measured by the ranging beam at time t _k-1 and time t _k be ρ(t _k-1 ) and ρ(t _k ), combined with the direction of the ranging beam and the attitude and position information at the corresponding time , solve for the height. Taking time t _k as an example, if the installation orientation of the ranging beam in this system is p ^b , then its orientation in the inertial system p ^I (t _k ) is

p ^I (t _k )=C _ib (t _k )p ^b (18)

Then the height can be calculated according to the distance measurement information of the ranging sensor, which is denoted as

${\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; radar</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\frac{<span\; class="notranslate">\; <</span>{\mathrm{<span\; class="notranslate">\; p</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; r</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; ></span>}{<span\; class="notranslate">\; |</span><span\; class="notranslate">\; |</span>{\mathrm{<span\; class="notranslate">\; r</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span>}\mathrm{<span\; class="notranslate">\; \&rho;</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 19</span>}<span\; class="notranslate">\; )</span>$

Similarly, the height h _radar (t k _-1 ) can be calculated according to the ranging information at the time t _k-1 . Then the speed of the lander in the direction of gravity obtained by the difference is

${\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; radar</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\frac{{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span>}{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 20</span>}<span\; class="notranslate">\; )</span>$

Thus, the difference in the gravity direction velocity calculated by two different means can be obtained as

δv _h =v _h,radar -v _h,ins (21)

c. Filter correction

Take the speed correction coefficient w _v to increase gradually with the decrease of speed, that is

${\mathrm{<span\; class="notranslate">\; w</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; w</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}}^{<span\; class="notranslate">\; \&CenterDot;</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; |</span><span\; class="notranslate">\; |</span>{\mathrm{<span\; class="notranslate">\; v</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; |</span><span\; class="notranslate">\; |</span><span\; class="notranslate">\; /</span>{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}}^{<span\; class="notranslate">\; *</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 19</span>}<span\; class="notranslate">\; )</span>$

和

为给定参数，可以根据不同的任务特点进行调整，一般

的取值在0～1之间，因此

and

For the given parameters, it can be adjusted according to different task characteristics, generally

The value of is between 0 and 1, so

Then correct the speed in the altitude direction of the inertial navigation

v ^I =v ^I +w _v δv _h r ^I /||r ^I || (20)

II. Algorithm for speed correction of beam direction using speed sensor

Using the speed of the lander relative to the lunar surface measured by the speedometer and the speed component in the direction of the speed measurement beam calculated by the inertial navigation system, the residual error of the speed measurement is calculated, and the speed is corrected according to the optimized correction coefficient. The specific algorithm is:

a. Navigation data extrapolation

和

按照(15)式外推测速敏感器测速数据对应时刻的着陆器惯性速度。The calculated speed of inertial navigation at time t _n-2 and t _n-1 is

and

The inertial velocity of the lander at the time corresponding to the velocity sensor velocity measurement data is extrapolated according to (15).

b. Error calculation

First, it is necessary to convert the speed calculated by inertial navigation into the speed relative to the lunar surface according to (16), and then project it to the beam direction. The specific algorithm is as follows.

则利用姿态信息可以将它转到着陆惯性坐标系中Assume that the direction of a certain beam of speed measuring radar in this system is

Then use the attitude information to transfer it to the landing inertial coordinate system

${\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; ib</span>}}{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}}^{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; twenty\; one</span>}<span\; class="notranslate">\; )</span>$

Then the component of the velocity calculated by inertial navigation in the direction of the radar beam is

${\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; u</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; ins</span>}}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; <</span>{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; g</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; ins</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; \&Center\; Dot;</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; ></span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; twenty\; two</span>}<span\; class="notranslate">\; )</span>$

The speed measurement actually obtained by the speed measuring radar is v _u,radar , therefore, the difference between the two is

δv _u =v _u,radar -v _u,ins (23)

c. Filter correction

According to formula (19), the speed correction coefficient is gradually increased as the speed decreases, and the speed correction is performed on the inertial navigation

${\mathrm{<span\; class="notranslate">\; v</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; v</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; w</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}}<span\; class="notranslate">\; \&Center\; Dot;</span>\mathrm{<span\; class="notranslate">\; \&delta;</span>}{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; u</span>}}<span\; class="notranslate">\; \&Center\; Dot;</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}}^{\mathrm{<span\; class="notranslate">\; I</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 20</span>}<span\; class="notranslate">\; )</span>$

(3) Correction strategy

The above distance measurement and speed measurement correction algorithms are all performed for a single beam, that is, as long as any beam is valid at the current moment, it can be used for navigation correction. In the correction process, the ranging correction is mainly aimed at the position error of the altitude channel, and the velocity correction is mainly aimed at the velocity error in the beam direction. For the soft landing task, the height information is the most important, which is directly related to the safety of the landing; and the accuracy requirement of the horizontal position information is not particularly high, so the position correction performed by the present invention is suitable. For speed correction, both horizontal and vertical speeds are very important, which are directly related to the size of the landing speed deviation and affect the success or failure of the mission. Although the velocity correction adopted in the present invention is mainly along the beam direction, as long as there are three beam measurements in different directions, the three-dimensional velocity correction can be completed. Therefore, this algorithm is also simple and effective.

simulation analysis. Taking the Chang'e-3 lunar lander as an example, the simulation analysis is carried out. The simulation conditions are as follows:

●Initial orbit: the position and velocity of the perilunary point of the 100km×15km orbit, the altitude is 15km, and the horizontal velocity is 1692m/s (there is a 1km random error in the height of the perilunar point and the apolunar point).

●Initial attitude: The attitude angle relative to the landing inertial system is 110° for pitch, and 0° for yaw and roll.

●Initial navigation error: track error includes position error 2km (0.45km in R direction, 1.8km in T direction, 0.6km in N direction), speed error 2m/s (1.65m/s in R direction, 1m/s in T direction, 0.6 in N direction m/s), the attitude error is 0.03°.

●Microwave ranging and speed measuring sensors have a total of five sets of antennas, including two sets of ranging and three sets of speed measuring, which are installed on the lander-Z surface. The specific orientation is: the unit vectors of the beam orientations of the three groups of speed measurement antennas (including the receiving antenna and the transmitting antenna) in the system of the lander are R1=[cos133°, cos90°, cos137°], R2=[cos154. 3265°, cos75.3330°, cos110.5823°], R3=[cos154.3265°, cos104.6670°, cos110.5823°]; two sets of ranging antennas (including receiving antenna and transmitting antenna) beam pointing to R4 and R5 is along the -Z axis and -X axis of the land vehicle body system respectively.

●The correction factor is taken as $w_h^{*}=0.5,h_h^{*}=8000m,w_v^{*}=0.4,v_v^{*}=300m/\mathrm{the\; s}.$

The navigation strategy is as follows: ranging information is introduced at an altitude of 8km from the lunar surface, and speed measurement information is introduced at an altitude of 3.5km from the lunar surface; only inertial navigation is used for altitudes above 8km and below 30m from the lunar surface.

Figure 3 shows the actual landing process and the position change curve under the landing inertial system given by the navigation, and the navigation error is shown in Figure 4. It can be seen from Figure 4 that before the introduction of external ranging and speed measuring information, the inertial navigation position solution is divergent; the introduction of ranging information at heights below 8km (after about 360s) helps to correct the position error in the x direction (height direction), However, there is no ability to correct the position error in the y and z directions (horizontal direction); when the speed measurement information is introduced below 3.5km (about 450s later), it can reduce the divergence speed of the horizontal position to a certain extent by correcting the speed in the y and z directions , improving the accuracy of navigation.

Fig. 5 shows the change curve of the lander's inertial velocity between the actual power descent process and that given by the navigation, and the navigation error is shown in Fig. 6. It can be seen from the data in Figure 6 that before the microwave speed measurement data is introduced into the navigation system, the speed error of the inertial navigation solution gradually increases; after the microwave speed measurement data is introduced, the speed error is corrected to ensure the accuracy of navigation.

Parts not described in detail in the present invention belong to the well-known technology in the art.

Claims (2)

1. the robust air navigation aid that multi-beam tests the speed and finds range and revise, is characterized in that performing step is as follows:

(1) inertial reference calculation

Utilize the inertia attitude of the angular speed calculation current time lander of gyro to measure in the previous sampling period, then utilize specific force that accelerometer measures obtains to calculate the speed of current time lander, and integration obtains the position of lander under inertial coordinates system;

(2) range finding is revised

If receive the valid data of range finding sensor, and flying height meets the introducing scope that range finding is revised, by these data by matrix being installed and the conversion of lander attitude matrix solves altitude information; Utilize the inertial reference calculation in the first two cycle simultaneously, calculate range finding sensor data lander height on corresponding time point, it is poor that the height that inertial navigation and stadimeter are resolved is respectively done, and according to the site error of altitude correction factor corrected altitude direction; This altitude correction factor changes with lander highly linear, and lander height is higher, and described altitude correction factor is less, and highly more low clearance correction factor is larger;

(3) speed correction

The signal source of speed correction is divided into two kinds, and a kind of is distance measuring signal, and another kind is tachometer signal, when distance measuring signal effective, and flying height meets while testing the speed correction introducing scope, the height that double distance measuring signal is solved carries out difference, obtain the speed of gravity direction, utilize the corresponding lander velocity inertial constantly of speed calculation distance measuring signal in the first two cycle in inertial reference calculation simultaneously, and be converted to the speed of relative lunar surface, it is poor that the speed that the speed that distance measuring signal is obtained and inertial navigation obtain is done, and according to the speed of speed correction factor correction gravity direction, this speed correction factor is with lander speed linear change, lander speed is faster, speed correction factor is less, speed is less, speed correction factor is larger, when tachometer signal is effective, utilize the corresponding lander velocity inertial constantly of speed calculation tachometer signal in the first two cycle in inertial reference calculation, and be converted to the speed of relative lunar surface, it is poor that the speed that the speed that tachometer signal is obtained and inertial navigation obtain is done, and according to the speed of speed correction factor correction beam direction, this speed correction factor is with lander speed linear change, lander speed is faster, speed correction factor is less, and speed is less, and speed correction factor is larger,

(4) each control cycle is repeated to the step of (1)～(3), complete navigation calculation.

2. the robust air navigation aid that multi-beam according to claim 1 tests the speed and finds range and revise, is characterized in that: described altitude correction factor and speed correction factor are 0-1.

Images