CN113720335A - High-fault-tolerance strong autonomous navigation method for landing of extraterrestrial celestial body - Google Patents
High-fault-tolerance strong autonomous navigation method for landing of extraterrestrial celestial body Download PDFInfo
- Publication number
- CN113720335A CN113720335A CN202111063826.9A CN202111063826A CN113720335A CN 113720335 A CN113720335 A CN 113720335A CN 202111063826 A CN202111063826 A CN 202111063826A CN 113720335 A CN113720335 A CN 113720335A
- Authority
- CN
- China
- Prior art keywords
- celestial body
- extraterrestrial celestial
- navigation
- landing
- lander
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 45
- 238000012937 correction Methods 0.000 claims abstract description 46
- 230000001133 acceleration Effects 0.000 claims abstract description 14
- 238000011156 evaluation Methods 0.000 claims abstract description 9
- 238000013213 extrapolation Methods 0.000 claims abstract description 8
- 238000004364 calculation method Methods 0.000 claims abstract description 5
- 230000005484 gravity Effects 0.000 claims description 49
- 238000005259 measurement Methods 0.000 claims description 38
- 230000009466 transformation Effects 0.000 claims description 9
- 238000001514 detection method Methods 0.000 claims description 7
- 239000000523 sample Substances 0.000 claims description 6
- 239000013598 vector Substances 0.000 claims description 5
- 239000011159 matrix material Substances 0.000 claims description 4
- 230000015572 biosynthetic process Effects 0.000 claims description 3
- 238000003786 synthesis reaction Methods 0.000 claims description 3
- 230000008569 process Effects 0.000 abstract description 11
- 230000009467 reduction Effects 0.000 abstract description 2
- 238000004422 calculation algorithm Methods 0.000 description 21
- 238000010586 diagram Methods 0.000 description 8
- 238000004590 computer program Methods 0.000 description 7
- 230000006870 function Effects 0.000 description 6
- 238000012545 processing Methods 0.000 description 4
- 238000003860 storage Methods 0.000 description 4
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 3
- 230000008030 elimination Effects 0.000 description 3
- 238000003379 elimination reaction Methods 0.000 description 3
- 238000012986 modification Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- JXASPPWQHFOWPL-UHFFFAOYSA-N Tamarixin Natural products C1=C(O)C(OC)=CC=C1C1=C(OC2C(C(O)C(O)C(CO)O2)O)C(=O)C2=C(O)C=C(O)C=C2O1 JXASPPWQHFOWPL-UHFFFAOYSA-N 0.000 description 1
- 230000002159 abnormal effect Effects 0.000 description 1
- 238000005266 casting Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 229920006395 saturated elastomer Polymers 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/24—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for cosmonautical navigation
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- Astronomy & Astrophysics (AREA)
- Automation & Control Theory (AREA)
- General Physics & Mathematics (AREA)
- Gyroscopes (AREA)
Abstract
A high fault-tolerant strong autonomous navigation method for the landing of an extraterrestrial celestial body belongs to the technical field of automatic control. The invention comprises the following steps: before the extraterrestrial celestial body lander is opened, inertial navigation extrapolation calculation is carried out to obtain t after the extraterrestrial celestial body lander is openedkMotion attitude data at a moment; after the extraterrestrial celestial body lander is opened, estimating the dynamics of the extraterrestrial celestial body lander during opening in real time according to the angular velocity and the angular acceleration of the extraterrestrial body lander after opening; and carrying out attitude correction or landing navigation on the extraterrestrial celestial body lander in real time according to the parachute opening dynamic evaluation result of the extraterrestrial celestial body lander. The invention aims at the navigation risks which are possibly induced by gyro performance reduction and even saturation caused by the complex and large dynamic landing process of Mars and the field value of a distance and speed measuring sensor, and realizes the strong autonomous landing autonomous navigation aiming at the attitude reference loss risk caused by the saturation of multiple gyroscopes.
Description
Technical Field
The invention relates to a high fault-tolerant strong autonomous navigation method for landing of an extraterrestrial celestial body, and belongs to the technical field of automatic control.
Background
The detector for the first Mars detection task 'Tianqu I' in China enters a transfer orbit from launching, is subjected to orbit correction for many times, captures Mars, surrounds the Mars until safe landing lasts for nearly 10 months. Among them, the Descent Landing process (EDL) is the most critical stage for ensuring the safe Landing of the "one-day-one" probe, and is also the most technically challenging stage.
The EDL process starts from the atmospheric entry point (about 125 km from the mars reference surface) and ends at the surface of the landing mars, with the main tasks of deceleration and maneuvering to ensure safe state (attitude and speed) of the probe in contact with the mars surface and safe landform in the landing zone. The EDL process requires that the speed of the detector is rapidly reduced to approximately 0 from about 1.7 kilometres per hour, and simultaneously a series of actions such as unfolding the leveling wings, opening the parachute, separating the outsole, unfolding the landing buffer mechanism, separating the parachute-back cover assembly, starting ignition of the active engine, avoiding the parachute-back cover assembly and avoiding obstacles are sequentially completed. The process is mainly completed by a guidance and control (GNC) subsystem for landing and inspection tour entering the cabin.
To achieve safe landing, navigation information and event triggering must be prepared reliably. But the design of an EDL process navigation system with a complex unknown mars presents a significant challenge: the parachute surge process inevitably causes large dynamic processes such as large angular acceleration and angular velocity and the like of the landing platform, and if the landing platform has large angular acceleration and angular velocity which cause the multiple (more than 3) gyros to measure saturation, the navigation reference of the system is lost and the task fails; even if the saturation number of the gyroscope is less than or equal to 3, the inertial navigation attitude determination precision is influenced by the cone effect caused by large dynamic, and the landing attitude error and the horizontal velocity are possibly caused to be overlarge, so that the rollover risk is caused; in the initial stage of the large bottom casting of the parachuting section, the angular velocity is very high, so that the number of the available distance and speed measuring sensor beams is small at this time, and the distance and speed measuring fault detection algorithm is difficult to work, so that if a fault beam occurs, inertial navigation can introduce the fault beam to cause the fault beam to be biased, and a normal beam is difficult to introduce subsequently, so that the risk of navigation error exists.
Disclosure of Invention
The technical problem solved by the invention is as follows: the method overcomes the defects of the prior art, provides a high fault tolerance and strong autonomous navigation method for landing of an extraterrestrial celestial body, and realizes strong autonomous landing autonomous navigation aiming at the navigation risks which are possibly induced by gyro performance reduction and even saturation possibly caused by the complex and large dynamic landing process of mars and the occurrence of field values of a distance and speed measuring sensor and the attitude reference loss risk caused by multi-gyro saturation; aiming at the problem of the descending precision of inertial navigation attitude determination caused by the large dynamic cone effect of the parachuting section and the risk that fault beams are possibly introduced into inertial navigation, a multi-level fault-tolerant distance measurement and speed measurement correction algorithm is provided.
The technical solution of the invention is as follows: a high fault-tolerant strong autonomous navigation method for the landing of an extraterrestrial celestial body comprises the following steps:
before the extraterrestrial celestial body lander is opened, inertial navigation extrapolation calculation is carried out to obtain t after the extraterrestrial celestial body lander is openedkMotion attitude data at a moment;
after the extraterrestrial celestial body lander is opened, estimating the dynamics of the extraterrestrial celestial body lander during opening in real time according to the angular velocity and the angular acceleration of the extraterrestrial body lander after opening;
and carrying out attitude correction or landing navigation on the extraterrestrial celestial body lander in real time according to the parachute opening dynamic evaluation result of the extraterrestrial celestial body lander.
Further, the method for evaluating the dynamics of the extraterrestrial celestial body lander during parachute opening comprises the following steps:
calculating an evaluation parameter ωmaxAnd amaxAnd setting an evaluation threshold value according to the gyro index
If it is
Or
The dynamic state of the extraterrestrial celestial body lander during parachute opening is set as the ultrahigh dynamic state, and the dynamic identifier is Flagdyn1, and carrying out strong autonomous navigation landing control on the extraterrestrial celestial body lander;
if it is
And is
The dynamic state of the extraterrestrial celestial body lander during parachute opening is set as the low dynamic state, and the dynamic identifier is FlagdynWhen the distance is equal to 0, performing inertial navigation and distance measurement and speed measurement correction on the extraterrestrial celestial body lander;
if omegamax,amaxIf the conditions of ultrahigh dynamic state and low dynamic state are not met, the dynamic state of the extraterrestrial celestial body lander during parachute opening is set as high dynamic state, and the dynamic identifier is FlagdynAnd 2, correcting the attitude of the extraterrestrial celestial body lander.
Further, in the evaluation of the dynamics of the extraterrestrial celestial lander when opening the parachute, [ t [ [ t ]k,tk+1]Angular acceleration of time being
Wherein, ω isk、ωk+1Angular velocities at the time k and k +1, respectively; [ t ] ofk,tk+1]The maximum value of angular acceleration at the time is amax。
Further, the strong autonomous navigation landing control comprises the following steps:
the gravity direction is obtained according to the measured value of the distance measuring sensor;
based on the direction of the gravity, calculating the beam incident angle of the speed measuring sensor to realize the optimal selection of the speed measuring beam;
determining a gravity direction based on dynamics according to the measurement quantity of the speed measuring sensor and the IMU speed increment;
based on the projection of the gravity direction under the inertial system based on dynamics, counting the variance of the estimated value of the gravity direction;
carrying out convergence judgment on the determined result of the gravity direction dynamics; if the navigation is converged, the navigation is reinitialized in the track, and the dynamic identifier is collocated to Flagdyn0; if not, continuously judging whether the gravity direction dynamics determination result is converged according to the time sequence until the gravity direction dynamics determination result is converged.
Further, the method for correcting the attitude of the extraterrestrial celestial body lander comprises the following steps:
calculating a projection of a dynamics-based gravitational direction estimate in a system
Projection of gravity direction estimated value given by inertial navigation obtained from inertial navigation state under the system
Calculating when the gravitational direction dynamics determination result converges
And
angle of (theta)k；
According to
And
angle of (theta)kCorrecting attitude of inertial navigation system of extraterrestrial celestial body lander
The corresponding quaternion is qins,kJuxtaposing the dynamic Flag as Flagdyn＝0。
Further, the inertial navigation and distance and speed measurement correction of the extraterrestrial celestial body lander comprises distance and speed measurement fault tolerance correction.
Further, the ranging fault tolerance correction comprises the following steps:
calculating the height obtained by the ith wave beam of the distance measuring sensor;
fault detection is carried out on the distance measuring sensor, and a plurality of data which are closest to a data center in effective beam data are selected for distance measuring correction;
correcting and fusing the distance measurement height;
and updating the inertial navigation position according to the corrected and fused ranging height.
Further, the speed measurement fault tolerance correction method comprises the following steps:
calculating a residual error of the wave beam observed quantity of the speed measuring sensor relative to inertial navigation;
carrying out fault detection on the speed measuring sensor, and selecting a plurality of data which are closest to a data center in the effective beam data for speed measurement correction;
correcting the speed of the ith wave beam of the speed measuring sensor along the wave beam direction;
and performing three-dimensional synthesis on the corrected speed to obtain the corrected speed in the inertial system.
Further, the gravity direction is obtained according to the measured value of the distance measuring sensor, specifically: the gravity direction of the distance measuring sensor is
By
Giving out; where sgn is a sign function,. represents a dot product of two vectors, pR(1),pR(2),pR(3)The projection of the pointing directions of three non-coplanar beams of the distance measuring sensor under a body coordinate system,
observations for the three beams;
incident angle alpha of ith wave beam of speed measuring sensoriIs composed of
Wherein p isV(i)The projection of the ith wave velocity of the speed measuring sensor under the system is obtained; remember that satisfies alphai<αmaxThe number of wave beams of the sensor speed-measuring sensor is m, wherein alphamaxIs the maximum beam incident angle; if m is less than or equal to 4, sending out work instructions of the m wave beams; if m is>4, numbering the 4 beams in m as s1,s2,s3,s4The corresponding beam pointing is noted as:
the 4 beams are selected such that
Minimum; wherein λ ismax() For maximum eigenvalue operator, m takes 4 to have
And taking 4 beams in the combination with the minimum K for the subsequent steps.
Further, the determining the convergence of the gravitational direction dynamics result specifically includes:
tkthe gravity direction estimation value under the moment inertia system is
Calculating the gravity direction estimation value transformation quantity of 8 beats:
judging convergence of the determined result of the gravity direction dynamics, and performing on-track navigation reinitialization once convergence occurs; if the following equation holds, the gravitational direction dynamics determination has converged
Wherein:
is a preset threshold value;
if the above expression is not satisfied, repeatedly calculating the gravity direction estimation value transformation quantity of 8 beats according to time sequence until the expression is satisfied;
if convergence, navigation re-initialization is performed:
wherein R ismIs a reference radius of the spark,
s is the number of usable beams of the ranging sensor,
is the velocity of the probe under the inertial system,
the speed of the probe under the system is the same,
is the attitude transformation matrix from the main system to the inertial system.
Compared with the prior art, the invention has the advantages that:
(1) the invention realizes the autonomous landing navigation adapting to different dynamic states of parachute opening by the gravity direction determination technology based on the multiple sensors;
(2) the multi-beam fault elimination algorithm of the distance measuring sensor is realized by a measuring and comparing method based on the median;
(3) the multi-beam fault elimination algorithm of the speed measuring sensor is realized by a residual error comparison method based on the median.
Drawings
FIG. 1 is a navigation flow chart according to the present invention.
Detailed Description
In order to better understand the technical solutions, the technical solutions of the present application are described in detail below with reference to the drawings and specific embodiments, and it should be understood that the specific features in the embodiments and examples of the present application are detailed descriptions of the technical solutions of the present application, and are not limitations of the technical solutions of the present application, and the technical features in the embodiments and examples of the present application may be combined with each other without conflict.
The method for extraterrestrial celestial body landing high fault-tolerance strong autonomous navigation provided by the embodiments of the present application is further described in detail below with reference to the accompanying drawings of the specification, and specific implementations may include (as shown in fig. 1):
1) inertial navigation extrapolation before parachute opening
2) Parachute opening dynamic assessment
Calculating the angular velocity and the angular acceleration when the umbrella is opened according to the angular increment of the adjacent sampling period of the gyroscope when the umbrella is opened, and evaluating the dynamics when the umbrella is opened to obtain the following 3 possibilities:
a) ultrahigh dynamic state: at least 4 gyroscopic saturations occur, and inertial navigation attitude reference is lost;
b) high dynamic state: the gyro saturation is less, the attitude reference is not lost, but the inertial navigation attitude determination precision is sharply reduced;
c) low dynamic state: the attitude angular velocity is small, and the gyro saturation phenomenon does not occur.
3) Ultra-high dynamic strong autonomous landing navigation
For ultrahigh dynamics, navigation attitude reference is lost, so that errors occur, and inertial navigation information is not reliable any more. At this time, the strong autonomous navigation algorithm is started.
a) Firstly, a geometric method is utilized to solve the gravity direction according to the measured value of the distance measuring sensor.
b) Based on the gravity direction determined geometrically, calculating the beam incident angle of the speed measuring sensor to realize the optimal selection of the speed measuring beam;
c) and realizing the accurate determination of the gravitational direction dynamics according to the measurement quantity of the speed measuring sensor and the IMU speed increment.
d) The amount of change in the gravitational direction estimate is calculated.
e) And (4) judging the convergence of the determined result of the gravity direction dynamics, and once the result is converged, finishing the reinitialization of navigation and switching back to a normal navigation scheme, namely inertial navigation extrapolation and distance measurement and speed measurement correction.
4) Vertical attitude correction algorithm based on speed measuring sensor and IMU under high dynamic state
For high dynamic state, the inertial navigation attitude determination precision is sharply reduced, the rollover risk during landing can be caused, and at the moment, a vertical attitude correction algorithm based on a speed measuring sensor and an IMU is started
a) Firstly, calculating the projection of the gravity direction estimated value based on dynamics under the system according to the step c) of the step 3).
b) The projection of the gravity direction estimated value given by inertial navigation in the system can be obtained from the inertial navigation state
c) And correcting the inertial navigation vertical attitude, and switching back to the normal mode.
5) Inertial navigation and distance measurement and speed measurement fault-tolerant correction algorithm
In the autonomous navigation algorithm of inertial extrapolation and ranging and speed measurement correction, the elimination of abnormal values is required, so the invention provides a fault-tolerant ranging and speed measurement correction algorithm suitable for on-satellite operation.
a) And (4) ranging fault-tolerant correction algorithm.
b) And (5) speed measurement fault tolerance correction algorithm.
In the scheme provided by the embodiment of the application, the method for the extraterrestrial celestial body landing high fault tolerance strong autonomous navigation specifically comprises the following processes:
1) and performing inertial navigation extrapolation before opening the umbrella. Noting the lander at tk-1The inertial position and velocity at the moment are respectively
At tk-1The attitude quaternion of the main system of the time lander relative to the inertial system is qk-1. Then the inertial navigation equation, the gyroscope and the accelerometer are in [ t ]k-1,tk]Can be extrapolated to obtain the lander at tkInertial position of time
Speed of rotation
And attitude quaternion qk
2) Parachute opening dynamic assessment
Note t after opening umbrellakThe angular velocities measured by the gyro at the moment are respectively omegakThen [ tk,tk+1]The magnitude of the angular acceleration at that moment is denoted as aω,kIt can be calculated from the following formula:
get
Wherein t issCorresponding to the moment of opening the umbrella.
According to omegamaxAnd amaxThe dynamics of opening the umbrella are evaluated to obtain the following 3 possibilities:
a) if it is
Or
(
A threshold set according to a gyro index). At this time, a plurality of gyros are saturated, so that the number of unsaturated gyros is less than 3,the dynamic state is ultrahigh dynamic state, and the dynamic identifier is FlagdynIt is set to 1.
b) If it is
And is
(
A threshold set according to a gyro index). At this time, both the angular velocity and the angular acceleration are small. The dynamic is low dynamic, set Flagdyn＝0。
c) If omegamax,amaxIf the dynamic condition is not met, and the dynamic condition is not met, the available gyro of the gyro is at least 3 at this time, but the performance of the gyro is reduced due to large angular velocity and angular acceleration. The dynamic state is high dynamic state and Flag is setdyn＝2。
3) Ultra-high dynamic strong autonomous landing navigation
If FlagdynWhen the navigation attitude reference is lost, the navigation attitude reference is wrong, and inertial navigation information is not reliable any more. At this time, the strong autonomous navigation algorithm is started as follows:
a) firstly, a geometric method is utilized to solve the gravity direction according to the measured value of the distance measuring sensor.
Recording the projection of the direction of 3 non-coplanar beams of the distance measuring sensor under the body coordinate system as pR(1),pR(2),pR(3)The observed quantities of the three beams are respectively
Then the projection of the gravity direction in the body coordinate system can be determined by using a geometric method and recorded as
Given by:
where sgn is a sign function,. represents a dot product of two vectors.
b) And calculating the beam incident angle of the speed measuring sensor based on the gravity direction determined geometrically to realize the optimal selection of the speed measuring beam.
Recording the incident angle alpha of ith wave beam of speed sensoriThen it can be calculated from the following equation:
wherein p isV(i)The projection of the ith wave velocity of the speed measuring sensor under the system. Remember that satisfies alphai<αmaxThe number of wave beams of the sensor speed-measuring sensor is m, wherein alphamaxIs the maximum beam incident angle. If m is less than or equal to 4, the work orders of the m wave beams are sent out. If m is>4, numbering the 4 beams in m as s1,s2,s3,s4The corresponding beam pointing is noted as:
the selection of these 4 beams minimizes the following:
λmax() For maximum eigenvalue operator, m takes 4 to have
And taking 4 beams in the combination with the minimum K for working.
c) And accurately determining the gravity direction based on dynamics according to the measurement quantity of the speed measuring sensor and the IMU speed increment. Recording the velocity sensor at tk,tk+1The speed under the system calculated at the moment is:
then the projection of the gravity direction under the system can be determined by using the kalman filtering technique, and the equation of state is given by
In the above equation S () is a cross-product,
for the projection of the angular velocity of the main system of the lander relative to the inertial system under the main system, obtained by gyroscopic measurements, fbThe specific force is measured by an accelerometer, and g is the gravitational acceleration of the system. The measurement equation corresponding to the ith beam is:
wherein:
for the velocity measurement of the ith beam, 01×3Is a 1 x 3 zero vector. V is(i)The noise is measured for the speed of the ith beam.
d) And carrying out statistics on the gravity direction estimation value variance based on the projection of the gravity direction based on dynamics in the inertial system.
Remember one step tkG of timebThe estimated value is
Unit vector of
An estimate of the direction of gravity in the inertial system (denoted as
) Given by:
wherein
The attitude transformation matrix from the main system to the inertial system is obtained by extrapolation of the gyroscope. Calculating the gravity direction estimation value transformation amount of 8 beats, which is given by the following formula:
e) and judging the convergence of the determined result of the gravity direction dynamics, and once the result is converged, performing on-track navigation reinitialization. The gravitational direction dynamics determination has converged if the following holds
Wherein:
the threshold value is typically set to 0.5 deg.. If the formula (10) does not hold, repeating the steps a) to d) until the hold is unknown. And if the formula (10) is satisfied, performing navigation reinitialization:
wherein R ismIs a reference radius of the spark,
and s is the number of usable wave beams of the distance measuring sensor.
After the initialization is completed, Flag is setdynAnd (5) when the navigation algorithm is equal to 0, switching the navigation algorithm to a normal mode, and performing inertial navigation and ranging speed measurement correction (see the step 5).
4) Vertical attitude correction algorithm based on speed measuring sensor and IMU under high dynamic state
If Flag is presentdynIf the working condition is a high dynamic environment, the inertial navigation attitude determination precision is sharply reduced, the rollover risk during landing may be caused, and at the moment, a vertical attitude correction algorithm based on a speed measurement sensor and an IMU is started
a) Firstly, according to step c) of step 2), calculating the projection of the gravity direction estimated value based on dynamics under the system
b) The projection of the gravity direction estimated value given by inertial navigation in the system can be obtained from the inertial navigation state and is recorded as
Given by:
wherein
Are each tkInertial position of inertial navigation output and attitude transformation matrix of inertial system to the main system.
c) When the formula (10) is satisfied, calculating
And
angle of (theta)k
The attitude quaternion to be corrected is then:
wherein ikFor a rotating euler shaft, given by:
let tkAttitude of time inertial navigation system
The corresponding quaternion is qins,kThe corrected quaternion is qkThen, there are:
wherein
Is a multiplication of quaternions.
After completing the formula (16), Flag is setdynAnd (5) when the navigation algorithm is equal to 0, switching to a normal mode, and performing inertial navigation and ranging speed measurement correction (see the step 5).
5) Inertial navigation and distance measurement and speed measurement fault-tolerant correction algorithm
If Flag is presentdynAnd if the distance is equal to 0, performing inertial navigation and distance measurement fault-tolerant correction algorithm.
a) Ranging fault tolerant correction
i. Calculating the height obtained by the ith wave beam of the distance measuring sensor and recording the height
Given by:
wherein
The calculation formula is shown in formula (12) for the gravity direction of inertial navigation calculation.
And ii, carrying out fault detection on the ranging sensor. The number of effective wave beams of the distance measuring sensor is recorded as s, and the number of the effective wave beams is recorded as i1,i2,…,isCalculating
Median of (2), note
Get
The smallest and next smallest two beams, numbered k, are used for height correction1,k2。
Range height correction fusion. The ranging height correction equation is given by:
wherein beta isiIs the fusion coefficient of the ith beam
hinsThe calculated altitude is the inertial navigation. Kh(i)Is a ranging correction coefficient.
The inertial navigation position update equation is given by:
b) speed measurement fault tolerance correction
i. And calculating the residual error of the wave beam observed quantity of the speed measuring sensor relative to the inertial navigation. The residual of the ith beam is denoted as Δ v(i)Given by:
wherein:
measuring the quantity v for the velocity of the ith beamins,(i)The component of the calculated ground speed for inertial navigation along the beam direction.
And ii, detecting and selecting a speed measurement fault. The number of effective wave beams of the distance measuring sensor is recorded as n, and the number of the effective wave beams is recorded as i1,i2,…,inCalculating Δ v(j)(j＝i1,i2,…,in) Median of (d), noted as Δ vmed. Take | Δ vmed-Δv(j)The smallest | four beams are used for height correction, and the four beams are numbered m1,m2,m3,m4。
And iii, recording the speed correction quantity of the ith wave beam of the speed measuring sensor along the wave beam direction as delta v(i)Given by:
δv(i)＝Kv(i)Δv(i),i＝m1,…,m4 (21)
Kv(i)the correction coefficient of the ith wave beam is measured.
Velocity-corrected three-dimensional synthesis. The three-dimensional correction of velocity is:
it can be given by:
the above formula has further fault-tolerant capability by adopting a least square algorithm.
v. thus the velocity correction equation under the inertial system can be given by:
further, in the present invention,
in one possible implementation of the solution according to the invention,
in one possible implementation form of the method,
alternatively to this, the first and second parts may,
a computer-readable storage medium having stored thereon computer instructions which, when executed on a computer, cause the computer to perform the method of fig. 1.
As will be appreciated by one skilled in the art, embodiments of the present application may be provided as a method, system, or computer program product. Accordingly, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment or an embodiment combining software and hardware aspects. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, optical storage, and the like) having computer-usable program code embodied therein.
The present application is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the application. It will be understood that each flow and/or block of the flow diagrams and/or block diagrams, and combinations of flows and/or blocks in the flow diagrams and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
It will be apparent to those skilled in the art that various changes and modifications may be made in the present application without departing from the spirit and scope of the application. Thus, if such modifications and variations of the present application fall within the scope of the claims of the present application and their equivalents, the present application is intended to include such modifications and variations as well.
Those skilled in the art will appreciate that those matters not described in detail in the present specification are well known in the art.
Claims (10)
1. A high fault-tolerant strong autonomous navigation method for the landing of an extraterrestrial celestial body is characterized by comprising the following steps:
before the extraterrestrial celestial body lander is opened, inertial navigation extrapolation calculation is carried out to obtain t after the extraterrestrial celestial body lander is openedkMotion attitude data at a moment;
after the extraterrestrial celestial body lander is opened, estimating the dynamics of the extraterrestrial celestial body lander during opening in real time according to the angular velocity and the angular acceleration of the extraterrestrial body lander after opening;
and carrying out attitude correction or landing navigation on the extraterrestrial celestial body lander in real time according to the parachute opening dynamic evaluation result of the extraterrestrial celestial body lander.
2. The method for high fault-tolerant strong autonomous navigation of extraterrestrial celestial body landing according to claim 1, wherein the method for evaluating the dynamics of extraterrestrial celestial body lander during parachute opening is as follows:
calculating an evaluation parameter ωmaxAnd amaxAnd setting an evaluation threshold value according to the gyro index
If it is
Or
The dynamic state of the extraterrestrial celestial body lander during parachute opening is set as the ultrahigh dynamic state, and the dynamic identifier is Flagdyn1, and carrying out strong autonomous navigation landing control on the extraterrestrial celestial body lander;
if it is
And is
The dynamic state of the extraterrestrial celestial body lander during parachute opening is set as the low dynamic state, and the dynamic identifier is FlagdynWhen the distance is equal to 0, performing inertial navigation and distance measurement and speed measurement correction on the extraterrestrial celestial body lander;
if omegamax,amaxIf the conditions of ultrahigh dynamic state and low dynamic state are not met, the dynamic state of the extraterrestrial celestial body lander during parachute opening is set as high dynamic state, and the dynamic identifier is FlagdynAnd 2, correcting the attitude of the extraterrestrial celestial body lander.
3. The method for extraterrestrial celestial body landing high fault-tolerant strong autonomous navigation according to claim 2, characterized in that: in the evaluation of the dynamics of the extraterrestrial celestial lander during parachute opening, [ t [ [ t ]k,tk+1]Angular acceleration of time being
Wherein, ω isk、ωk+1Angular velocities at the time k and k +1, respectively; [ t ] ofk,tk+1]The maximum value of angular acceleration at the time is amax。
4. The method for high fault-tolerant strong autonomous navigation for extraterrestrial celestial body landing according to claim 2, wherein the strong autonomous navigation landing control comprises the following steps:
the gravity direction is obtained according to the measured value of the distance measuring sensor;
based on the direction of the gravity, calculating the beam incident angle of the speed measuring sensor to realize the optimal selection of the speed measuring beam;
determining a gravity direction based on dynamics according to the measurement quantity of the speed measuring sensor and the IMU speed increment;
based on the projection of the gravity direction under the inertial system based on dynamics, counting the variance of the estimated value of the gravity direction;
carrying out convergence judgment on the determined result of the gravity direction dynamics; if the navigation is converged, the navigation is reinitialized in the track, and the dynamic identifier is collocated to Flagdyn0; if not, continuously judging whether the gravity direction dynamics determination result is converged according to the time sequence until the gravity direction dynamics determination result is converged.
5. The method for extraterrestrial celestial body landing high fault-tolerant strong autonomous navigation according to claim 2, wherein the method for attitude correction of extraterrestrial celestial body lander comprises the following steps:
calculating a projection of a dynamics-based gravitational direction estimate in a system
Projection of gravity direction estimated value given by inertial navigation obtained from inertial navigation state under the system
Calculating when the gravitational direction dynamics determination result converges
And
angle of (theta)k；
According to
And
angle of (theta)kCorrecting attitude of inertial navigation system of extraterrestrial celestial body lander
The corresponding quaternion is qins,kJuxtaposing the dynamic Flag as Flagdyn＝0。
6. The method for extraterrestrial celestial body landing high-fault-tolerance and strong-autonomous navigation according to claim 2, wherein the inertial navigation and ranging and speed measurement correction of the extraterrestrial celestial body lander comprises ranging fault-tolerance correction and speed measurement fault-tolerance correction.
7. The method for high fault-tolerant strong autonomous navigation of landing of an extraterrestrial celestial body according to claim 6, wherein the ranging fault-tolerant correction comprises the following steps:
calculating the height obtained by the ith wave beam of the distance measuring sensor;
fault detection is carried out on the distance measuring sensor, and a plurality of data which are closest to a data center in effective beam data are selected for distance measuring correction;
correcting and fusing the distance measurement height;
and updating the inertial navigation position according to the corrected and fused ranging height.
8. The method for high fault-tolerant strong autonomous navigation for landing of an extraterrestrial celestial body according to claim 6, wherein the speed measurement fault-tolerant correction comprises the following steps:
calculating a residual error of the wave beam observed quantity of the speed measuring sensor relative to inertial navigation;
carrying out fault detection on the speed measuring sensor, and selecting a plurality of data which are closest to a data center in the effective beam data for speed measurement correction;
correcting the speed of the ith wave beam of the speed measuring sensor along the wave beam direction;
and performing three-dimensional synthesis on the corrected speed to obtain the corrected speed in the inertial system.
9. The method for high fault-tolerant strong autonomous navigation for landing of an extraterrestrial celestial body according to claim 1, wherein the gravity direction is obtained according to a measured value of a ranging sensor, specifically: the gravity direction of the distance measuring sensor is
By
Giving out; where sgn is a sign function,. represents a dot product of two vectors, pR(1),pR(2),pR(3)The projection of the pointing directions of three non-coplanar beams of the distance measuring sensor under a body coordinate system,
observations for the three beams;
incident angle alpha of ith wave beam of speed measuring sensoriIs composed of
Wherein p isV(i)The projection of the ith wave velocity of the speed measuring sensor under the system is obtained; remember that satisfies alphai<αmaxThe number of wave beams of the sensor speed-measuring sensor is m, wherein alphamaxIs the maximum beam incident angle; if m is less than or equal to 4, sending out work instructions of the m wave beams; if m is>4, numbering the 4 beams in m as s1,s2,s3,s4The corresponding beam pointing is noted as:
the 4 beams are selected such that
Minimum; wherein λ ismax() For maximum eigenvalue operator, m takes 4 to have
And taking 4 beams in the combination with the minimum K for the subsequent steps.
10. The method for high fault-tolerant strong autonomous navigation for landing of an extraterrestrial celestial body according to claim 1, wherein the determining of convergence of the gravitational direction dynamics specifically includes:
tkthe gravity direction estimation value under the moment inertia system is
Calculating the gravity direction estimation value transformation quantity of 8 beats:
judging convergence of the determined result of the gravity direction dynamics, and performing on-track navigation reinitialization once convergence occurs; if the following equation holds, the gravitational direction dynamics determination has converged
Wherein:
is a preset threshold value;
if the above expression is not satisfied, repeatedly calculating the gravity direction estimation value transformation quantity of 8 beats according to time sequence until the expression is satisfied;
if convergence, navigation re-initialization is performed:
wherein R ismIs a reference radius of the spark,
s is the number of usable beams of the ranging sensor,
is the velocity of the probe under the inertial system,
the speed of the probe under the system is the same,
is the attitude transformation matrix from the main system to the inertial system.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111063826.9A CN113720335A (en) | 2021-09-10 | 2021-09-10 | High-fault-tolerance strong autonomous navigation method for landing of extraterrestrial celestial body |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111063826.9A CN113720335A (en) | 2021-09-10 | 2021-09-10 | High-fault-tolerance strong autonomous navigation method for landing of extraterrestrial celestial body |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN113720335A true CN113720335A (en) | 2021-11-30 |
Family
ID=78683311
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111063826.9A Pending CN113720335A (en) | 2021-09-10 | 2021-09-10 | High-fault-tolerance strong autonomous navigation method for landing of extraterrestrial celestial body |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113720335A (en) |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20110153205A1 (en) * | 2009-12-22 | 2011-06-23 | General Electric Company | Relative Navigation System |
| CN104251711A (en) * | 2014-09-11 | 2014-12-31 | 上海卫星工程研究所 | Deep space exploration combined autonomous navigation ground verification system and methods thereof |
| CN104913784A (en) * | 2015-06-19 | 2015-09-16 | 北京理工大学 | Method for autonomously extracting navigation characteristic on surface of planet |
| CN113030517A (en) * | 2021-02-18 | 2021-06-25 | 北京控制工程研究所 | Attitude correction method by using speed measuring sensor in Mars landing process |
-
2021
- 2021-09-10 CN CN202111063826.9A patent/CN113720335A/en active Pending
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20110153205A1 (en) * | 2009-12-22 | 2011-06-23 | General Electric Company | Relative Navigation System |
| CN104251711A (en) * | 2014-09-11 | 2014-12-31 | 上海卫星工程研究所 | Deep space exploration combined autonomous navigation ground verification system and methods thereof |
| CN104913784A (en) * | 2015-06-19 | 2015-09-16 | 北京理工大学 | Method for autonomously extracting navigation characteristic on surface of planet |
| CN113030517A (en) * | 2021-02-18 | 2021-06-25 | 北京控制工程研究所 | Attitude correction method by using speed measuring sensor in Mars landing process |
Non-Patent Citations (1)
| Title |
|---|
| 黄翔宇，等: "天问一号进入下降着陆过程GNC关键技术", 国际太空, no. 7, pages 14 - 18 * |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US9978285B2 (en) | Autonomous and non-autonomous dynamic model based navigation system for unmanned vehicles | |
| EP1862764B1 (en) | High speed gyrocompass alignment via multiple kalman filter based hypothesis testing | |
| US8185309B2 (en) | Enhanced inertial system performance | |
| EP2259023B1 (en) | Inertial navigation system error correction | |
| US5574650A (en) | Method and apparatus for calibrating the gyros of a strapdown inertial navigation system | |
| US20090182503A1 (en) | Method for determining the attitude, position, and velocity of a mobile device | |
| CN112414413B (en) | Relative angular momentum-based angle-only maneuvering detection and tracking method | |
| CN110567457B (en) | Inertial navigation self-detection system based on redundancy | |
| RU2762143C2 (en) | System for determining course and angular spatial position made with the possibility of functioning in polar region | |
| EP4053504B1 (en) | Systems and methods for model based inertial navigation for a spinning projectile | |
| Chang-Siu et al. | Time-varying complementary filtering for attitude estimation | |
| CN113030517B (en) | Attitude correction method by using speed measuring sensor in Mars landing process | |
| CN113483759B (en) | Integrity protection level calculation method for GNSS, INS, vision integrated navigation system | |
| CN113959462A (en) | Quaternion-based inertial navigation system self-alignment method | |
| US5505410A (en) | Instrument calibration method including compensation of centripetal acceleration effect | |
| CN113720335A (en) | High-fault-tolerance strong autonomous navigation method for landing of extraterrestrial celestial body | |
| CN111351490A (en) | Method for quickly reconstructing inertial navigation reference in planet landing process | |
| CN113703019B (en) | Fault processing method of navigation system, electronic equipment and storage medium | |
| Vaganay et al. | Attitude estimation for a vehicle using inertial sensors | |
| CN110132267B (en) | Optical fiber inertial navigation system of air-space-ground integrated aircraft and optical fiber inertial navigation on-orbit alignment method | |
| Bai et al. | A novel method of attitude measurement for floated inertial platform using optical sensors | |
| Stovner et al. | GNSS-antenna lever arm compensation in aided inertial navigation of UAVs | |
| Grof et al. | Runway relative positioning of aircraft with IMU-camera data fusion | |
| JP2525072B2 (en) | Method of initializing flying body guidance device | |
| RU2798209C1 (en) | Method for autonomous positioning of free-platform navigation system in the process of controlled object exit from dive after water touchdown |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |