Deep space probe optical navigation terrestrial landmark hierarchical matching pose estimation method

Abstract

This invention discloses a hierarchical matching pose estimation method for optical navigation landmarks in deep space probes, belonging to the field of autonomous navigation technology for deep space exploration. The implementation method is as follows: A probe pose dynamics model and an optical observation model are established to obtain the probe's initial position, attitude, and camera parameters, used to predict the probe's state and the set of visible landmarks at the observation time; a navigation landmark observability index function is constructed based on the Fisher information matrix; a hierarchical landmark selection and matching strategy is constructed, considering landmark visibility constraints, and selecting a primary landmark set, a secondary landmark set, and a candidate landmark set from the visible landmark set at the observation time based on the probe's trajectory and the set of visible landmarks at the observation time; sequential matching is performed during online matching to suppress the decrease in navigation information caused by landmark matching loss; and step-by-step matching is used during probe navigation to achieve rapid landmark identification and high-precision pose estimation.

Description

Technical Field

[0001] This invention relates to a method for hierarchical matching pose estimation of optical navigation landmarks for deep space probes, belonging to the field of autonomous navigation technology for deep space exploration. Background Technology

[0002] In deep space exploration missions, probes typically need to achieve scientific objectives such as target observation, data measurement, and sample collection, which places high demands on the probe's precise positioning and pointing. Ground-based telemetry and control methods based on deep space networks suffer from significant communication delays and limited attitude estimation accuracy, making it difficult to provide real-time, high-precision navigation information for the probe. Therefore, the probe must possess autonomous navigation capabilities. The surfaces of extraterrestrial objects are rich in natural information; the various types of landmarks existing on celestial surfaces can provide navigation information for the probe. The probe uses the landmark information acquired by optical sensors to perform autonomous attitude estimation through onboard computer calculations. Therefore, landmark-based attitude estimation methods are a key research focus in the optical navigation of deep space probes.

[0003] During autonomous optical navigation, deep space probes use navigation cameras to photograph target celestial bodies, acquiring a large amount of landmark information for state estimation. Due to the limited computing power of the onboard computer, it is necessary to select a subset of landmarks to meet real-time requirements. Simultaneously, the probe's pose is uncertain; if the selected landmarks are located at the edge of the nominal field of view, pose perturbations may cause them to fall outside the actual field of view, leading to lost navigation information and compromising robustness.

[0004] To address the above issues, the first technology [1] (Hu R, Huang X, Xu C. Visual navigation with fast landmark selection based on error analysis for asteroid descent stage[J]. Advances in Space Research, 2021, 68(9): 3765-3780.) analyzed the measurement error and systematic error, designed a fast landmark selection method based on error propagation analysis, and defined two landmark geometric indices for minimizing the approximate state uncertainty. The first technology [2] (Xu C, Huang X, Li M, et al. Landmark database selection for vision-aided inertial navigation in planetarylanding missions[J]. Aerospace Science and Technology, 2021, 118: 107040.) proposed a landmark selection method based on linearized covariance estimation, which uses linearized covariance recursively to analyze the visibility of landmarks and their impact on navigation performance, and combines a greedy algorithm to achieve the optimal selection of the landmark set. However, none of the above methods consider alternative landmarks in the case of landmark loss. Furthermore, when considering pose uncertainty, the selection of landmarks is relatively conservative, which can easily lead to a decrease in the amount of navigation information of the selected landmarks and affect the accuracy of pose estimation.

[0005] Existing methods for tiered matching of landmarks for optical navigation of deep space probes do not consider landmark backup schemes when landmarks are invisible or matching fails, nor do they establish a tiered matching strategy for landmarks, making it difficult to meet the robustness and accuracy requirements of optical navigation pose estimation for deep space probes. Summary of the Invention

[0006] To address the high precision, robustness, and real-time requirements of optical navigation pose estimation for deep space probes, this invention aims to provide a landmark-level matching pose estimation method. Based on the Fisher information matrix, a landmark configuration observability evaluation index function is designed, and a landmark-level optimization and matching strategy is constructed. Considering landmark visibility constraints, primary landmarks, secondary landmarks, and candidate landmark sets are selected from the visible landmark library in each frame. Sequential matching is performed during online matching to suppress the decrease in navigation information caused by landmark matching loss, achieving high-precision navigation state estimation and improving the accuracy and robustness of optical navigation pose estimation.

