Robust planning optimization method for extraterrestrial body landing navigation landmarks
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- BEIJING INST OF TECH
- Filing Date
- 2026-03-06
- Publication Date
- 2026-06-09
Smart Images
Figure CN122174472A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to a robust planning and optimization method for landmarks in extraterrestrial landing navigation, which is particularly suitable for autonomous optical navigation of probes and belongs to the field of deep space exploration. Background Technology
[0002] Landing exploration is a crucial method for extraterrestrial object exploration. Extraterrestrial object exploration missions involve long journeys, and ground-based telemetry and navigation suffer from significant communication delays and reduced navigation accuracy, making it difficult to meet the requirements for real-time, high-precision navigation. Therefore, the probe must possess autonomous navigation capabilities. The surfaces of extraterrestrial objects are rich in information, providing navigational information for the probe's landing. The probe uses onboard optical sensors to acquire landmark information from the target object's surface and performs autonomous navigation through calculations by an onboard computer. Therefore, landmark-based navigation methods are a key research focus for optical navigation during the landing phase.
[0003] During landing navigation, probes typically acquire a significant amount of landmark information. However, due to limitations in the onboard computer's computing power, it's difficult to use all observed landmarks for state estimation. Therefore, it's necessary to predict landing observation information based on the given landing point and trajectory, select a subset of the landmark database, and use landmarks with excellent visibility and information content for state estimation to improve the accuracy of landing navigation.
[0004] Among the existing methods for selecting landmarks in optical navigation for extraterrestrial landing, the first technology [1] (Beijing Institute of Technology. Feature planning and selection method for autonomous operation optical navigation on the surface of extraterrestrial bodies: CN202210996854.4[P]. 2022-11-15.) constructed the relationship between pose uncertainty and field vertex measurement, evaluated the range of field vertex uncertainty by sampling, and constructed the field intersection region for robust landmark selection; the second technology [2] (Beijing Institute of Technology. Robust selection method for landmarks in autonomous optical navigation for planetary landing: CN202310026242.7[P]. 2023-05-09.) established the relationship between the field vertex error ellipse and the spacecraft pose error using the error propagation theory, constructed the field intersection set through the relationship between the field vertex and the pose error, constrained the landmark selection range, and used the Fisher information matrix as the optimization index for landmark selection, realizing offline landmark selection at multiple observation times. However, the above methods are all qualitative landmark selection strategies, which do not quantify the visibility probability of landmarks and make it difficult to systematically evaluate the impact of the visibility of selected landmarks on navigation accuracy.
[0005] Existing methods for selecting the best landmarks for optical navigation in extraterrestrial landings have not achieved rapid calculation of the visibility probability of landmarks and multiple landmark configurations in the landing area, nor have they comprehensively considered the impact of the visibility of the selected landmark configuration on navigation accuracy, thus failing to meet the reliability and high accuracy requirements of landing navigation. Summary of the Invention
[0006] The purpose of this invention is to provide a robust planning and optimization method for landmarks in extraterrestrial landing navigation. This method considers the uncertainty of the detector's pose, utilizes the geometric relationship between the camera's field of view model and the detector's pose to construct a camera field of view perturbation model, obtains the probability density function of the camera's field of view perturbation angle, and realizes the calculation of the visibility probability of the landmark configuration. Based on the visibility probability of the landmark configuration, a landmark optimization index function is constructed, and the landmark is robustly planned and optimized by solving the optimization problem, thereby improving the accuracy of landing navigation state estimation.
[0007] The present invention is achieved through the following technical solution.
[0008] The robust planning and optimization method for landmarks in extraterrestrial body landing navigation disclosed in this invention includes the following steps:
[0009] 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 status and observation range of the probe.
[0010] Step 1.1: Establish a dynamic model of the probe's landing posture to predict the probe's landing 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] 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.
[0014] The orbital dynamics equations for the probe's landing are:
[0015]
[0016] in, For acceleration noise, Here is the acceleration noise covariance matrix. This represents the rotational velocity of the target celestial body in a fixed coordinate system. This represents the gravitational acceleration received by the probe in a fixed coordinate system relative to an extraterrestrial object. This represents the thrust acceleration experienced by the probe in a fixed coordinate system relative to an extraterrestrial object.
[0017] The attitude dynamics equations of the detector are:
[0018]
[0019] in, For attitude noise, Here is the attitude noise covariance matrix. 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 a 4th-order antisymmetric matrix.
[0020] The landing dynamics system state satisfies:
[0021]
[0022] in, Let be the known control quantity at time k. This represents state noise. Let the partial derivatives of the system state be:
[0023]
[0024] After obtaining the initial position and attitude information of the probe, the landing process state of the probe can be predicted by using the dynamic equations of the probe landing.
[0025] Step 1.2: Establish an optical observation model for the probe landing. Using the predicted landing process state and camera parameters, obtain the set of visible landmarks and their observations at the observation time.
[0026] 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:
[0027]
[0028] 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.
[0029] The observation equation for land reference i is defined as follows:
[0030]
[0031] in, For measuring noise.
[0032] Let the partial derivatives of the observation equation be:
[0033]
[0034] To determine whether a landmark falls within the camera's nominal field of view, based on the camera observation model, the following definition is used:
[0035]
[0036] in , Let and represent the angles in the x and y directions, respectively, between the line of sight of landmark j at time k and the camera's optical axis. Let the half-angles of the camera's horizontal and vertical field of view be and respectively. , Then the visibility criterion for landmark j at time k is:
[0037]
[0038] Therefore, the set of visible landmarks at the nominal observation time is:
[0039]
[0040] Step 2: Considering the uncertainty of the detector's pose, construct the covariance matrix of the camera field of view error angle caused by the uncertainty of attitude and position, respectively. Based on the two covariance matrices, construct the camera field of view perturbation model. Based on the detector state and observation range, further calculate the visibility probability of the landmark configuration.
[0041] Step 2.1: Establish a camera field of view perturbation model that considers the uncertainty of the detector attitude. Using the attitude covariance matrix of the detector, calculate the covariance matrix of the camera field of view error angle caused by the attitude uncertainty.
[0042] Let the nominal pose of the camera in the k-th frame be represented as... Assume the basis vectors of the camera coordinate system are , , Then the coordinates of the detector camera coordinate system in the world coordinate system are represented as follows: , , Assume a small-angle perturbation of the detector's attitude angle in the detector-camera coordinate system. satisfy When considering attitude errors, the optical axis perturbation is the same as the field-of-view perturbation; therefore, the field-of-view error can be described by constructing an optical axis perturbation. Under the small-angle approximation assumption, the perturbation angle of the field of view in the x and y directions is equal to the projection of the optical axis perturbation onto the camera coordinate system base coordinates:
[0043]
[0044] Based on the small-angle perturbation model on the Lie group SO(3), Let the cross product matrix be the vector, and the actual pose of the camera be represented as:
[0045]
[0046] Therefore, the true state of the camera's optical axis is represented as:
[0047]
[0048] According to the properties of cross product matrices, we have According to the properties of rotation matrices, we have Therefore, the optical axis error value It can be represented as:
[0049]
[0050] Knowing that for any vector Satisfies scalar triple product: Under the small angle assumption, we have Then the optical axis error value It can be represented as:
[0051]
[0052] The field-of-view error angle caused by attitude disturbance can be expressed as:
[0053]
[0054] Therefore, the expression for the camera field of view perturbation caused by attitude uncertainty is:
[0055]
[0056] The covariance matrix of the camera field-of-view error angle caused by attitude uncertainty is:
[0057]
[0058] Step 2.2: Establish a camera field of view perturbation model that considers the uncertainty of the detector position, and use the position covariance matrix of the detector to calculate the covariance matrix of the camera field of view error angle caused by the uncertainty of position.
[0059] Similarly, let the nominal pose of the camera in the k-th frame be represented as... Small perturbation of the probe's position in a fixed coordinate system for extraterrestrial objects satisfy .
[0060] The nominal line of sight is The line-of-sight unit vector is represented as The line of sight caused by camera position error is represented as: Assuming the landmark's position remains constant, the change in the line of sight is caused by the change in the camera's position. Therefore, the change in the line of sight vector is inversely proportional to the change in the camera's position. The projection of the positional uncertainty onto the camera coordinate system base coordinates is:
[0061]
[0062] Taking the first-order differential of the line-of-sight unit vector, we have:
[0063]
[0064] Retaining the first-order approximation, the error value of the line-of-sight unit vector is now... It can be represented as:
[0065]
[0066] Therefore, the projection of the positional uncertainty can be written as:
[0067]
[0068] The covariance matrix of the camera field-of-view error angle caused by positional uncertainty is:
[0069]
[0070] Step 2.3: Using the covariance matrix of the camera field of view error angle, establish a camera field of view perturbation model that considers the uncertainty of the detector pose, and calculate the visibility probability of the landmark configuration.
[0071] The camera field of view perturbation can be written as:
[0072]
[0073] Therefore, the covariance matrix of the camera field of view perturbation satisfies:
[0074]
[0075] Since the pose error follows a Gaussian distribution, the probability density function corresponding to the camera field of view perturbation is:
[0076]
[0077] Based on the probability density function established above, the land reference configuration is defined. Visible probability The calculation method is as follows:
[0078]
[0079] in, The integral representing the set of landmarks formed by the landmark configuration has the following upper and lower bounds:
[0080]
[0081] Other parameters are:
[0082]
[0083] In the above formula, and This represents the half-angle of the camera's field of view in the x and y directions. and This represents the angle between the line-of-sight envelope of the landmark configuration in the camera coordinate system and the coordinate axis.
[0084] Step 3: Construct the landmark optimization index function, and use the visibility probability value of the landmark configuration, combined with state covariance prediction, to achieve robust planning optimization of the landmark.
[0085] Suppose that the navigation system uses N landmarks for state estimation at time k, and the landmark configuration at time k is as follows: ,satisfy .set up Represents a set The i-th landmass was observed. Let the i-th landmark be a subset of the landmark configuration, indicating that it has not been observed. It is an invisible landmark set.
[0086] Record of events express exist The remainder of the middle All visible, the expression for this event is:
[0087]
[0088] event The probability of occurrence is expressed as:
[0089]
[0090] Record of events express exist The remainder of the middle All visible and Invisible, the expression for this event is:
[0091]
[0092] event The probability of occurrence is expressed as:
[0093]
[0094] in, The set U represents the number of sets; in equation (79), set U represents the set of invisible landmarks. A subset of, i.e. .
[0095] The optimization function for the landmark configuration is constructed as follows:
[0096]
[0097] in, The evaluation function for the state covariance matrix is set to [function name] in this embodiment. ; Expressing expectations; Indicates the use of land landmark configuration The detector state covariance matrix obtained from observation updates.
[0098] The method for calculating the detector state covariance matrix is as follows:
[0099] When there are no observation updates, i.e. or Assume the detector state at time k-1 Covariance matrix of the state Given that the state covariance matrix is recursively derived using the state partial derivative matrix, we obtain:
[0100]
[0101] in, ,in In this embodiment, the recursive time step is set to 10s.
[0102] If there is an observation update, i.e. Then the state covariance matrix is updated as follows:
[0103]
[0104] Among them, according to equations (52) and (53), the landmark configuration can be obtained. The predicted observation values of each landmark in the middle are used to obtain the observation matrix at time k. .
[0105] Therefore, utilizing the land landmark configuration And consider the invisible landmark set The detector state covariance matrix obtained by observation update is:
[0106]
[0107] Beneficial effects:
[0108] 1. The robust planning and optimization method for landmarks for extraterrestrial landing navigation disclosed in this invention considers the uncertainty of the detector's pose, utilizes the geometric relationship between the camera's field of view model and the detector's pose to construct a camera field of view perturbation model, obtains the probability density function of the camera's field of view perturbation angle, and realizes the quantitative calculation of the visibility probability value of the landmark configuration.
[0109] 2. The robust planning and optimization method for landmarks in extraterrestrial landing navigation disclosed in this invention constructs a landmark configuration optimization function by considering the failure of different landmark combinations in the landmark configuration and combining the prediction of the landmark configuration visibility probability and the state covariance matrix. By solving the optimization problem, the landmark planning optimization under the uncertainty of the detector pose is realized, which improves the accuracy of navigation state estimation. Attached Figure Description
[0110] Figure 1 This is a flowchart of the robust planning and optimization method for extraterrestrial landing navigation landmarks according to the present invention;
[0111] Figure 2 This is a schematic diagram showing the relationship between the line-of-sight envelope and the coordinate axis angle of the landmark configuration in step 2 of this embodiment of the invention in the camera coordinate system;
[0112] Figure 3 This is a comparison chart of the visibility probability of the landmark configuration calculated by the camera perturbation model in step 2 of this embodiment and the Monte Carlo simulation verification results;
[0113] Figure 4To set the initial position covariance of the simulation to
[100] 2 ;100 2 ;100 2 ]m, the initial velocity covariance is [0.1 2 0.1 2 0.1 2 [m / s, initial attitude covariance is [0.1] m / s, 2 0.1 2 0.1 2 [deg, the robust planning optimization results of the landmarks in the visible area at the next shooting time;]
[0114] Figure 5 To set the initial position covariance of the simulation to
[300] 2 300 2 300 2 ]m, the initial velocity covariance is [0.1 2 0.1 2 0.1 2 [m / s, initial attitude covariance is [0.1] m / s, 2 0.1 2 0.1 2 [deg, the robust planning optimization results of the landmarks in the visible area at the next shooting time;]
[0115] Figure 6 To set the initial position covariance of the simulation to
[100] 2 ;100 2 ;100 2 ]m, the initial velocity covariance is [0.1 2 0.1 2 0.1 2 ]m / s, initial attitude covariance is [1 2 ;1 2 ;1 2 [deg], the optimal result of robust planning for landmarks in the area visible at the next shooting time. Figure 7 This is a comparison chart of the land landmark planning and selection results achieved by the method proposed in this invention and the land landmark optimization method based on Fisher information matrix. Detailed Implementation
[0116] 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.
[0117] 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.
[0118] 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:
[0119] 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.
[0120] 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:
[0121]
[0122] in, and These represent the probe's position and velocity in a fixed coordinate system relative to the extraterrestrial body. The attitude quaternion from the fixed coordinate system of the extraterrestrial object to the coordinate system of the detector camera.
[0123] The orbital dynamics equations for the probe's landing are:
[0124]
[0125] in, For acceleration noise, Here is the acceleration noise covariance matrix. This represents the rotational velocity of the target celestial body in a fixed coordinate system. This represents the gravitational acceleration received by the probe in a fixed coordinate system relative to an extraterrestrial object. This represents the thrust acceleration experienced by the probe in a fixed coordinate system relative to an extraterrestrial object.
[0126] The attitude dynamics equations of the detector are:
[0127]
[0128] in, For attitude noise, Here is the attitude noise covariance matrix. 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 a 4th-order antisymmetric matrix.
[0129] The landing dynamics system state satisfies:
[0130]
[0131] in, Let be the known control quantity at time k. Representing state noise; let the partial derivatives of the system state be:
[0132]
[0133] After obtaining the initial position and attitude information of the probe, the landing process state of the probe can be predicted by using the dynamic equations of the probe landing.
[0134] 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.
[0135] 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:
[0136]
[0137] in, , and These are the polynomial guidance coefficients in each direction. For time.
[0138] The expressions for the velocity and displacement components in each direction with respect to time, obtained through numerical integration, are as follows:
[0139]
[0140] Define the initial state as:
[0141]
[0142] Define the terminal state as:
[0143]
[0144] in, The time required for the landing process can be obtained by assuming that the vertical acceleration is linear.
[0145]
[0146] Combining the above equations, the polynomial guidance coefficient expression for each direction can be solved as follows:
[0147]
[0148] Therefore, the detector's control acceleration is:
[0149]
[0150] 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:
[0151]
[0152] 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.
[0153] The observation equation for land reference i is defined as follows:
[0154]
[0155] in, For measuring noise.
[0156] Linearized observation matrix at time k for:
[0157]
[0158] To determine whether a landmark falls within the camera's field of view, based on the camera observation model, the following definition is used:
[0159]
[0160] in , Let and represent the angles between the line of sight of landmark j at time k and the camera's optical axis in the x and y directions, respectively. Let the half-angles of the camera's field of view in the x and y directions be respectively... , Then the visibility criterion for landmark j at time k is:
[0161]
[0162] Therefore, the set of visible landmarks at the nominal observation time is:
[0163]
[0164] Step 2: Considering the uncertainty of the detector's pose, construct the covariance matrix of the camera field of view error angle caused by the uncertainty of attitude and position respectively. Based on the two covariance matrices, construct the camera field of view perturbation model. Based on the detector's landing state and observation range, further calculate the visibility probability of the landmark configuration.
[0165] First, a camera field-of-view perturbation model considering detector attitude uncertainty is established. Using the detector's attitude covariance matrix, the covariance matrix of the camera field-of-view error angle caused by attitude uncertainty is calculated. Let the nominal camera pose in the k-th frame be represented as... Assume the basis vectors of the camera coordinate system are , , Then the coordinates of the detector camera coordinate system in the world coordinate system are represented as follows: , , Assume a small-angle perturbation of the detector's attitude angle in the detector-camera coordinate system. satisfy When considering attitude errors, the optical axis perturbation is the same as the field-of-view perturbation; therefore, the field-of-view error can be described by constructing an optical axis perturbation. Under the small-angle approximation assumption, the perturbation angle of the field of view in the x and y directions is equal to the projection of the optical axis perturbation onto the camera coordinate system base coordinates:
[0166]
[0167] Based on the small-angle perturbation model on the Lie group SO(3), Let the cross product matrix be the vector, and the actual pose of the camera be represented as:
[0168]
[0169] Therefore, the true state of the camera's optical axis is represented as:
[0170]
[0171] According to the properties of cross product matrices, we have According to the properties of rotation matrices, we have Therefore, the optical axis error value It can be represented as:
[0172]
[0173] Knowing that for any vector Satisfies scalar triple product: Under the small angle assumption, we have Then the optical axis error value It can be represented as:
[0174]
[0175] The field-of-view error angle caused by attitude disturbance can be expressed as:
[0176]
[0177] Therefore, the expression for the camera field of view perturbation caused by attitude uncertainty is:
[0178]
[0179] The covariance matrix of the camera field-of-view error angle caused by attitude uncertainty is:
[0180]
[0181] Subsequently, a camera field-of-view perturbation model considering detector position uncertainty is established. Using the detector's position covariance matrix, the covariance matrix of the camera field-of-view error angle caused by position uncertainty is calculated. Similarly, let the nominal camera pose of the k-th frame be represented as... Small perturbation of the probe's position in a fixed coordinate system for extraterrestrial objects satisfy .
[0182] The nominal line of sight is The line-of-sight unit vector is represented as The line of sight caused by camera position error is represented as: Assuming the landmark's position remains constant, the change in the line of sight is caused by the change in the camera's position. Therefore, the change in the line of sight vector is inversely proportional to the change in the camera's position. The projection of the positional uncertainty onto the camera coordinate system base coordinates is:
[0183]
[0184] Taking the first-order differential of the line-of-sight unit vector, we have:
[0185]
[0186] Retaining the first-order approximation, the error value of the line-of-sight unit vector is now... It can be represented as:
[0187]
[0188] Therefore, the projection of the positional uncertainty can be written as:
[0189]
[0190] The covariance matrix of the camera field-of-view error angle caused by positional uncertainty is:
[0191]
[0192] Using the covariance matrix of the camera's field-of-view error angle, a camera field-of-view perturbation model considering detector pose uncertainty is established. The camera field-of-view perturbation can be written as:
[0193]
[0194] Therefore, the covariance matrix of the camera field of view perturbation satisfies:
[0195]
[0196] Since the pose error follows a Gaussian distribution, the probability density function corresponding to the camera field of view perturbation is:
[0197]
[0198] Based on the probability density function established above, the landmark configuration observed at time k is defined. Visible probability The calculation method is as follows:
[0199]
[0200] The upper and lower bounds of the integral are:
[0201]
[0202] Other parameters are:
[0203]
[0204] In the above formula, and This represents the half-angle of the camera's field of view in the x and y directions. and The diagram shows the angle between the line-of-sight envelope of the landmark configuration in the camera coordinate system and the coordinate axes. Figure 2 As shown.
[0205] To verify the effectiveness of the designed camera field-of-view perturbation model, a Monte Carlo experiment was conducted. The camera position was set to [2800; 2800; 7800] m, and the camera attitude was set to [-135; -25; 180] degrees; the camera position covariance was [300...]. 2 300 2 300 2 ]m, the camera pose covariance is [0.5 2 0.5 2 0.5 2 The camera's field of view in the x and y directions is 20 degrees, and other camera parameters are the same as in the above embodiment. Four landmarks are set at positions of [800;500;0] m, [-1000;-800;0] m, [-600;900;0] m, and [200;1300;0] m, respectively. 20,000 Monte Carlo experiments are performed, and the simulation results are as follows: Figure 3 As shown, the visibility probability of this landmark configuration was obtained through Monte Carlo verification. The visibility probability of this landmark configuration calculated by the camera field-of-view perturbation model designed in this invention is: It can be seen that the calculation error meets the speed and accuracy requirements of land standard planning.
[0206] Step 3: Construct the landmark optimization index function, and use the visibility probability value of the landmark configuration, combined with state covariance prediction, to achieve robust planning optimization of the landmark.
[0207] Assume the navigation system uses N landmarks for state estimation at time k; in this embodiment, N=4. The landmark configuration at time k is as follows: ,satisfy .set up Represents a set The i-th landmass was observed. Let the i-th landmark be a subset of the landmark configuration, indicating that it has not been observed. It is an invisible landmark set.
[0208] Record of events express exist The remainder of the middle All visible, the expression for this event is:
[0209]
[0210] event The probability of occurrence is expressed as:
[0211]
[0212] Record of events express exist The remainder of the middle All visible and Invisible, the expression for this event is:
[0213]
[0214] event The probability of occurrence is expressed as:
[0215]
[0216] in, The set U represents the number of sets; in equation (79), set U represents the set of invisible landmarks. A subset of, i.e. .
[0217] The optimization function for the landmark configuration is constructed as follows:
[0218]
[0219] in, The evaluation function for the state covariance matrix is set to [function name] in this embodiment. ; Expressing expectations; Indicates the use of land landmark configuration The detector state covariance matrix obtained from observation updates.
[0220] The method for calculating the detector state covariance matrix is as follows:
[0221] When there are no observation updates, i.e. or Assume the detector state at time k-1 Covariance matrix of the state Given that the state covariance matrix is recursively derived using the state partial derivative matrix, we obtain:
[0222]
[0223] in, ,in In this embodiment, the recursive time step is set to 10s.
[0224] If there is an observation update, i.e. Then the state covariance matrix is updated as follows:
[0225]
[0226] Among them, according to equations (52) and (53), the landmark configuration can be obtained. The predicted observation values of each landmark in the middle are used to obtain the observation matrix at time k. .
[0227] Therefore, utilizing the land landmark configuration And consider the invisible landmark set The detector state covariance matrix obtained by observation update is:
[0228]
[0229] In this embodiment, to facilitate the analysis of the detector's state, the landmark configuration optimization function is decomposed into three parts: position, velocity, and attitude. Based on different optimization requirements, the above landmark configuration optimization function is optimized to achieve optimal planning of the observed landmarks. This embodiment mainly considers the impact of observations on position estimation; therefore, the following landmark configuration optimization function is used for robust landmark planning:
[0230]
[0231] To verify the effectiveness of the proposed robust planning and optimization method for land landmarks, this embodiment sets up a simulation verification of robust planning and optimization for land landmarks under a probe landing scenario. The system parameters set in this embodiment are as follows:
[0232] Q r = diag([1 -4 ; 1 -4 ; 1 -4 ]);
[0233] Q v = diag([1 -6 ; 1 -6 ; 1 -6 ]);
[0234] Q q = diag([2.5×10 -3 2.5×10 -3 2.5×10 -3 ]);
[0235] Q k = diag([Q r Q v Q q ]);
[0236] In this embodiment, the camera observation noise is set to 1 pixel. Assuming that n landmarks are observed at time k, the observation noise matrix is... Set to:
[0237] R k j = diag([1; 1]);
[0238] R k = diag([R k 1 ;…; R k n ]);
[0239] The camera field-of-view perturbation model proposed in this invention is an approximate representation of the actual pose error, and therefore has a certain numerical error, namely, the sum of the visibility probabilities of arbitrary configuration subsets. The numerical calculation is usually less than 1. Therefore, the threshold for the sum of the visibility probabilities of the landmark subset is set to 1. During any solution process The results will not be adopted.
[0240] The greedy algorithm is used to optimize the land reference optimization index function proposed in this invention. The simulation conditions are set as follows: (a) the initial position covariance is
[100] . 2 ;100 2;100 2 ]m, the initial velocity covariance is [0.1 2 0.1 2 0.1 2 [m / s, initial attitude covariance is [0.1] m / s, 2 0.1 2 0.1 2 ]deg; (b) The initial position covariance is [300 2 300 2 300 2 ]m, the initial velocity covariance is [0.1 2 0.1 2 0.1 2 [m / s, initial attitude covariance is [0.1] m / s, 2 0.1 2 0.1 2 ]deg; (c) The initial position covariance is [100 2 ;100 2 ;100 2 ]m, the initial velocity covariance is [0.1 2 0.1 2 0.1 2 ]m / s, initial attitude covariance is [1 2 ;1 2 ;1 2 [deg.] Using the initial position, orbital recursion is performed to predict the visible area at the next shooting time t1, and observation landmarks are planned within this area. The landmark planning results are as follows: Figure 4 , Figure 5 and Figure 6 As shown.
[0241] Monte Carlo simulation was used to verify the results of robust optimization of land references. The same landing trajectory was set, and random noise satisfying a Gaussian distribution was generated based on the system noise. 20,000 observation index calculations were performed, and the average value of the obtained observation indexes was compared with the planning index results of the proposed method. The results are shown in the table below.
[0242]
[0243] To verify the superiority of this invention, a land landmark selection method based on Fisher Information Matrix (FIM) is compared with the method proposed in this invention through simulation. The optimization function of the FIM-based land landmark selection method is:
[0244]
[0245] in, Indicates finding the matrix
[0246] Other simulation conditions are as follows: The same initial position covariance is set to
[100] . 2 ;100 2 ;100 2 ]m, the initial velocity covariance is [0.1 2 0.1 2 0.1 2 [m / s, initial attitude covariance is [0.1] m / s, 2 0.1 2 0.1 2 [deg] Using the initial position, the orbit is recursively calculated to predict the visible area at the next shooting time t1, and the observation landmarks for the next shooting time are planned within this area. Both methods use a greedy algorithm to solve the optimization function, and the planning results are as follows: Figure 7 As shown. Two benchmark optimization results were used to perform EKF Monte Carlo simulation comparison verification. With the same landing trajectory and system noise, 20,000 EKF filtering simulations were performed. The RMSE and filter divergence rate of the obtained position estimates were compared. For comparison, this embodiment sets a location estimate value. If the filter is considered to be divergent, then the RMSE of the position estimate is calculated as follows:
[0247]
[0248] in, Indicates the number of Monte Carlo simulations. This represents the position estimate obtained in the i-th simulation. Indicates the actual location. This represents the L2 norm.
[0249] The comparison results are shown in the table below.
[0250]
[0251] The comparative results show that the robust planning and optimization method for extraterrestrial landing navigation landmarks proposed in this invention can effectively improve the state estimation accuracy of the selected landmark configurations, fully consider the landmark visibility, and ensure the reliability of navigation landmark selection.
[0252] 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 robust planning and optimization method for landmarks in extraterrestrial landing navigation, characterized by: Includes the following steps: 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. Step 2: Construct the covariance matrix of the camera field of view error angle caused by attitude and position uncertainties respectively; based on the two covariance matrices, construct the camera field of view perturbation model, and further calculate the visibility probability of the landmark configuration based on the landing state and observation range of the instrument. Step 3: Construct the landmark optimization index function, and use the visibility probability value of the landmark configuration, combined with state covariance prediction, to achieve robust planning optimization of the landmark.
2. The method as described in claim 1, characterized in that: The covariance matrix of the camera field-of-view error angle caused by attitude uncertainty in step 2 is expressed as follows: in, This indicates the small-angle perturbation of the detector's attitude angle in the camera coordinate system. The covariance matrix of the Gaussian distribution it follows, i.e. .
3. The method as described in claim 1, characterized in that: The covariance matrix of the camera field-of-view error angle caused by position uncertainty in step 2 is expressed as follows: in, This represents a small perturbation in the probe's position within a fixed coordinate system on an extraterrestrial body. The covariance matrix of the Gaussian distribution it follows, i.e. ; Indicates the camera's nominal line of sight. The unit vector representing the line of sight. and This represents the coordinates of the detector's camera coordinate system in the world coordinate system. Represents the identity matrix.
4. The method as described in claim 1, characterized in that: The camera field-of-view perturbation model described in step 2 is expressed as follows: in, and This represents the perturbation angle of the field of view in the x and y directions in the detector's camera coordinate system. and This represents the perturbation angle of the detector's field of view in the camera coordinate system caused by attitude perturbation. and This represents the perturbation angle of the detector's field of view in the camera coordinate system caused by pose perturbation.
5. The method as described in claim 1, characterized in that: The method for calculating the visibility probability of landmark configurations in step 2 is as follows: According to equations (1) to (3), the covariance matrix of the camera field of view perturbation satisfies: The probability density function corresponding to the camera field of view perturbation is: Landmark configuration Visibility probability Represented as: The upper and lower bounds of the integral are: Other parameters are: In the above formula, and This represents the half-angle of the camera's field of view in the x and y directions. and Represents the set of land landmark configurations The angle between the line-of-sight envelope and the coordinate axes in the camera coordinate system.
6. The method as described in claim 1, characterized in that: Step 3 involves using the visibility probability values of the landmark configurations to obtain the set of invisible landmarks. In the configuration of landmark The remainder of the middle All visible and The method for calculating the probability value of invisible events is as follows: Record of events express exist The remainder of the middle The probability of this event occurring is expressed as follows: Record of events express exist The remainder of the middle All visible and The probability of this event occurring, which is not visible, can be expressed using the inclusion-exclusion principle as follows: in, Represents the number of sets, sets Indicates the invisible landmark set A subset of, i.e. .
7. The method as described in claim 1, characterized in that: The optimization function for constructing the landmark configuration described in step 3 is expressed as follows: in, The evaluation function is the state covariance matrix. Expressing expectations; Indicates the use of land landmark configuration The detector state covariance matrix is obtained through observation updates; robust planning and optimal selection of observation landmarks are achieved by optimizing the above landmark configuration optimization function.
8. The method as described in claim 1, characterized in that: The set of visible landmarks at the nominal observation time is: in , Let x and y represent the angles between the line of sight of land marker j at time k and the camera's optical axis in the x and y directions, respectively. , Let x and y be the half-angles of the camera's field of view. Let j be the position of the land reference j in the fixed coordinate system of extraterrestrial bodies. The z-axis component represents the position of land reference j in the detector camera coordinate system.