Evaluation and optimization method for autonomous optical navigation observation information of lunar base equipment

Abstract

The moon-based equipment autonomous optical navigation observation information evaluation optimization method disclosed by the application belongs to the field of autonomous navigation.The application realizes the method as follows: a kinematic model of moon-based equipment is established, an optical navigation position and posture estimation model is established based on a landmark observation equation, linearization equations of observation angle deviation and position deviation are obtained under the condition of small deviation linearization, position and posture information of the moon-based equipment is estimated through the linearization equations of observation angle deviation and position deviation, influence of landmark distance and observation angle on positioning accuracy of the moon-based equipment is analyzed, the observation angle is preliminarily selected, the observation angle after the preliminary selection is grouped according to the required number of observation angles, each group of observation angles is evaluated through an information entropy IE algorithm, an optimal observation angle combination is optimized according to an evaluation index to perform position and posture estimation, and the observation angle combination satisfying the moon surface autonomous optical navigation is obtained.The application can avoid influence of error-sensitive observation information on navigation performance and improve autonomous positioning accuracy.

Description

Technical Field

[0001] This invention relates to a method for evaluating and optimizing observation information for autonomous optical navigation of lunar equipment, which is particularly applicable to lunar equipment navigation systems that use optical navigation landmarks as observation information, and belongs to the field of autonomous navigation. Background Technology

[0002] Obtaining accurate position and attitude information for lunar-based equipment is a crucial prerequisite for lunar surface exploration. Due to the complex terrain, vast area to be explored, and unknown environment of the lunar surface, lunar-based equipment requires strong autonomous navigation capabilities. Because inertial measurement units (IMUs) exhibit constant bias and drift, navigation accuracy decreases over time, making it difficult for a single IMU to meet accuracy requirements. Simultaneously, with the development of optical sensors such as navigation cameras, autonomous navigation methods for lunar-based equipment based primarily on optical information have become a promising research direction. Acquiring the absolute position information of natural terrain features such as rocks and craters on the lunar surface, as well as artificial landmarks, before the mission begins, and using optical cameras to obtain these features as navigation landmarks for autonomous navigation, demonstrates high navigation performance.

[0003] When exploring the complex lunar environment, the quality of prior information obtained from the environment is low, causing lunar-based equipment to be unable to meet the requirements of long-term, high-precision navigation. Using the observation angle formed by the line-of-sight vectors of two landmarks for pose estimation can expand navigation information and enable autonomous navigation of the lunar-based equipment when it is difficult to obtain distance information between the equipment and the landmarks. However, due to the limited computing resources of the lunar-based equipment, it may not be possible to use all observation angles for real-time calculation. Furthermore, the optical navigation camera is inevitably affected by factors such as the characteristics of the optical sensor components, imaging environment conditions, and the moving target itself, leading to processing errors in landmark identification and matching, and consequently, errors in pose estimation. Therefore, selecting appropriate observation angles under varying landmark observation quality to achieve the desired performance is a key technology for optically-based landmark navigation. Summary of the Invention

[0004] The purpose of this invention is to provide a method for evaluating and optimizing autonomous optical navigation observation information for lunar-based equipment. This method establishes an optical navigation observation model based on the observation angle of a land landmark, estimates the position and attitude information of the lunar-based equipment through the optical navigation observation model, analyzes the influence of the distance to the land landmark and the observation angle on the positioning accuracy of the lunar-based equipment, and thus makes a preliminary selection of the observation angle; groups the initially selected observation angles according to the required number of observation angles, evaluates each group of observation angles using the Information Entropy (IE) algorithm, and optimizes the optimal combination of observation angles based on the evaluation index to estimate the pose, thereby obtaining the observation angles that meet the requirements for autonomous optical navigation on the lunar surface, thus realizing the evaluation and optimization of optical navigation observation information.

[0005] The objective of this invention is achieved through the following technical solution.

[0006] The method for evaluating and optimizing autonomous optical navigation observation information for lunar-based equipment disclosed in this invention includes the following steps:

[0007] Step 1: Establish a lunar surface exploration coordinate system and a kinematic model of the lunar-based equipment. Based on the terrestrial landmark observation equations, establish an optical navigation attitude estimation model. Under small-bias linearization conditions, obtain the linearized equations for observation angle deviation and position deviation. Based on the estimated position and attitude of the lunar-based equipment given by inertial navigation, obtain the estimated observation angle and estimated observation vector, respectively. Construct a linear observation matrix based on the observation vector. Use the observation angle deviation to compensate for the position estimation error, obtaining the decoupled estimation equations for the lunar-based equipment's position and attitude. These two decoupled estimation equations achieve decoupled estimation of the observation equations. The lunar surface exploration coordinate system includes the navigation coordinate system Σ and the lunar-based equipment body coordinate system Σ. b and camera coordinate system Σ c .

[0008] Define the following coordinate systems: navigation coordinate system Σ, lunar base equipment body coordinate system Σ b and camera coordinate system Σ c To simplify calculations, the navigation coordinate system is defined as the initial coordinate system of the lunar base equipment, which is stationary relative to the Moon and is a Cartesian coordinate system in three-dimensional space. The position coordinates of the lunar base equipment are represented in the navigation coordinate system. The origin B of the lunar base equipment's coordinate system is taken at the center of mass of the lunar base equipment, X... b The axis is parallel to the chassis plane and points forward, Y b The axis is parallel to the chassis plane and points to the left, Z b The axis is perpendicular to the chassis plane and points upwards. Camera coordinate system Σ c The origin C is located at the optical center of the camera. b The axis is parallel to the camera's optical axis and points towards the center of the target. X c and Y c Directional indicator image plane.

[0009] The kinematic model of lunar-based equipment is as follows:

[0010] x v (k+1)=f v (x v (k))+V v (k) (1)