[0007] The present invention is achieved through the following technical solution.

[0008] The proposed method for graded matching pose estimation of optical navigation landmarks for deep space probes, disclosed in this invention, includes the following steps:

[0009] Step 1: Establish the detector pose dynamics model and optical observation model to obtain the detector's initial position, attitude, and camera parameters, which are used to predict the detector's state and the visible landmark set at the observation time.

[0010] Step 1.1: Establish a detector pose dynamics model to predict the detector state.

[0011] Define the fixed coordinate system of the extraterrestrial object as L, and the coordinate system of the detector camera as c. The detector system state is as follows:

[0012]

[0013] The dynamic equations of the probe near the target celestial body are:

[0014]

[0015] in, This is system state noise. Let be the system noise covariance matrix. and These represent the probe's position and velocity in a fixed coordinate system relative to the extraterrestrial body. This represents the rotational velocity of the target celestial body in a fixed coordinate system. Let be the attitude quaternion from the fixed coordinate system of the extraterrestrial object to the coordinate system of the detector camera. This represents the angular velocity of the probe's rotation relative to the fixed coordinate system of the extraterrestrial object, in the probe's camera coordinate system. This represents the gravitational acceleration received by the probe in a fixed coordinate system relative to an extraterrestrial object. The thrust acceleration experienced by the probe in a fixed coordinate system relative to an extraterrestrial object. This represents a 4th-order antisymmetric matrix.

[0016] Let the partial derivatives of the system's state matrix be:

[0017]

[0018] After obtaining the initial position and attitude information of the detector, the detector's process state is predicted using the detector's dynamic equations.

[0019] Step 1.2: Establish the optical observation model of the detector, and use the predicted state of the detector's motion process and camera parameters to obtain the visible landmark set and its observations at the observation time.

[0020] Typically, the detector's attitude and position contain errors. Let the nominal pose of the detector's camera be represented as... The position of the landmark in the detector camera coordinate system is defined as follows:

[0021]

[0022] in, This indicates the position of the landmass in a fixed coordinate system relative to extraterrestrial objects. This indicates the position of the land marker in the detector's camera coordinate system. and These represent the position and attitude errors of the detector, respectively.

[0023] The observation equation for n landmarks is:

[0024]

[0025] in, For measuring noise.

[0026] To determine whether a landmark falls within the camera's field of view, based on the camera observation model, the following definition is used:

[0027]

[0028] in , Let and represent the angles in the x and y directions between the line of sight of landmark j and the camera's optical axis during the k-th observation. Considering the uncertainty of the camera pose, field-of-view perturbation angle compensation needs to be added. , Let the absolute values ​​of the half-angles of the camera's horizontal and vertical fields of view be respectively... , Then the visibility criterion for landmark j at time k is:

[0029]

[0030] Therefore, the set of visible landmarks at the nominal observation time k is:

[0031]

[0032] Step 2: Construct the observability index function for navigation landmarks; Based on the probe trajectory and the set of visible landmarks at the observation time, select the main landmark set, the auxiliary landmark set, and the candidate landmark set from the set of visible landmarks at the observation time; During the probe navigation process, achieve rapid landmark identification and high-precision pose estimation through step-by-step matching.

[0033] Step 2.1: Analyze the observability of the navigation system based on the Fisher information matrix, and construct the observability index function of navigation landmarks as the performance index for landmark selection optimization.

[0034] The observability of a navigation system is typically expressed using the Fisher information matrix, which is calculated from the second derivative of the log-likelihood function. Its basic form is as follows:

[0035]

[0036] in, For observation data In parameters The probability density function or likelihood function under the given conditions; Let be the first derivative of the log-likelihood function, which describes the effect of parameter variations on the likelihood function. This represents the expected value of the observed data, which is the weighted average of all possible observations.

[0037] Based on the optical camera observation model, the Fisher information matrix is ​​calculated using the probability density function of a multivariate Gaussian distribution:

[0038]

[0039] in, The detector state at observation time k. For the i-th observation, Let i be the predicted observation value of the i-th landmark. To observe the noise covariance matrix, This is the Mahalanobis distance.

[0040] The observation noise model is , For observation noise, the observation noise covariance matrix This indicates that only the impact of image processing errors on the observed values ​​has been considered, and its representation is as follows:

[0041]

[0042] Considering that n landmarks are observed at the k-th observation time, the joint probability density function is:

[0043]

[0044] Assuming that the noise covariance of different land landmarks is consistent, i.e. Taking the logarithm and derivative of the joint probability density function, and applying the chain rule and the differentiation rules for symmetric matrices, we obtain the gradient sequence of the log-likelihood function:

[0045]

[0046] set up ,when Timely satisfaction

[0047]

[0048] when When 0 is satisfied

[0049]

[0050] Then the Fisher information matrix at the k-th observation can be expressed as:

[0051]

[0052] The determinant of the Fisher information matrix and the detector state error covariance matrix The relationship between the determinants can be described as follows:

[0053]

[0054] in, These are the eigenvalues ​​of the Fisher matrix.

[0055] Therefore, the optimal performance index function for selecting a single observation land reference set is:

[0056]

[0057] in, Let t be the set of landmarks observed and selected at time t. This represents the observability evaluation function. Generally, a larger function value results in a smaller optimization function and stronger observability of the selected landmark. Various methods can be used to evaluate observability using the Fisher matrix, such as tr(F), det(F), and log det(F).

[0058] Step 2.2: Based on the observability index function of navigation landmarks, construct a method for selecting primary landmarks and select the primary landmark set at time t.

[0059] Based on the requirements of the navigation system, it is assumed that selection is required. State estimation solutions are performed for each land landmark. Let... Let be the set of visible landmarks within the nominal field of view of the detector at time t. A single-observation landmark selection optimization performance index function is constructed, and the main landmark selection optimization equation is established:

[0060]

[0061] in, Let t be the set of land references. The set has a constraint on the number of elements. The optimal landmark set is obtained by solving the above equation. As the primary landmark set at time t If it exists Then let Furthermore, no further classification is performed at time t.

[0062] Step 2.3: Based on the observability index function of navigation landmarks, construct the auxiliary landmark selection method and auxiliary landmark matching strategy using the remainder set after the selection of the primary landmarks, and select the auxiliary landmark set at time t.

[0063] The primary landmark set at time t has been selected. Based on this, auxiliary land markers were selected. For In the set The optimal landmark is selected by using a single-observation landmark configuration optimization performance index function. As a landmark Alternatives in case of failure:

[0064]

[0065] in, The distance between the primary landmark and the secondary landmark. The maximum distance limit for a land landmark is set as the land landmark. With sets The minimum distance to the remaining landmasses, that is, satisfying The optimal land reference obtained from this calculation is: Selected auxiliary land markers .

[0066] After the initial selection is completed, subsequent selections need to be made in the set. Remove the selected auxiliary landmarks from the list, i.e., select landmarks that satisfy... By iterating through all the landmarks in the principal landmark set at time t, the set is obtained through calculation (x):

[0067]

[0068] As a set of auxiliary landmarks at time t If no other landmarks exist within the constraints, then this primary landmark is skipped, and it is determined that the primary landmark has no corresponding secondary landmarks or candidate landmarks. Clearly, the set of secondary landmarks satisfies... Furthermore, the order of the auxiliary landmarks and the primary landmarks in the set corresponds one-to-one, which is beneficial for quickly searching and supplementing landmarks when there are insufficient landmarks during navigation.

[0069] Step 2.4: Based on the observability index function of navigation landmarks, construct a candidate landmark selection method and a candidate landmark matching strategy using the remainder set after two selections, and select the candidate landmark set at time t.

[0070] When it exists ,in and set If the remaining landmarks cannot be correctly identified or matched in optical navigation, then the alternative landmark set is used. The set of landmarks that have been matched in the data Supplementing the existing set of land reference points to meet navigation system requirements. The construction method is as follows: based on the landmarks in the primary landmark set, in the set... The search is performed while satisfying the distance constraint. And construct a subset of candidate landmarks, where the elements of the subset represent the search order of the landmarks. Assume that each primary landmark can have a maximum of [number missing]. Given several alternative landmasses, define a fractional function:

[0071]