[0011] In the formula, x v f represents the state of lunar-based equipment. v Define the kinematic model of lunar-based equipment, V v The mean is zero and the variance is Q. v Gaussian white noise.

[0012] In the camera coordinate system, the location of the navigation landmark is:

[0013] r i b =C cb C bw (r-ρ i ) = C cw (r-ρ i (2)

[0014] In the formula, r i b Let r be the position of the navigation landmark, r be the position of the lunar base equipment in the navigation coordinate system, and ρ be the position of the navigation landmark. i For the i-th landmark location, the transformation matrix from the navigation coordinate system to the lunar base equipment coordinate system is denoted as C. bw The transformation matrix from the lunar base equipment's body coordinate system to the camera coordinate system is denoted as C. cb .

[0015] If the origin of the camera coordinate system coincides with the origin of the lunar base equipment's coordinate system, then the pixel and image line coordinates of the land target will be represented as:

[0016]

[0017] In the formula, p i Let l be the pixel coordinates of the landmark. i x, y, z are the image line coordinates of the land-based target, and x, y, z are the three-axis position coordinates of the lunar-based equipment. i y i z i C represents the coordinates of the three axes of the land reference. ij For the transfer matrix C cw The equivalent coefficient is f, where f is the focal length of the navigation camera.

[0018] Let A represent the observation angle of the line-of-sight vectors of the i-th and j-th landmarks. ij ,but:

[0019]

[0020] In the formula, r i r j r represents the position of the land landmark relative to the lunar base equipment in the navigation coordinate system. i r j This indicates the distance between the land-based marker and the lunar-based equipment.

[0021] Using the pixel image line coordinates in an optical image, the observation angle is expressed as:

[0022]

[0023] For connecting two landmarks P i P j line segment P i P j There must exist a toroidal surface, which is obtained by passing through P. i P j The arc generated by rotating the axis satisfies the arbitrary angle formed between line segments connecting any point on the annular surface, and the two landmarks satisfy the same A. ij The center of arc O is located on line segment P. i P j On the perpendicular bisector of the arc. The radius of the arc can be determined by P. i P j Length and A ij The angle is calculated as follows:

[0024]

[0025] In the formula, R is the radius of the arc. The distance from center O to line segment P... i P j The distance is:

[0026] L=RcosA ij (7)

[0027] According to equation (7), r is obtained. i and r j The inner product relationship, equation (7), can be expressed by a vector equation:

[0028] (r-ρ i )·(r-ρ j )=|r-ρ i ||r-ρ j |cosA ij (8)

[0029] From equation (8), we know that the observed angle A ij It depends only on the position r of the lunar base equipment and is independent of the attitude state of the lunar base equipment. Therefore, the position of the lunar base equipment can be obtained independently by solving equation (8).

[0030] To address the difficulty of directly solving equation (8) nonlinearly, it is linearized based on the small deviation linearization condition, resulting in a linear measurement equation around the nominal trajectory. Under the small deviation linearization condition, equation (8) becomes:

[0031]

[0032] The unit line-of-sight vector is expressed as The above formula simplifies to:

[0033]

[0034] The auxiliary vector is defined as:

[0035]

[0036] Equation (10) can be expressed using an auxiliary vector as follows:

[0037]

[0038] Define the observation vector as:

[0039]

[0040] Equation (12) is expressed as:

[0041] δA ij =h ij δr (14)

[0042] During the actual exploration process of the lunar-based equipment, the estimated position r given by the inertial navigation algorithm was used. * , obtain the observation angle and observation vector h ij The estimated value. Then, using equation (14) with the observation angle deviation δA ij Compensating for the estimated position error δr can improve the accuracy of the current position. For the case of n landmarks, the corresponding observation angle deviation is:

[0043] δA=[δA 12 δA 13 … δA ij … δA (n-1)n ] T i,j∈{1,2…n},i≠j (15)

[0044] The linear observation matrix is ​​represented as:

[0045] H = [h] 12 h 13 … h ij … h (n-1)n ] T (16)

[0046] have:

[0047] δA=Hδr (17)

[0048] The current position vector of the lunar-based equipment is updated as follows:

[0049] r = r * +(H T H) -1 H T δA (18)

[0050] According to equation (2), in the camera coordinate system, the line-of-sight vector of the landmark is expressed as:

[0051]

[0052] Equation (19) can be expressed using the pixel coordinates of the landmass as follows:

[0053]

[0054] Given the position of the lunar base equipment, based on the principle of multi-vector attitude determination, the optimal solution for the attitude transfer matrix from the navigation coordinate system to the lunar base equipment body coordinate system is obtained, expressed as:

[0055]

[0056] In the formula,

[0057]

[0058] Given the location of the land landmark, the position of the lunar base relative to the navigation coordinate system can be obtained by iterating through equation (18), and the attitude of the lunar base relative to the navigation coordinate system can be determined by equation (21).

[0059] Step 2: Utilizing the geometric relationship between lunar-based equipment and land-based landmarks, under small-bias linearization, the observation angle A is... ij With the size and position of the landmark j unchanged, the linearized equation for the observation angle deviation constructed in step one is simplified. Based on this equation, the impact of changes in the position of landmark i on the accuracy of lunar equipment position estimation is analyzed. Considering the influence of changes in the position of landmark i on the accuracy of lunar equipment position estimation, preliminary boundary conditions for selecting the observation angle are set based on the sensitivity of lunar equipment position changes to changes in landmark position and observation angle. These preliminary boundary conditions are then used to quickly select the observation angle, yielding an observation angle that is insensitive to landmark observation errors.

[0060] During the exploration process, there are situations where multiple navigation landmarks are observed simultaneously. Each pair of navigation landmarks forms an observation angle. Calculating each observation angle individually significantly increases the burden of angle calculation; moreover, low-quality fusion of observation angles not only fails to improve pose estimation accuracy but also leads to a decrease in accuracy. Therefore, it is necessary to optimize the selection of observation angles.