[0072] Let the sorting operator be argsort. Use a fractional function to score and sort the landmarks within the set, taking the top-ranked ones. One landmass as the main landmass The candidate land landmarks are obtained, and the subset of candidate land landmarks at time t is obtained. The calculation expression is:

[0073]

[0074] All primary landmarks corresponding to the alternative landmark subsets By merging, we obtain the candidate land reference set at time t. for:

[0075]

[0076] This completes the graded matching pose estimation of the optical navigation landmarks for deep space probes.

[0077] Beneficial effects:

[0078] 1. The present invention discloses a hierarchical matching pose estimation method for optical navigation landmarks of deep space probes. Based on the Fisher information matrix, the method designs landmarks to construct an observability evaluation index function, considers landmark visibility constraints, constructs a landmark optimization strategy, realizes landmark optimization for navigation, and ensures that landmarks have visibility and high observability.

[0079] 2. The deep space probe optical navigation landmark hierarchical matching pose estimation method disclosed in this invention utilizes the selection of primary landmarks, secondary landmarks and candidate landmark sets from the visible landmark library in each frame, and performs sequential matching during online matching to suppress the decrease in navigation information caused by landmark matching loss, thereby achieving high-precision navigation state estimation. Attached Figure Description

[0080] Figure 1 This is a flowchart of the deep space probe optical navigation landmark hierarchical matching pose estimation method of the present invention;

[0081] Figure 2 This is a diagram showing the selection results of the primary landmark, secondary landmark, and candidate landmark set at the observation time in step 2 of this embodiment of the invention;

[0082] Figure 3 This is a comparison diagram of the detector position estimation error achieved by the method in step 2 of this embodiment and the position estimation error achieved by the comparison method.

[0083] Figure 4 This is a comparison diagram of the detector velocity estimation error achieved by the method in step 2 of this embodiment and the velocity estimation error achieved by the comparison method.

[0084] Figure 5 This is a comparison diagram of the detector attitude estimation error achieved by the method in step 2 of this embodiment and the attitude estimation error achieved by the comparison method. Detailed Implementation

[0085] To better illustrate the purpose and advantages of the present invention, the invention will be further described below in conjunction with the accompanying drawings and examples.

[0086] To verify the feasibility of this invention, this embodiment selects the optical navigation scenario of a Mars landing mission. The initial position of the probe in the fixed coordinate system of the extraterrestrial body is set to [1000; 1000; 5000] m, the initial velocity to be [-2; -3; -65] m / s, the initial attitude to be [2; -1; 180] deg, and the target landing point to be [-2000; -2000; 300] m. The field of view of the probe camera in the x and y directions is 30 deg, the focal length is 0.717 m, and the pixel count is 1024*1024. The simulation duration is 100 s, the simulation step size is 0.1 s, and the probe observation interval is 10 s. 3000 landmarks are randomly generated in a uniform distribution within the landing area. The landmark classification method for extraterrestrial body landing optical navigation designed in this invention is used for navigation landmark classification and optimization, and landing navigation mathematical simulation verification is performed.

[0087] The method for classifying landmarks for optical navigation in extraterrestrial landings disclosed in this embodiment is as follows: Figure 1 As shown, the specific implementation steps are as follows:

[0088] Step 1: Establish the landing posture dynamics model and optical observation model of the probe to obtain the initial position, attitude and camera parameters of the probe, which are used to predict the landing state and observation range of the probe.

[0089] First, a dynamic model of the probe's landing pose is established to predict its landing state. The fixed coordinate system for the extraterrestrial body is defined as L, and the probe's camera coordinate system as c. The probe system state is as follows:

[0090]

[0091] in, and These represent the probe's position and velocity in a fixed coordinate system relative to the extraterrestrial body. The attitude quaternion is the coordinate system of the extraterrestrial object fixed in the coordinate system of the detector camera.

[0092] The dynamic equations for the probe's landing are:

[0093]

[0094] in, This is system state noise. Let be the system noise covariance matrix.

[0095] In this embodiment, the initial covariance of the system state is set as follows:

[0096] [200 2 200 2 200 2 0.52 0.5 2 0.5 2 ;1 2 0.0009 2 0.0009 2 0.0009 2 ].

[0097] The rotational velocity of the target celestial body is represented in the fixed coordinate system of the extraterrestrial celestial body. In this embodiment, the rotational velocity of Mars is 0.0041 deg / s. The attitude quaternion is the coordinate system of the extraterrestrial object fixed in the coordinate system of the detector camera. This represents the angular velocity of the detector relative to the fixed coordinate system of the extraterrestrial object in the detector's camera coordinate system. In this embodiment, it is set to [0;0;0.573] deg / s. This represents the gravitational acceleration received by the probe in a fixed coordinate system of an extraterrestrial body. In this embodiment, the gravitational acceleration on the surface of Mars is approximately 3.7 m / s². The thrust acceleration experienced by the detector in the fixed coordinate system of the extraterrestrial object is given by the polynomial guidance method in this embodiment; This represents a 4th-order antisymmetric matrix.

[0098] Let the partial derivatives of the system's state matrix be:

[0099]

[0100] The landing dynamics equations of the probe are used to predict the landing process state. In this embodiment, a polynomial guidance method is used to design the thrust acceleration experienced by the probe, as detailed below.

[0101] The polynomial guidance algorithm assumes that the detector's acceleration in all three directions is a quadratic function of time. The acceleration components in the three directions are represented as follows:

[0102]

[0103] in, , and These are the polynomial guidance coefficients in each direction. For time.

[0104] The expressions for the velocity and displacement components in each direction with respect to time, obtained through numerical integration, are as follows:

[0105]

[0106] in, and These represent the initial position and initial velocity in different directions within the fixed coordinate system of the extraterrestrial body.

[0107] Define the initial state as:

[0108]

[0109] Define the terminal state as:

[0110]

[0111] in, The time required for the landing process can be obtained by assuming that the vertical acceleration is linear.

[0112]

[0113] or:

[0114]

[0115] Combining the above equations, the polynomial guidance coefficient expression for each direction can be solved as follows:

[0116]

[0117] Therefore, the detector's control acceleration is:

[0118]

[0119] Subsequently, an optical observation model for the probe's landing was established. Using the predicted landing process state and camera parameters, the set of visible landmarks and their observations at the time of observation were obtained. Typically, the probe's attitude and position contain errors. Let the nominal pose of the probe's camera be represented as... The position of the landmark in the detector camera coordinate system is defined as follows:

[0120]

[0121] in, This indicates the position of the landmass in a fixed coordinate system relative to extraterrestrial objects. This indicates the position of the land marker in the detector's camera coordinate system. and These represent the position and attitude errors of the detector, respectively.

[0122] The observation equation for n landmarks is:

[0123]

[0124] in, For measuring noise.

[0125] To determine whether a landmark falls within the camera's field of view, based on the camera observation model, the following definition is used:

[0126]

[0127] in , Let and represent the angles in the x and y directions, respectively, between the line of sight of landmass j at observation time k and the camera's optical axis. Considering the uncertainty of the camera pose, field-of-view perturbation angle compensation needs to be added. , This embodiment sets Let the half-angles of the camera's field of view in the x and y directions be respectively... , Then the visibility criterion for land reference j at the k-th observation time is:

[0128]

[0129] Therefore, the set of visible landmarks at the k-th observation time is obtained as follows:

[0130]

[0131] Step 2: Establish a hierarchical selection and matching strategy for landmarks. Based on the landing trajectory of the probe and the set of landmarks visible at the observation time, select primary landmarks, secondary landmarks and candidate landmarks from the landmark set. During the landing navigation process of the probe, the landmarks are matched step by step to achieve rapid identification and high-precision navigation.

[0132] First, based on the Fisher information matrix analysis of the navigation system's observability, a navigation landmark observability index function is constructed as an optimization performance index for landmark selection. The Fisher information matrix is ​​calculated using a multivariate Gaussian distribution probability density function based on the optical camera observation model.

[0133]

[0134] in, The detector state at observation time k. For the i-th observation, Let i be the predicted observation value of the i-th landmark. To observe the noise covariance matrix, This is the Mahalanobis distance.

[0135] The observation noise model is , For observation noise, the observation noise covariance matrix This indicates that only the impact of image processing errors on the observed values ​​was considered.

[0136] Considering that n landmarks are observed at the k-th observation time, the joint probability density function is:

[0137]