[0061] Observation angle A ij With the size and position of landmark j unchanged, we analyze the impact of changes in the position of landmark i on the accuracy of lunar base equipment position estimation. Equation (9) can be rewritten as:

[0062]

[0063] The first half of the right side of the equation represents the influence of landmark i on pose estimation, and the second half of the right side of the equation represents the influence of landmark j on pose estimation.

[0064] When r i =r j cos A ij At this time, the observation vector h and the observation vector of the landmark j point in the same direction, and the positional deviation δr i The direction of change is from landmark i to landmark j, and the rate of change is |r i -r j |(r i 3 tan A ij ) -1 When r i <r j cos A ij At that time, the observation vector h is located outside the observation angle, closer to the observation vector side of the landmark j, with a positional deviation δr. i The angle between the direction of change and the observed vector of the landmark j decreases, and the rate of change increases, leading to L i The increase in this magnitude makes the lunar base equipment position deviation δr more sensitive to errors. The same logic applies to the analysis of the land-based target j.

[0065] The initial boundary selection for the observation angle is as follows:

[0066]

[0067] When the boundary conditions for the preliminary selection of the observation angle shown in Equation (23) are met, the observation vector h is located within the observation angle. At this time, the position deviation δr of the lunar base equipment is not sensitive to the position error of the landmass. Using Equation (23), the observation angle is quickly and initially selected to obtain the observation angle that is not sensitive to the observation error of the landmass.

[0068] Step 3: Group the observation angles initially selected in Step 2 according to the required number of observation angles, evaluate each group of observation angles using the Information Entropy (IE) algorithm, and select the optimal combination of observation angles based on the evaluation criteria.

[0069] The location of the i-th landmark is represented by ρ. i This means that, since two landmarks form an observation angle, the state equation for the nth observation angle is:

[0070] ψ n (k+1)=[ρ i (k); ρ j (k)] (24)

[0071] The enhanced state equation, which includes the lunar base equipment status and all observation angle statuses, is expressed as follows:

[0072] x(k+1)=f(x(k),ψ(k))+V(k) (25)

[0073] In the formula

[0074]

[0075] Pose estimation using observation angles requires multiple observation angles. If n observation angles are selected for pose estimation, then the number of combinations of observation angles is: The observation model for the m-th observation angle is written in the following form:

[0076] z m (k)=H m (x(k),Ω m (k))+w v (k) (26)

[0077] In the formula, Ω m (k)=[Ψ1(k);Ψ2(k);…;Ψ6(k)] is the state vector of 6 observation angles, and the output function H m The sensor output is correlated with the state vector when observing the m-th set of observation angles, w v (k) Gaussian white noise.

[0078] Given a set of measurements Z(k+1)={z(k+1),z(k),…,z(1)}, the posterior probability density function PDF of x(k+1) can be expressed by Bayes' theorem as:

[0079]

[0080] The nonlinear Kalman filter (EKF) is an approximation of Bayes' theorem. It is an efficient estimator for calculating the state of a given nonlinear dynamic system, defining the noise process through Gaussian noise modeling, and minimizing errors due to the linearization of the nonlinear system. The EKF provides the system state information for a lunar-based equipment navigation system. The estimation of the state P and the estimation of its covariance. The estimation of the state covariance provides an estimate of the state covariance. The confidence level estimate. The form of the system's estimation error covariance P is:

[0081]

[0082] In the formula, submatrix P vv P vi and P ii These are the covariances from lunar base equipment to lunar base equipment, from lunar base equipment to observation angle, and from observation angle to observation angle. They are used to distinguish the uncertainties associated with lunar base equipment and each individual observation angle.

[0083] The criterion used to optimize sensor measurements in a system described by a continuous probability distribution is based on minimizing the expected value of the entropy change of the lunar-based equipment navigation system itself. The IE of a continuous signal source is:

[0084] H(x)=-∫p(x)lnp(x)dx (29)

[0085] The impact of a set of sensor data on the confidence level of the lunar base equipment location is determined according to equation (29):

[0086]