[0138] Assuming that the observed noise covariance of different landmarks is consistent, i.e. Taking the logarithm and derivative of the joint probability density function yields the gradient sequence of the log-likelihood function:

[0139]

[0140] set up ,when Timely satisfaction

[0141]

[0142] when When 0 is satisfied

[0143]

[0144] Then the Fisher information matrix at the k-th observation time can be expressed as:

[0145]

[0146] In this embodiment, the optimized performance index function for selecting the single-observation landmark configuration is:

[0147]

[0148] in, Let be the set of landmarks observed and selected at time t. The logarithm of the determinant of a matrix.

[0149] Secondly, based on the observability index function of navigation landmarks, a primary landmark selection method is constructed to select the primary landmark set at time t. It is assumed that, according to the requirements of the navigation system, it is necessary to select... State estimation solutions are performed for each land landmark. Let... Let be the set of visible landmarks within the nominal field of view of the detector at time t. A performance index function for selecting landmark configurations in a single observation is constructed. Combined with the visibility probability constraint of the landmark configurations, an optimization equation for selecting the primary landmark is established:

[0150]

[0151] in, Let t be the set of land references. To constrain the number of elements in the set, this embodiment sets it to 4. The optimal set of landmarks is obtained by solving the above equation. As the primary landmark set at time t If it exists Then let Furthermore, no further classification is performed at time t.

[0152] Subsequently, based on the observability index function of navigation landmarks, and utilizing the remainder set after the selection of primary landmarks, a method for selecting secondary landmarks and a matching strategy for secondary landmarks are constructed to select the secondary landmark set at time t. The primary landmark set at time t has already been selected. Based on this, auxiliary land markers were selected. For In the set The optimal landmark is selected by using a single-observation landmark configuration optimization performance index function. As a landmark Alternatives in case of failure:

[0153]

[0154] in, The distance between the primary landmark and the secondary landmark. The maximum distance limit for a land landmark is set as the land landmark. With sets The minimum distance to the remaining landmasses, that is, satisfying The optimal land reference obtained from this calculation is: Selected auxiliary land markers .

[0155] After the initial selection is completed, subsequent selections need to be made in the set. Remove the selected auxiliary landmarks from the list, i.e., select landmarks that satisfy... By iterating through all the landmarks in the principal landmark set at time t, the set is obtained through calculation (x):

[0156]

[0157] As a set of auxiliary landmarks at time t If no other landmark exists within the constraints, this primary landmark is skipped, and it is assumed that the primary landmark has no corresponding secondary landmark or candidate landmark. Clearly, the set of secondary landmarks satisfies... Furthermore, the order of the auxiliary landmarks and the primary landmarks in the set corresponds one-to-one, which is beneficial for quickly searching and supplementing landmarks when there is a shortage of landmarks during landing navigation.

[0158] Finally, based on the observability index function of navigation landmarks and the visibility probability of landmark configurations, a candidate landmark selection method and a candidate landmark matching strategy are constructed using the remainder set after two selections, and a candidate landmark set is selected at time t. When there exists... ,in and set If the remaining landmarks cannot be correctly identified or matched in optical navigation, then the alternative landmark set is used. The set of landmarks that have been matched in the data Supplementing the existing set of land reference points to meet navigation system requirements. The construction method is as follows: based on the landmarks in the primary landmark set, in the set... The search is performed while satisfying the distance constraint. And construct a subset of candidate landmarks, where the elements of the subset represent the search order of the landmarks. Assume that each primary landmark can have a maximum of [number missing]. Given several alternative landmasses, define a fractional function:

[0159]

[0160] Let the sorting operator be argsort. Use a fractional function to score and sort the landmarks within the set, taking the top-ranked ones. One landmass as the main landmass The alternative land markers are set in this embodiment. Obtain the candidate landmark subset at time t. The calculation expression is:

[0161]

[0162] All primary landmarks corresponding to the alternative landmark subsets By merging, we obtain the candidate land reference set at time t. for:

[0163]

[0164] Based on the predicted landing state of the probe obtained in Step 1, and using the landmark analysis and optimization method designed in this step, a greedy algorithm is employed to calculate the optimization problem, resulting in a multi-level landmark sequence for the probe's descent. The selection results of the primary landmark, secondary landmark, and candidate landmark sets at each observation time are as follows: Figure 3 As shown.