[0087] Equation (30) is the entropy of the posterior distribution before the addition of measurement data. The probability distribution of the random variable x is a Gaussian distribution, let m i For variable x i The mean of the two centers is then μ. ij =E[(x i -m i (x) j -m j Then equation (30) transforms into:

[0088]

[0089] Therefore, for a Gaussian distributed vector, calculating its entropy only requires the dimension T and covariance P. The prior information metric is defined as:

[0090]

[0091] At time step k+1, the system is updated using only one set of observation angles to obtain the covariance P. k+1|k+1,m At the same time step, update the system using only one set of observation angles for all combinations of observation angles to obtain the covariance P. k+1|k+1,m The operation is as follows. The possible contribution of each set of observed angles to the positioning is obtained according to equation (31):

[0092]

[0093] The information difference for each group of observed angles in the map will be calculated at time step k+1, as shown below:

[0094] Δi=im k+1|k+1,m -im k+1|k (34)

[0095] According to the evaluation index Δi described in equation (34), the combination of observation angles with Δi greater than the preset threshold is selected. That is, according to equations (32)-(34), each group of observation angles is evaluated by the information entropy IE algorithm to select the optimal combination of observation angles.

[0096] Step 4: Use the observation angle selected in Step 3 to perform pose estimation, avoid the impact of error-sensitive observation information on navigation performance, improve the autonomous positioning accuracy of lunar-based equipment, and improve the robustness and efficiency of autonomous optical navigation of lunar-based equipment.

[0097] Beneficial effects:

[0098] 1. The method for evaluating and optimizing autonomous optical navigation observation information of lunar-based equipment disclosed in this invention uses the observation angle of the land reference to decouple the position and attitude of the lunar-based equipment in the observation equation, which improves the efficiency of solving the position and attitude of the lunar-based equipment compared with directly solving the observation equation.

[0099] 2. The method for evaluating and optimizing observation information for autonomous optical navigation of lunar equipment disclosed in this invention optimizes the observation angle by using the geometric relationship between the land landmark and the lunar equipment and the information entropy algorithm. Compared with using all observation angles, it reduces the pose calculation time.

[0100] 3. The method for evaluating and optimizing autonomous optical navigation observation information for lunar-based equipment disclosed in this invention utilizes the geometric relationship between the lunar-based equipment and land landmarks to optimize the observation angle A under small-deviation linearization. ij With the size and position of the landmark j unchanged, the linearization equation for the observation angle deviation is simplified. Based on this equation, the impact of changes in the position of landmark i on the accuracy of lunar-based equipment position estimation is analyzed. Considering the impact of landmark i position changes on the accuracy of lunar-based equipment position estimation, preliminary boundary conditions for observation angle selection are set based on the sensitivity of lunar-based equipment position changes to landmark position and observation angle changes. These preliminary boundary conditions are used to quickly select observation angles that are insensitive to landmark observation errors. The initially selected observation angles are grouped according to the required number of observation angles. Each group of observation angles is evaluated using the Information Entropy (IE) algorithm. The optimal combination of observation angles is selected based on the evaluation criteria. This optimal combination avoids the influence of error-sensitive observation information on the navigation system, thereby improving the accuracy and robustness of the lunar-based equipment navigation system. Attached Figure Description

[0101] Figure 1 This is a flowchart of the method for evaluating and selecting the best autonomous optical navigation observation information for lunar-based equipment disclosed in this invention;

[0102] Figure 2 This is a schematic diagram of the location of the landmark and the observation vector in a specific implementation method of the present invention;

[0103] Figure 3 This is a schematic diagram showing the positions of lunar-based equipment and land landmarks within the field of view in a specific embodiment of the present invention;

[0104] Figure 4 The figure shows the simulation results of the random selection of observation angle method and the error of this method in a specific embodiment of the present invention. Figure 4 (a) is a comparison chart of position errors, and 4(b) is a comparison chart of attitude errors. Detailed Implementation

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

[0106] To verify the feasibility of the present invention, this embodiment takes lunar surface exploration using lunar-based equipment as an example.

[0107] like Figure 1 As shown in this embodiment, the method for evaluating and optimizing autonomous optical navigation observation information for lunar-based equipment has the following specific implementation steps:

[0108] Step 1: Establish a lunar surface exploration coordinate system and a kinematic model of the lunar-based equipment. Based on the terrestrial landmark observation equations, establish an optical navigation attitude estimation model. Under small-bias linearization conditions, obtain the linearized equations for observation angle deviation and position deviation. Based on the estimated position and attitude of the lunar-based equipment given by inertial navigation, obtain the estimated observation angle and estimated observation vector, respectively. Construct a linear observation matrix based on the observation vector. Use the observation angle deviation to compensate for the position estimation error, obtaining the decoupled estimation equations for the lunar-based equipment's position and attitude. These two decoupled estimation equations achieve decoupled estimation of the observation equations. The lunar surface exploration coordinate system includes the navigation coordinate system Σ and the lunar-based equipment body coordinate system Σ. b and camera coordinate system Σ c .

[0109] Define the following coordinate systems: navigation coordinate system Σ, lunar base equipment body coordinate system Σ b and camera coordinate system Σ c To simplify calculations, the navigation coordinate system is defined as the initial coordinate system of the lunar base equipment, which is stationary relative to the Moon and is a Cartesian coordinate system in three-dimensional space. The position coordinates of the lunar base equipment are represented in the navigation coordinate system. The origin B of the lunar base equipment's coordinate system is taken at the center of mass of the lunar base equipment, X... b The axis is parallel to the chassis plane and points forward, Y b The axis is parallel to the chassis plane and points to the left, Z b The axis is perpendicular to the chassis plane and points upwards. Camera coordinate system Σ c The origin C is located at the optical center of the camera. b The axis is parallel to the camera's optical axis and points towards the center of the target. X c and Y c Directional indicator image plane.

[0110] The kinematic model of lunar-based equipment is as follows:

[0111] x v (k+1)=fv (x v (k))+V v (k) (35)

[0112] In the formula, x v f represents the state of lunar-based equipment. v Define the kinematic model of lunar-based equipment, V v The mean is zero and the variance is Q. v Gaussian white noise.

[0113] In the camera coordinate system, the location of the navigation landmark is:

[0114] r i b =C cb C bw (r-ρ i ) = C cw (r-ρ i (36)

[0115] In the formula, r i b Let r be the position of the navigation landmark, r be the position of the lunar base equipment in the navigation coordinate system, and ρ be the position of the navigation landmark. i For the i-th landmark location, the transformation matrix from the navigation coordinate system to the lunar base equipment coordinate system is denoted as C. bw The transformation matrix from the lunar base equipment's body coordinate system to the camera coordinate system is denoted as C. cb .

[0116] If the origin of the camera coordinate system coincides with the origin of the lunar base equipment's coordinate system, then the pixel and image line coordinates of the land target will be represented as:

[0117]

[0118] In the formula, p i Let l be the pixel coordinates of the landmark. i x, y, z are the image line coordinates of the land-based target, and x, y, z are the three-axis position coordinates of the lunar-based equipment. i y i z i C represents the coordinates of the three axes of the land reference. ij For the transfer matrix C cw The equivalent coefficient is f, where f is the focal length of the navigation camera.

[0119] Let A represent the observation angle of the line-of-sight vectors of the i-th and j-th landmarks. ij ,but:

[0120]

[0121] In the formula, r i r jr represents the position of the land landmark relative to the lunar base equipment in the navigation coordinate system. i r j This indicates the distance between the land-based marker and the lunar-based equipment.

[0122] Using the pixel image line coordinates in an optical image, the observation angle is expressed as:

[0123]

[0124] For connecting two landmarks P i P j line segment P i P j There must exist a toroidal surface, which is obtained by passing through P. i P j The arc generated by rotating the axis satisfies the arbitrary angle formed between line segments connecting any point on the annular surface, and the two landmarks satisfy the same A. ij The center of arc O is located on line segment P. i P j On the perpendicular bisector of the arc. The radius of the arc can be determined by P. i P j Length and A ij The angle is calculated as follows:

[0125]

[0126] In the formula, R is the radius of the arc. The distance from center O to line segment P... i P j The distance is:

[0127] L=RcosA ij (41)

[0128] According to equation (41), r is obtained. i and r j The inner product relationship, equation (7), can be expressed by a vector equation:

[0129] (r-ρ i )·(r-ρ j )=|r-ρ i ||r-ρ j |cosA ij (42)

[0130] From equation (42), we know that the observed angle A ij It depends only on the position r of the lunar base equipment and is independent of the attitude state of the lunar base equipment. Therefore, the position of the lunar base equipment can be obtained independently by solving equation (42).

[0131] To address the difficulty of directly solving the nonlinear equation (42), it is linearized based on the small deviation linearization condition, resulting in a linear measurement equation around the nominal trajectory. Under the small deviation linearization condition, equation (42) becomes:

[0132]

[0133] The unit line-of-sight vector is expressed as The above formula simplifies to:

[0134]

[0135] The auxiliary vector is defined as:

[0136]

[0137] Equation (44) can be expressed using an auxiliary vector as follows:

[0138]

[0139] Define the observation vector as:

[0140]

[0141] Equation (43) is expressed as:

[0142] δA ij =h ij δr (48)

[0143] During the actual exploration process of the lunar-based equipment, the estimated position r given by the inertial navigation algorithm was used. * , obtain the observation angle and observation vector h ij The estimated value. Then, using equation (48) with the observation angle deviation δA ij Compensating for the estimated position error δr can improve the accuracy of the current position. For the case of n landmarks, the corresponding observation angle deviation is:

[0144] δA=[δA 12 δA 13 … δA ij … δA (n-1)n ] T i,j∈{1,2…n},i≠j (49)

[0145] The linear observation matrix is ​​represented as:

[0146] H = [h] 12 h 13 … h ij … h (n-1)n ] T (50)

[0147] Therefore:

[0148] δA=Hδr (51)

[0149] The current position vector of the lunar-based equipment is updated as follows:

[0150] r = r * +(H T H) -1 H T δA (52)

[0151] According to equation (36), in the camera coordinate system, the line-of-sight vector of the landmark is expressed as:

[0152]

[0153] Equation (53) can be expressed using the pixel coordinates of the landmass as follows:

[0154]

[0155] Given the position of the lunar base equipment, based on the principle of multi-vector attitude determination, the optimal solution for the attitude transfer matrix from the navigation coordinate system to the lunar base equipment body coordinate system is obtained, expressed as:

[0156]

[0157] In the formula,

[0158]

[0159] Given the location of the landmark, the position of the lunar base relative to the navigation coordinate system can be obtained by iterating through equation (52), and the attitude of the lunar base relative to the navigation coordinate system can be determined by equation (55).

[0160] Step 2: Utilizing the geometric relationship between lunar-based equipment and land-based landmarks, under small-bias linearization, the observation angle A is... ij With the size and position of the landmark j unchanged, the linearized equation for the observation angle deviation constructed in step one is simplified. Based on this equation, the impact of changes in the position of landmark i on the accuracy of lunar equipment position estimation is analyzed. Considering the influence of changes in the position of landmark i on the accuracy of lunar equipment position estimation, preliminary boundary conditions for selecting the observation angle are set based on the sensitivity of lunar equipment position changes to changes in landmark position and observation angle. These preliminary boundary conditions are then used to quickly select the observation angle, yielding an observation angle that is insensitive to landmark observation errors.

[0161] During the exploration process, there are situations where multiple navigation landmarks are observed simultaneously. Each pair of navigation landmarks forms an observation angle. Calculating each observation angle individually significantly increases the burden of angle calculation; moreover, low-quality fusion of observation angles not only fails to improve pose estimation accuracy but also leads to a decrease in accuracy. Therefore, it is necessary to optimize the selection of observation angles.

[0162] Observation angle A ij With the size and position of landmark j unchanged, we analyze the impact of changes in the position of landmark i on the accuracy of lunar base equipment position estimation. Equation (43) can be rewritten as:

[0163]

[0164] The first half of the right side of the equation represents the influence of landmark i on pose estimation, and the second half of the right side of the equation represents the influence of landmark j on pose estimation.

[0165] When r i =r j cos A ij At this time, the observation vector h and the observation vector of the landmark j point in the same direction, and the positional deviation δr i The direction of change is from landmark i to landmark j, and the rate of change is |r i -r j |(r i 3 tan A ij ) -1 When r i <r j cos A ij At that time, the observation vector h is located outside the observation angle, closer to the observation vector side of the landmark j, with a positional deviation δr. i The angle between the direction of change and the observed vector of the landmark j decreases, and the rate of change increases, leading to L i The increase in this magnitude makes the lunar base equipment position deviation δr more sensitive to errors. The same logic applies to the analysis of the land-based target j.

[0166] The initial boundary selection for the observation angle is as follows:

[0167]

[0168] When the preliminary boundary conditions for selecting the observation angle shown in equation (57) are met, the observation vector h is located within the observation angle. At this time, the lunar base equipment position deviation δr is not sensitive to the landmass position error. Using equation (57), a rapid preliminary selection of the observation angle is obtained, yielding an observation angle that is insensitive to landmass observation errors. The relationship between the landmass position and the observation vector is as follows: Figure 2 As shown.

[0169] The initial position of the lunar-based equipment is [1.2475; 0; 0], and the initial attitude is [0; 0; 0]. At this time, there are 12 landmarks within the camera's field of view. Assuming that 6 observation angles are selected, the number of observation angle combinations after the initial selection is 3003. The positions of the lunar-based equipment and the landmarks within the field of view are as follows: Figure 3 As shown.

[0170] Step 3: Group the observation angles initially selected in Step 2 according to the required number of observation angles, evaluate each group of observation angles using the Information Entropy (IE) algorithm, and select the optimal combination of observation angles based on the evaluation criteria.

[0171] The location of the i-th landmark is represented by ρ. i This means that, since two landmarks form an observation angle, the state equation for the nth observation angle is:

[0172] ψ n (k+1)=[ρ i (k); ρ j (k)] (58)

[0173] The enhanced state equation, which includes the lunar base equipment status and all observation angle statuses, is expressed as follows:

[0174] x(k+1)=f(x(k),ψ(k))+V(k) (59)

[0175] In the formula

[0176]

[0177] Pose estimation using observation angles requires multiple observation angles. If n observation angles are selected for pose estimation, then the number of combinations of observation angles is: The observation model for the m-th observation angle is written in the following form:

[0178] z m (k)=H m (x(k),Ω m (k))+w v (k) (60)

[0179] In the formula, Ω m (k)=[Ψ1(k);Ψ2(k);…;Ψ6(k)] is the state vector of 6 observation angles, and the output function H m The sensor output is correlated with the state vector when observing the m-th set of observation angles, w v (k) Gaussian white noise.

[0180] Given a set of measurements Z(k+1)={z(k+1),z(k),…,z(1)}, the posterior probability density function PDF of x(k+1) can be expressed by Bayes' theorem as:

[0181]

[0182] The nonlinear Kalman filter (EKF) is an approximation of Bayes' theorem. It is an efficient estimator for calculating the state of a given nonlinear dynamic system, and it defines that noisy processes can be well modeled by Gaussian noise, with minimal error due to the linearization of the nonlinear system. The EKF provides the system state information for a lunar-based equipment navigation system. The estimation of the state P and the estimation of its covariance. The estimation of the state covariance provides an estimate of the state covariance. The confidence level estimate. The form of the system's estimation error covariance P is:

[0183]

[0184] In the formula, submatrix P vv P vi and P ii These are the covariances from lunar base equipment to lunar base equipment, from lunar base equipment to observation angle, and from observation angle to observation angle. They are used to distinguish the uncertainties associated with lunar base equipment and each individual observation angle.

[0185] The criterion used to optimize sensor measurements in a system described by a continuous probability distribution is based on minimizing the expected value of the entropy change of the lunar-based equipment navigation system itself. The IE of a continuous signal source is:

[0186] H(x)=-∫p(x)lnp(x)dx (63)

[0187] The impact of a set of sensor data on the confidence level of the lunar base equipment location is determined according to equation (63):

[0188]

[0189] Equation (64) is the entropy of the posterior distribution before the addition of measurement data. The probability distribution of the random variable x is a Gaussian distribution, let m i For variable x i The mean of the two centers is then μ. ij =E[(x i -m i (x) j -m j Then equation (64) transforms into:

[0190]

[0191] Therefore, for a Gaussian distributed vector, calculating its entropy only requires the dimension T and covariance P. The prior information metric is defined as:

[0192]

[0193] At time step k+1, the system is updated using only one set of observation angles to obtain the covariance P. k+1|k+1,m At the same time step, update the system using only one set of observation angles for all combinations of observation angles to obtain the covariance P. k+1|k+1,m The operation is as follows. The possible contribution of each set of observed angles to the positioning is obtained according to equation (65):

[0194]

[0195] The information difference for each group of observed angles in the map will be calculated at time step k+1, as shown below:

[0196] Δi=im k+1|k+1,m -im k+1|k (68)

[0197] According to the evaluation index Δi described in equation (34), the combination of observation angles with Δi greater than the preset threshold is selected. That is, according to equations (66)-(68), each group of observation angles is evaluated by the information entropy IE algorithm to select the optimal combination of observation angles.

[0198] Step 4: Use the observation angle selected in Step 3 to perform pose estimation, avoid the impact of error-sensitive observation information on navigation performance, improve the autonomous positioning accuracy of lunar-based equipment, and improve the robustness and efficiency of the autonomous optical navigation algorithm of lunar-based equipment.

[0199] To verify the superiority of our proposed method for lunar-based equipment pose estimation, we used randomly selected observation angles to update the lunar-based equipment's pose, comparing it with our proposed method. After 1000 Monte Carlo simulations, the error simulation results for the lunar-based equipment random observation angle selection method and our proposed method are shown below. Figure 4 As shown, where Figure 4 (a) is a comparison chart of position errors, and (b) is a comparison chart of attitude errors. The pose estimation results are shown in the table below.

[0200] Table 1 Comparison of position estimation errors between the proposed method and different methods.

[0201]

[0202] Table 2 Comparison of attitude estimation error results between the proposed method and different methods

[0203]

[0204] Simulation results demonstrate that the method described in this invention exhibits higher pose estimation accuracy and stability, regardless of the mean or standard deviation, thus validating the effectiveness of the selected results. This indicates that the quality of the observation angles significantly impacts pose determination accuracy. This method, based on decoupling the observation equations using observation angles, introduces information entropy to optimize the combination of observation angles, avoiding the impact of error-sensitive observation information on navigation accuracy, reducing computational burden, and ultimately improving the pose estimation accuracy of lunar-based equipment.

[0205] Thus, the optimal evaluation method for autonomous optical navigation observation information of lunar-based equipment was completed.

[0206] 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 evaluating and optimizing autonomous optical navigation observation information for lunar-based equipment, characterized in that: Includes the following steps, Step 1: Establish a lunar surface exploration coordinate system and a kinematic model of the lunar-based equipment. Based on the terrestrial landmark observation equations, establish an optical navigation attitude estimation model. Under small-deviation linearization conditions, obtain the linearized equations for observation angle deviation and position deviation. Based on the estimated position and attitude of the lunar-based equipment given by inertial navigation, obtain the estimated observation angle and estimated observation vector, respectively. Construct a linear observation matrix based on the observation vector. Use the observation angle deviation to compensate for the position estimation error, obtaining the decoupled estimation equations for the lunar-based equipment's position and attitude. These two decoupled estimation equations achieve decoupled estimation of the observation equations. The lunar surface exploration coordinate system includes the navigation coordinate system Σ and the lunar-based equipment body coordinate system Σ. b and camera coordinate system Σ c ; Step 2: Utilizing the geometric relationship between lunar-based equipment and land-based landmarks, under small-bias linearization, the observation angle A is... ij With the size and position of the landmark j unchanged, the linearized equation of the observation angle deviation constructed in step one is simplified. The influence of the change in the position of the landmark i on the accuracy of the position estimation of the lunar equipment is analyzed based on the linearized equation of the observation angle deviation. Based on the influence of the change in the position of the landmark i on the accuracy of the position estimation of the lunar equipment, the preliminary boundary conditions for the selection of the observation angle are set based on the sensitivity of the change in the position of the lunar equipment to the changes in the position of the landmark and the observation angle. The preliminary boundary conditions for the selection of the observation angle are used to quickly select the observation angle, and the observation angle that is not sensitive to the error of the landmark observation is obtained. Step 3: Group the observation angles initially selected in Step 2 according to the required number of observation angles, evaluate each group of observation angles using the Information Entropy (IE) algorithm, and select the optimal combination of observation angles based on the evaluation criteria.

2. The method for evaluating and optimizing autonomous optical navigation observation information for lunar-based equipment as described in claim 1, characterized in that: It also includes step four: using the observation angle selected in step three to perform pose estimation, avoiding the impact of error-sensitive observation information on navigation performance, improving the autonomous positioning accuracy of lunar-based equipment, and improving the robustness and efficiency of autonomous optical navigation of lunar-based equipment.

3. The method for evaluating and optimizing autonomous optical navigation observation information for lunar-based equipment as described in claim 1 or 2, characterized in that: The implementation method for step one is as follows: Define the following coordinate systems: navigation coordinate system Σ, lunar base equipment body coordinate system Σ b and camera coordinate system Σ c To simplify calculations, the navigation coordinate system is defined as the initial coordinate system of the lunar base equipment, which is stationary relative to the moon and is a Cartesian coordinate system in three-dimensional space. The position coordinates of the lunar base equipment are represented in the navigation coordinate system. The origin B of the lunar base equipment's coordinate system is taken at the center of mass of the lunar base equipment. b The axis is parallel to the chassis plane and points forward, Y b The axis is parallel to the chassis plane and points to the left, Z b The axis is perpendicular to the chassis plane and points upward; camera coordinate system Σ c The origin C is located at the optical center of the camera; Z b The X axis is parallel to the camera's optical axis and points towards the center of the target object; c and Y c Directional marker image plane; The kinematic model of lunar-based equipment is as follows: x v (k+1)=f v (x v (k))+V v (k) (1) In the formula, x v f represents the state of lunar-based equipment. v Define the kinematic model of lunar-based equipment, V v The mean is zero and the variance is Q. v Gaussian white noise; In the camera coordinate system, the location of the navigation landmark is: In the formula, Let r be the position of the navigation landmark, r be the position of the lunar base equipment in the navigation coordinate system, and ρ be the position of the navigation landmark. i For the i-th landmark location, the transformation matrix from the navigation coordinate system to the lunar base equipment coordinate system is denoted as C. bw The transformation matrix from the lunar base equipment's body coordinate system to the camera coordinate system is denoted as C. cb ; If the origin of the camera coordinate system coincides with the origin of the lunar base equipment's coordinate system, then the pixel and image line coordinates of the land target will be represented as: In the formula, p i Let l be the pixel coordinates of the landmark. i x, y, z are the image line coordinates of the land-based target, and x, y, z are the three-axis position coordinates of the lunar-based equipment. i y i z i C represents the coordinates of the three axes of the land reference. ij For the transfer matrix C cw The equivalent coefficient, where f is the focal length of the navigation camera; Let A represent the observation angle of the line-of-sight vectors of the i-th and j-th landmarks. ij ,but: In the formula, r i r j r represents the position of the land landmark relative to the lunar base equipment in the navigation coordinate system. i r j Indicates the distance between the land-based marker and the lunar-based equipment; Using the pixel image line coordinates in an optical image, the observation angle is expressed as: For connecting two landmarks P i P j line segment P i P j There must exist a toroidal surface, which is obtained by passing through P. i P j The arc generated by rotating the axis satisfies the arbitrary angle formed between line segments connecting any point on the annular surface, and the two landmarks satisfy the same A. ij The center of arc O is located on line segment P. i P j On the perpendicular bisector; the radius of the arc can be obtained through P. i P j Length and A ij The angle is calculated as follows: In the formula, R is the radius of the arc; the distance from the center O to the line segment P... i P j The distance is: L=RcosA ij (7) According to equation (7), r is obtained. i and r j The inner product relationship, equation (7), can be expressed by a vector equation: (r-r i )·(r-ρ j )=|r-ρ i ||r-r j |cosA ij (8) From equation (8), we know that the observed angle A ij It depends only on the position r of the lunar base equipment and is independent of the attitude state of the lunar base equipment; therefore, the position of the lunar base equipment can be obtained independently by solving equation (8). Linearizing it based on the small deviation linearization condition yields a linear measurement equation around the nominal trajectory; under the small deviation linearization condition, equation (8) is obtained: The unit line-of-sight vector is expressed as The above formula simplifies to: The auxiliary vector is defined as: Equation (10) can be expressed using an auxiliary vector as follows: Define the observation vector as: Equation (12) is expressed as: δA ij =h ij δr (14) During the actual exploration process of the lunar-based equipment, the estimated position r given by the inertial navigation algorithm was used. * , obtain the observation angle and observation vector h ij The estimated value; then, using equation (14) with the observation angle deviation δA ij Compensating for the estimated position error δr can improve the accuracy of the current position; for the case of n landmarks, the corresponding observation angle deviation is: δA=[δA 12 δA 13 …δA ij …δA (n-1)n ] T i,j∈{1,2…n},i≠j (15) The linear observation matrix is ​​represented as: H=[h 12 h 13 …h ij …h (n-1)n ] T (16) have: δA=Hδr (17) The current position vector of the lunar-based equipment is updated as follows: r=r * +(H T H) -1 H T δA (18) According to equation (2), in the camera coordinate system, the line-of-sight vector of the landmark is expressed as: Equation (19) can be expressed using the pixel coordinates of the landmass as follows: Given the position of the lunar base equipment, based on the principle of multi-vector attitude determination, the optimal solution for the attitude transfer matrix from the navigation coordinate system to the lunar base equipment body coordinate system is obtained, expressed as: In the formula, Given the location of the landmark, the position of the lunar base relative to the navigation coordinate system can be obtained by iterating through equation (18), and the attitude of the lunar base relative to the navigation coordinate system is determined by equation (21).

4. The method for evaluating and optimizing autonomous optical navigation observation information for lunar-based equipment as described in claim 3, characterized in that: The second step is implemented as follows: Observation angle A ij With the size and position of the landmark j unchanged, we analyze the impact of changes in the position of the landmark i on the accuracy of the lunar base equipment position estimation; we rewrite equation (9) as follows: The first half of the right side of the equation represents the influence of landmark i on pose estimation, and the second half of the right side of the equation represents the influence of landmark j on pose estimation. When r i =r j cosA ij At this time, the observation vector h and the observation vector of the landmark j point in the same direction, and the positional deviation δr i The direction of change is from landmark i to landmark j, and the rate of change is... When r i <r j cosA ij At that time, the observation vector h is located outside the observation angle, closer to the observation vector side of the landmark j, with a positional deviation δr. i The angle between the direction of change and the observed vector of the landmark j decreases, and the rate of change increases, leading to L i The increase in this value makes the lunar base equipment position deviation δr more sensitive to errors; the same logic applies to the analysis of the land-based target j. The initial boundary selection for the observation angle is as follows: When the boundary conditions for preliminary selection of the observation angle shown in Equation (23) are met, the observation vector h is located within the observation angle. At this time, the position deviation δr of the lunar base equipment is not sensitive to the position error of the landmass. Using Equation (23), the observation angle is quickly and initially selected to obtain the observation angle that is not sensitive to the observation error of the landmass.

5. The method for evaluating and optimizing autonomous optical navigation observation information for lunar-based equipment as described in claim 4, characterized in that: The method for implementing step three is as follows: The location of the i-th landmark is represented by ρ. i This means that, since two landmarks form an observation angle, the state equation for the nth observation angle is: ψ n (k+1)=[ρ i (k);p j (k)] (24) The enhanced state equation, which includes the lunar base equipment status and all observation angle statuses, is expressed as follows: x(k+1)=f(x(k),ψ(k))+V(k) (25) In the formula Pose estimation using observation angles requires multiple observation angles. If n observation angles are selected for pose estimation, then the number of combinations of observation angles is: The observation model for the m-th observation angle is written in the following form: z m (k)=H m (x(k),Ω m (k))+w v (k) (26) In the formula, Ω m (k)=[Ψ1(k);Ψ2(k);…;Ψ6(k)] is the state vector of 6 observation angles, and the output function H m The sensor output is correlated with the state vector when observing the m-th set of observation angles, w v (k) Gaussian white noise; Given a set of measurements Z(k+1)={z(k+1),z(k),…,z(1)}, the posterior probability density function PDF of x(k+1) can be expressed by Bayes' theorem as: The EKF system provides system status information via the lunar-based equipment navigation system. The estimation of the state P and the estimation of its covariance; The covariance of the estimated state provides an estimate Confidence level estimation; the form of the system's estimation error covariance P is: In the formula, submatrix P vv P vi and P ii These are the covariances from lunar base equipment to lunar base equipment, from lunar base equipment to observation angle, and from observation angle to observation angle; Used to distinguish uncertainties associated with lunar-based equipment and each individual observation angle; The criterion used to optimize sensor measurements in a system described by a continuous probability distribution is based on minimizing the expected value of the entropy change of the lunar-based equipment navigation system itself. The IE of a continuous signal source is: H(x)=-∫p(x)lnp(x)dx (29) The impact of a set of sensor data on the confidence level of the lunar base equipment location is determined according to equation (29): Equation (30) is the entropy of the posterior distribution before the addition of measurement data; the probability distribution of the random variable x is a Gaussian distribution, let m i For variable x i The mean of the two centers is then μ. ij =E[(x i -m i (x) j -m j Then equation (30) transforms into: For a Gaussian distributed vector, calculating its entropy requires only the dimension T and covariance P; the prior information metric is defined as: At time step k+1, the system is updated using only one set of observation angles to obtain the covariance P. k+1|k+1,m At the same time step, update the system using only one set of observation angles for all combinations of observation angles to obtain the covariance P. k+1|k+1,m The operation; according to equation (31), the possible contribution of each set of observation angles to the positioning is obtained: The information difference for each group of observed angles in the map will be calculated at time step k+1, as shown below: Δi=im k+1|k+1,m -and k+1|k (34) According to the evaluation index Δi described in equation (34), the combination of observation angles with Δi greater than the preset threshold is selected. That is, according to equations (32)-(34), each group of observation angles is evaluated by the information entropy IE algorithm to select the optimal combination of observation angles.

Images