[0165] Step 3: Construct a probe landing navigation state estimation method based on the unscented Kalman filter method, use the selected navigation landmark set to perform state estimation solution, obtain the change of probe state estimation error, and conduct comparative simulation to verify the effectiveness of the designed method.

[0166] This invention employs the unscented Kalman filter method for navigation state estimation, and the specific process is as follows:

[0167] First, construct the unscented transformation. Perform an unscented transformation on the state vector to construct the state at time k-1. Sigma points Simultaneously, the corresponding weights of the unscented transformation are constructed to obtain the statistical characteristics of the system.

[0168] The Sigma point sampling method is as follows:

[0169]

[0170] in, , where In this embodiment, n is set to 13. The state error covariance matrix is ​​constructed using the state covariance at the initial time.

[0171] The corresponding mean weights and variance weights are:

[0172]

[0173] Therefore, the state variables and observations in time can be obtained from the following formula. Internal updates and measurement updates:

[0174]

[0175] The measurement corrections are as follows:

[0176]

[0177] in, The system process noise matrix is ​​set as follows in this embodiment:

[0178] Q r= diag([1 -4 ; 1 -4 ; 1 -4 ]);

[0179] Q v = diag([1 -6 ; 1 -6 ; 1 -6 ]);

[0180] Q q = diag([2.5×10 -3 2.5×10 -3 2.5×10 -3 ]);

[0181] Q = diag([Q r Q v Q q ]);

[0182] In this embodiment, the camera observation noise is set to 1 pixel. Assuming that n landmarks are observed at the k-th observation time, the observation noise matrix is... Set to:

[0183] ∑ k j = diag([1; 1]);

[0184] ∑ k = diag([∑ k 1 ;…;∑ k n ]);

[0185] During observation times, the navigation system uses the Kalman filter method for state recursion; during non-observation times, a linearized state transition matrix is ​​used for state recursion.

[0186] The system's state transition matrix is:

[0187]

[0188] in, This indicates the simulation time step.

[0189] The linearized observation matrix H of the navigation filter is:

[0190]

[0191] The following landmark usage rate indicators are set to reflect the effectiveness of this method in achieving step-by-step matching:

[0192] Main land marker full utilization rate for:

[0193]

[0194] Where K is the total number of observations and M is the number of simulations. This represents the main land reference number used at the k-th observation time in the m-th simulation. This represents the total number of landmasses used at the k-th observation time in the m-th simulation. This is an indicator function that indicates whether the condition is true or false, satisfying:

[0195]

[0196] Auxiliary Landmark Usage Rate for:

[0197]

[0198] in, The auxiliary standard number used at the k-th observation time in the m-th simulation is represented by the symbol. This indicates that conditions a and b are satisfied simultaneously.

[0199] Alternative land standard usage rate for:

[0200]

[0201] in, This represents the number of alternative landmasses used at the k-th observation time in the m-th simulation.

[0202] Average usage rate of main land markers for:

[0203]

[0204] Average usage rate of auxiliary land markers for:

[0205]

[0206] Average percentage of alternative land-based standards used for:

[0207]

[0208] Landmark selection results are used to simulate probe landing pose estimation. A control experiment is set up using only the main landmark set for pose estimation, comparing it with the Monte Carlo pose estimation simulation of the method proposed in this invention. Both sets of experiments use the same landing trajectory and noise data. In this embodiment, the number of Monte Carlo simulations is set to [number missing]. The simulation results for position, velocity, and attitude estimation are as follows: Figure 3 , Figure 4 and Figure 5 As shown. Meanwhile, the simulation results for the land landmark utilization rate are as follows:

[0209]

[0210] Based on the pose estimation results, the proposed method effectively improves the estimation accuracy. As can be seen from the landmark usage index, when only the main landmark set is used for pose estimation, there is a certain probability that the main landmark will be lost. Through the landmark hierarchical and step-by-step matching strategy proposed in this invention, the landmark can be effectively supplemented in the case of landmark loss, thus ensuring the reliability of navigation.

[0211] The above detailed description further illustrates the purpose, technical solution, and beneficial effects of the invention. It should be understood that the above description is only a specific embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for graded matching pose estimation of landmarks for optical navigation of deep space probes, characterized in that: Includes the following steps: Step 1: Establish the detector pose dynamics model and optical observation model to obtain the detector's initial position, attitude, and camera parameters, which are used to predict the detector's state and the visible landmark set at the observation time. Step 2: Construct the observability index function for navigation landmarks; Based on the probe trajectory and the set of visible landmarks at the observation time, select the main landmark set, the auxiliary landmark set, and the candidate landmark set from the set of visible landmarks at the observation time; During the probe navigation process, achieve rapid landmark identification and high-precision pose estimation through step-by-step matching.

2. The method as described in claim 1, characterized in that: Step 2 describes the construction of a navigation landmark observability index function, which serves as an optimization performance index for landmark selection. in, Let be the set of landmarks observed and selected at time t; This represents the observable evaluation function.

3. The method as described in claim 1, characterized in that: The method for selecting the principal set of landmarks from the visible landmark set at the observation time is as follows: Based on the requirements of the navigation system, it is assumed that selection is required. State estimation solutions are performed for each land landmark; let... Let be the set of visible landmarks within the nominal field of view of the detector at time t. Based on the navigation landmark observability index function, an optimization equation for selecting the main landmarks is constructed: The optimal set of landmarks obtained by solving equation (2) As the primary landmark set at time t If it exists Then let Furthermore, no further classification is performed at time t.

4. The method as described in claim 1, characterized in that: The method for selecting the auxiliary landmark set from the visible landmark set at the observation time is as follows: Using the observability index function of navigation landmarks, the remainder after the selection of the primary landmarks... Select an optimal land reference from the list. As a landmark Alternatives in case of failure: in, The distance between the primary landmark and the secondary landmark. The maximum distance limit for a land landmark is set as the land landmark. With sets The minimum distance to the remaining landmasses, that is, satisfying ; The optimal landmark obtained according to equation (3) For the selected auxiliary land marker After the initial selection is completed, subsequent selections need to be made within the set. Remove the selected auxiliary landmarks from the list, i.e., select landmarks that satisfy... ; By iteratively calculating all the landmarks in the main landmark set at time t, we obtain the set: As a set of auxiliary landmarks at time t If there are no other landmarks within the range that meet the constraints, the main landmark is skipped, and it is determined that the main landmark has no corresponding auxiliary landmark or candidate landmark. The auxiliary landmarks and the main landmarks are in a one-to-one order in the set, which is beneficial for quickly searching and supplementing landmarks when there are insufficient landmarks during navigation.

5. The method as described in claim 1, characterized in that: The method for selecting candidate landmark sets from the visible landmark set at the observation time is as follows: When it exists ,in and set If the remaining landmarks cannot be correctly identified or matched in optical navigation, then the alternative landmark set is used. The set of landmarks that have been matched in the data Supplementing the system to meet navigation system requirements; Fractional functions: The sorting operator is argsort, which uses a fractional function to sort the remainder after two selections. The land markers within the area are scored and ranked, and the top ones are selected. One landmass as the main landmass The candidate land landmarks are obtained, and the subset of candidate land landmarks at time t is obtained. : All primary landmarks corresponding to the alternative landmark subsets By merging, we obtain the candidate land reference set at time t. for: 。 6. The method as described in claim 1, characterized in that: The detector pose dynamics model described in step 1 is expressed as follows: in, Represents the detector state variables. This is system state noise. Let be the system noise covariance matrix. This represents the thrust acceleration experienced by the detector.

7. The method as described in claim 1, characterized in that: The optical observation model described in step 1 is expressed as follows: in, The position of the land landmark in the fixed coordinate system of extraterrestrial bodies. , and Let represent the three-axis components of the position of land marker i in the detector camera coordinate system at time k. Indicates the camera's focal length. For measuring noise.

8. The method as described in claim 1, characterized in that: The visible land reference set at the observation time mentioned in step 1 is represented as follows: in, , Let represent the angles in the x and y directions between the line of sight of land target j at time k and the camera's optical axis. , These represent the absolute values ​​of the half-angles of the camera's field of view in the horizontal and vertical directions, respectively. , This represents the compensation value for the field of view disturbance angle in the x and y directions.

Images