Optimal planning method for optical navigation features of autonomous operations on extraterrestrial surfaces

Abstract

The present invention discloses an optical navigation feature planning optimization method for autonomous operation on the surface of an extraterrestrial body, which belongs to the field of autonomous navigation. The implementation method of the present invention is as follows: at a camera sampling time interval, according to the planned detector trajectory and attitude, combined with the camera parameters, the visible field of view range of the optical camera at the corresponding moment is calculated; considering the uncertainty of the detector position and attitude, combined with the camera field of view range formula, the uncertainty area of ​​the field of view range is obtained, and the intersection area of ​​the uncertainty area is taken to obtain the estimated field of view range for navigation feature selection at the current moment; before the start of the detection task, the Cramér-Rao lower bound is used as the performance indicator to optimize the selection of optical navigation features within the sequence estimated field of view range, and obtain optical navigation features that meet the requirements of autonomous operation on the surface of an extraterrestrial body. According to the present invention, the preferred navigation feature points are planned for navigation, the influence of the uncertainty of the detector position and attitude on the navigation performance is eliminated, and the robustness and efficiency of autonomous operation navigation are improved.

Description

Technical Field

[0001] The invention relates to an optical navigation feature planning optimization method for autonomous operation on the surface of an extraterrestrial body, and is particularly suitable for an extraterrestrial body detector navigation system that uses optical navigation features as observation information, and belongs to the field of autonomous navigation. Background Art

[0002] Obtaining accurate position information of the probe is an important prerequisite for conducting surface exploration of extraterrestrial bodies. Since extraterrestrial bodies are usually far away from the earth, there is a time delay in the communication between the probe and the ground, which poses new challenges to the autonomous navigation technology of the probe. Real-time navigation methods for probes, such as inertial navigation, have been proposed one after another. Due to the constant deviation and drift of the inertial measurement unit, the navigation accuracy decreases over time, and a single inertial navigation system is difficult to meet the accuracy requirements. At the same time, with the development of optical sensors such as navigation cameras, autonomous navigation methods of probes based on optical information have become a hot research direction in the future. There are a large number of natural topographic features such as craters on the surface of extraterrestrial bodies. The autonomous navigation method that uses optical cameras to obtain the above features as navigation landmarks has a higher navigation performance.

[0003] Autonomous optical navigation methods for detectors based on optical navigation features have become a hot topic of research, in which the position and number of navigation landmarks are key factors affecting navigation accuracy. Due to the limited computing resources of the detector, it may not be possible to extract and match navigation features in real time. In the actual detection process, there is uncertainty in the position and posture of the detector, and the camera field of view will also change accordingly. The originally visible navigation features will be missing, resulting in inaccurate pre-selection of optical navigation landmarks before the detection mission begins. How to pre-select appropriate landmarks when the field of view changes with the position and posture of the detector in order to achieve the desired performance requirements is a key technology for navigation based on optical navigation features. Summary of the invention

[0004] The purpose of the present invention is to provide an optical navigation feature planning optimization method for autonomous operation on the surface of an extraterrestrial body. The method calculates the visible field of view range of an optical camera at a corresponding moment according to the planned detector trajectory and posture in combination with camera parameters at a given camera sampling time interval; considers the uncertainty of the detector position and posture, combines it with the camera field of view range formula, obtains the field of view range uncertainty area, takes the intersection area of ​​the uncertainty area, and obtains the estimated field of view range for navigation feature selection at the current moment; before the start of the detection mission, takes the Cramér-Rao lower bound as a performance indicator, optimizes and selects the optical navigation features within the sequence estimated field of view range, obtains the optical navigation features that meet the requirements of autonomous operation on the surface of an extraterrestrial body, and realizes the optimization of optical navigation feature planning.

[0005] The purpose of the present invention is achieved through the following technical solutions.

[0006] The present invention discloses an optical navigation feature planning optimization method for autonomous operation on the surface of an extraterrestrial body, comprising the following steps:

[0007] Step 1: Establish an optical navigation observation model to plan the detector trajectory and attitude.

[0008] Define the world coordinate system O w X w Y w Z w :To simplify the calculation, the origin O w Defined as the detector position at the initial moment, O w X w Axis is the direction of motion of the detector at the initial moment, O w Z w Perpendicular to the lunar surface operation plane, O w Y w The axis is determined by the right-hand rule. Define the detector body coordinate system O b X b Y b Z b : The origin is located at the center of mass of the detector, O b X b Parallel to the chassis plane and pointing forward, O b Z b Parallel to the chassis plane and pointing upward, O b Y b The axis is determined by the right-hand rule. Define the camera coordinate system O c X c Y c Z c :The focal center of the pinhole camera model, i.e. the optical center, is taken as the origin, and the optical axis of the camera is taken as O c Z c The camera image coordinate system Ouv is defined as follows: the origin is the intersection of the camera optical axis and the imaging plane, and the u-axis and v-axis are parallel to the camera coordinate system Ouv. c X c Axis and O c Y c axis.

[0009] Without considering the rotation of the celestial body, the dynamic equation of the detector in the world coordinate system is simplified as follows:

[0010]

[0011] in, w r and w v is the position and velocity of the probe in the world system, a is the resultant acceleration of all forces acting on the probe, is the attitude quaternion from the world coordinate system to the detector body coordinate system, abbreviated as q,b ω represents the angular velocity of the detector relative to the world coordinate system in the detector body coordinate system. For any angular velocity ω, Ω(ω) represents the antisymmetric matrix, which is defined as:

[0012]

[0013] In order to achieve accurate navigation of the probe, the navigation system needs to determine the position, velocity and attitude information of the probe in the world coordinate system. The state equation of the navigation system is defined as:

[0014] x=[ w r T w v T q T ] T (3)

[0015] According to formula (1), the navigation system state equation is as follows:

[0016]

[0017] Among them, R w2b represents the transformation matrix from the world coordinate system to the detector body coordinate system, a c represents the control acceleration in the coordinate system of the detector body, g represents the gravitational acceleration in the world system, and n represents the navigation system noise, whose components are independent Gaussian white noise.

[0018] The navigation landmarks are used as observation feature points. The navigation camera model adopts the pinhole imaging model. Any navigation landmark point P on the surface of the celestial body i The measurement of feature points detected in any camera image is:

[0019]

[0020] Among them, (u i ,v i ) represents the feature point P i The pixel coordinates in the image coordinate system, f represents the focal length of the camera, C x i , C y i , C z i Represents navigation landmark point P i The three-axis components of the position vector in the camera coordinate system satisfy:

[0021]

[0022] in, w P i Represents navigation landmark point P iThe position vector in the world coordinate system, c P i is an intermediate variable.

[0023] The observation equation for the observed feature points is:

[0024] U i =h u (x)+v u (7)

[0025] Among them, v u Represents the measurement error of the feature point, which is uncorrelated Gaussian white noise, h u (x) is the measurement quantity of the observed feature point.

[0026] Using the Rodrigues parameter ξ=θu=[ξ 1 ξ 2 ξ 3 ] T represents the posture of the detector, where the unit vector u=[u 1 u 2 u 3 ] T represents the axis of rotation between coordinate systems, θ is the rotation angle with the same rotation direction as u, then the relationship between the quaternion and the Rodriguez parameter is:

[0027]

[0028] According to the Rodriguez formula, the rotation matrix from the world coordinate system to the detector body coordinate system represented by the Rodriguez parameter is:

[0029] R w2b =exp(ξ × )=cosθI 3 +(1-cosθ)uu T +sinθu × (9)

[0030] Among them, I 3 is a 3×3 identity matrix.

[0031] The motion trajectory of the detector is obtained according to the task and obstacle avoidance requirements. The camera optical axis points to the direction of the detector's movement, so the rotation matrix from the world coordinate system to the camera coordinate system is:

[0032] R w2c =R w2b R b2c (10)

[0033] According to the motion trajectory of the detector, the detector planning posture represented by the rotation matrix is ​​obtained

[0034] Step 2: Based on the planned trajectory and posture, combined with the camera parameters, the field of view range formula is established according to the geometric relationship between the imaging plane and the field of view range projection, and the visible field of view range of the optical camera at each sampling moment is calculated.

[0035] The sampling time series of the optical camera is t 0 ,t 1 ,...,t n , the expression is shown in formula (11).

[0036] t j+1 -t j =Δt(j=1,2,...,n-1) (11)

[0037] Wherein, Δt represents the sampling time interval.

[0038] When the imaging plane is a square, its side length is expressed in the image coordinate system as:

[0039] d=2f tan(fov / 2) (12)

[0040] Where f is the focal length of the camera, and fov is the field of view of the camera. p1 ,P p2 ,P p3 ,P p4 In the image coordinate system, they are:

[0041]

[0042] The camera frustum is obtained based on the camera field of view angle, the camera optical axis direction and the camera posture. The unit direction of the camera frustum edge i in the world coordinate system is:

[0043]

[0044] Among them, e io is the initial unit orientation of the camera's view frustum edge i in the world coordinate system.

[0045] The quadrilateral range where the camera's view cone intersects the working plane is the camera's field of view. The Z-axis coordinate of the camera's field of view vertex i in the camera coordinate system is:

[0046]

[0047] in, w z c Represents the Z-axis coordinate of the optical camera in the world coordinate system; α i represents the angle between the cone edge i and the optical axis, β iRepresents the angle between the view cone edge i and the downward vector of the vertical working plane, satisfying:

[0048]

[0049] Among them, e o is the unit direction of the camera optical axis, is the normal vector pointing upward on the working plane.

[0050] From equations (5), (6) and (15), we can get: j The camera field of view vertex P at the moment i Position coordinates in the world coordinate system:

[0051]

[0052] in,[ w x i w y i ] T The camera's field of view vertex P i Position coordinates on the working plane, P pi =[u i v i ] T is the vertex P of the imaging plane pi Coordinates in the image plane coordinate system.

[0053] From the field of view formula (17), we can get t j The coordinates of the four vertices of the camera field of view on the working plane at the moment. 1 , P 2 , P 3 , P 4 They are the camera field of view vertices at the upper left corner, upper right corner, lower right corner, and lower left corner. Connect the four vertices P 1 , P 2 , P 3 , P 4 That is, we get t j The field of view of the camera at this moment is S j .

[0054] Step 3: Determine the noise value range based on the disturbances suffered by the detector in the actual mission, that is, consider the uncertainty of the detector position and attitude, and combine it with the field of view range formula in step 2 to obtain the field of view range intersection area as the estimated field of view range for navigation feature selection at the current moment.

[0055] Introducing position uncertainty in the detector's three-axis position:

[0056]

[0057] Among them, εl It represents the three-axis position uncertainty of the detector, and its components are mutually uncorrelated Gaussian white noises determined according to the disturbances suffered by the detector in the actual mission.

[0058] Introducing the detector attitude uncertainty:

[0059]

[0060] Among them, ε a Represents the uncertainty of the detector's attitude, and its components are mutually uncorrelated Gaussian white noises determined according to the disturbances experienced by the detector in the actual mission.

[0061] The rotation matrix from the world coordinate system to the camera coordinate system with uncertainty is expressed as:

[0062]

[0063] in:

[0064]

[0065] t j Substituting a series of uncertain detection positions and attitudes at each moment into equation (17), it is simplified to:

[0066]

[0067] in, Represents the edge vertex P of the field of view i The uncertainty of the Z-axis coordinate in the camera coordinate system.

[0068] From formula (22), we can get the extreme value of the uncertainty of the x and y coordinates of each vertex, denoted by P i xmax , P i ymax , P i xmin , P i ymin They are the camera field of view vertices P after considering uncertainty. i The x- and y-coordinates of the extreme points.

[0069] straight line Intersection Point That is t j The vertex of the intersection area of ​​the field of view at the moment. Connection point Get j Estimated field of view for navigation feature optimization at this moment

[0070] Step 4: Before the detection task begins, use the Cramér-Rao lower bound as the performance indicator to estimate the field of view at different sampling times The optical navigation features in the are planned and optimized to obtain the optical navigation features that meet the requirements of autonomous operations on the surface of extraterrestrial bodies, that is, to achieve the optimization of optical navigation feature planning.

[0071] The Fisher information matrix F of the navigation feature point observations about the detector position is shown in formula (23):

[0072]

[0073] Among them, R i satisfy σ i It represents the standard deviation of observation noise. The standard deviation of observation noise of each feature point is consistent. n is the number of observation landmark points.

[0074] When the number of observed landmarks is n, different landmarks at the same time C z j The coordinate components in the direction are approximately equal, that is, C z 1 ≈…≈ C z n = C z, then the Fisher information matrix of the detector position is:

[0075]

[0076] According to the Cramér-Rao inequality and the Fisher information matrix, the common lower bound of the variance of the estimated state estimator is determined, and the estimated error variance matrix P * The trace of is as follows (25):

[0077]

[0078] In the formula, λ i represents the eigenvalue of the Fisher information matrix.

[0079] Combined with the properties of the rotation matrix, the lower limit of the detector position error in the case of n feature points is obtained as:

[0080]

[0081] The above formula is used to optimize the selection of feature points in the image. The feature points in the estimated field of view of the sequence obtained in step 3 are optimized and selected. The performance index is as shown in formula (27). n feature points are selected as observation features:

[0082]

[0083] The observation characteristics shown in formula (27) are the optical navigation characteristics that meet the requirements for autonomous operation on the surface of extraterrestrial bodies, thereby achieving the optimization of optical navigation feature planning.

[0084] Step 5: Use the navigation feature points selected in step 4 to perform navigation, eliminate the impact of the detector's position and attitude uncertainty on navigation performance, improve the detector's autonomous positioning accuracy, and improve the robustness and efficiency of the autonomous operation navigation algorithm on the surface of extraterrestrial bodies.

[0085] Beneficial effects:

[0086] 1. The present invention discloses an optical navigation feature planning optimization method for autonomous operation on the surface of an extraterrestrial body. The trajectory and attitude of the detector are used in combination with the camera parameters to calculate the visible field of view of the optical camera at each sampling moment. Combined with the position and attitude uncertainty of the detector during the detection process, the estimated field of view for pre-selection of navigation features at the current moment is obtained, eliminating the influence of the detector posture error on the pre-selection of optical navigation features, thereby improving the reliability of the navigation algorithm.

[0087] 2. The present invention discloses a method for optimizing planning of optical navigation features for autonomous operations on the surface of an extraterrestrial body. Before the start of a detection mission, the Cramér-Rao lower bound is used as a performance indicator to achieve optimal selection of optical navigation features within the estimated field of view of the sequence, thereby avoiding real-time image processing during the detection process, reducing the amount of onboard computing of the detector, and meeting the needs of autonomous operations on the surface of an extraterrestrial body. BRIEF DESCRIPTION OF THE DRAWINGS

[0088] Figure 1 A flowchart of the method for optimizing the planning of optical navigation features for autonomous operation on the surface of an extraterrestrial body disclosed in the present invention;

[0089] Figure 2 In the specific implementation mode of the present invention, A is used * Trajectory diagram obtained by algorithm planning;

[0090] Figure 3 A schematic diagram of the imaging plane and field of view of an optical camera in a specific embodiment of the present invention;

[0091] Figure 4 A schematic diagram of the geometric relationship between the camera cone and the field of view in a specific embodiment of the present invention;

[0092] Figure 5 It is a schematic diagram of the intersection area of ​​the field of view considering uncertainty in a specific implementation manner of the present invention;

[0093] Figure 6 It is the sequence field range planned and the sequence characteristics obtained by optimization in the specific implementation mode of the present invention. DETAILED DESCRIPTION

[0094] In order to better illustrate the purpose and advantages of the present invention, the present invention is further described below in conjunction with the accompanying drawings and specific embodiments.

[0095] To verify the feasibility of the present invention, this embodiment takes lunar surface exploration as an example.

[0096] like Figure 1 As shown, the present embodiment discloses a method for optimizing optical navigation feature planning for autonomous operation on the surface of an extraterrestrial body, and the specific implementation steps are as follows:

[0097] Step 1: Establish an optical navigation observation model to plan the probe trajectory and attitude.

[0098] Define the world coordinate system O w X w Y w Z w To simplify the calculation, the origin O w Defined as the detector position at the initial moment, O w X w Axis is the direction of motion of the detector at the initial moment, O w Z w Perpendicular to the lunar surface tangent plane, O w Y w The axis is determined by the right-hand rule. Define the detector body coordinate system O b X b Y b Z b : The origin is located at the center of mass of the detector, O b X b Parallel to the chassis plane and pointing forward, O b Z b Parallel to the chassis plane and pointing upward, O b Y b The axis is determined by the right-hand rule. Define the camera coordinate system O c X c Y c Z c :The focal center of the pinhole camera model, i.e. the optical center, is taken as the origin, and the optical axis of the camera is taken as O c Z c The camera image coordinate system Ouv is defined as follows: the origin is the intersection of the camera optical axis and the imaging plane, and the u-axis and v-axis are parallel to the camera coordinate system Ouv. c X c Axis and O c Y c axis.

[0099] If the rotation of the celestial body is not considered, the dynamic equation of the probe in the world coordinate system is simplified as follows:

[0100]

[0101] in, w r and w v is the position and velocity of the probe in the world system, a is the resultant acceleration of all forces acting on the probe, is the attitude quaternion from the world coordinate system to the detector body coordinate system, abbreviated as q, b ω represents the angular velocity of the detector relative to the world coordinate system in the detector body coordinate system. For any angular velocity ω, Ω(ω) represents the antisymmetric matrix, which is defined as:

[0102]

[0103] In order to achieve accurate navigation of the probe, the navigation system needs to determine the position, velocity and attitude information of the probe in the world coordinate system. The state equation of the navigation system is defined as:

[0104] x=[ w r T w v T q T ] T (30)

[0105] According to formula (1), the navigation system state equation is as follows:

[0106]

[0107] Among them, R w2b represents the transformation matrix from the world coordinate system to the detector body coordinate system, a c represents the control acceleration in the coordinate system of the detector body, g represents the gravitational acceleration in the world system, and n represents the navigation system noise, whose components are independent Gaussian white noise.

[0108] The navigation landmarks are used as observation feature points. The navigation camera model adopts the pinhole imaging model. Any navigation landmark point P on the surface of the celestial body i The measurement of feature points detected in any camera image is:

[0109]

[0110] Among them, (u i ,v i ) represents the feature point P i The pixel coordinates in the image coordinate system, f represents the focal length of the camera, C x i , C y i , C z i Represents navigation landmark point P iThe three-axis components of the position vector in the camera coordinate system satisfy

[0111]

[0112] in, w P i Represents navigation landmark point P i The position vector in the world coordinate system, c P i is an intermediate variable.

[0113] The observation equation for observing feature points is:

[0114] U i =h u (x)+v u (34)

[0115] Among them, v u Represents the measurement error of the feature point, which is uncorrelated Gaussian white noise, h u (x) is the measurement quantity of the observed feature point.

[0116] Using Alpha * The algorithm plans the path of the detector. The evaluation function is designed as:

[0117] F(n)=G(n)+H(n) (35)

[0118] Among them, F(n) is the heuristic function of the current node n, that is, the cost required to reach the target point from the starting point through node n. G(n) is the energy consumed from the starting point to the current node n, and H(n) is the energy consumed from the current node n to the destination through the best route. s =[2 5 0] T , end point P e =[40 26 0] T , Α * The trajectory obtained by algorithm planning is as follows Figure 2 shown.

[0119] Using the Rodrigues parameter ξ=θu=[ξ 1 ξ 2 ξ 3 ] T represents the posture of the detector, where the unit vector u=[u 1 u 2 u 3 ] T represents the axis of rotation between coordinate systems, θ is the rotation angle with the same rotation direction as u, then the relationship between the quaternion and the Rodriguez parameter is:

[0120]

[0121] According to the Rodriguez formula, the rotation matrix from the world coordinate system to the detector body coordinate system represented by the Rodriguez parameter is:

[0122] R w2b =exp(ξ × )=cosθI 3 +(1-cosθ)uu T +sinθu × (37)

[0123] Among them, I 3 is a 3×3 identity matrix.

[0124] The motion trajectory of the detector is obtained according to the task and obstacle avoidance requirements. The camera optical axis points to the direction of the detector's movement, so the rotation matrix from the world coordinate system to the camera coordinate system is:

[0125] R w2c =R w2b R b2c (38)

[0126] According to the motion trajectory of the detector, the detector planning posture represented by the rotation matrix is ​​obtained

[0127] Step 2: Based on the planned trajectory and posture, combined with the camera parameters, the field of view range formula is established according to the geometric relationship between the imaging plane and the field of view range projection, and the visible field of view range of the optical camera at each sampling moment is calculated.

[0128] The relationship between the imaging plane and the field of view of an optical camera is as follows: Figure 3 First, the position and posture of the detector at each moment in the moving process are determined through the trajectory and posture obtained in step 1, and the field of view of the camera is determined.

[0129] The sampling time series of the optical camera is t 0 ,t 1 ,...,t n , the expression is shown in formula (39).

[0130] t j+1 -t j =Δt(j=1,2,...,n-1) (39)

[0131] Wherein, Δt represents the sampling time interval.

[0132] The imaging plane is a square, and its side length is expressed in the image coordinate system as:

[0133] d=2f tan(fov / 2) (40)

[0134] Where f is the focal length of the camera, and fov is the field of view of the camera. p1 ,P p2 ,P p3 ,P p4 In the image coordinate system, they are:

[0135]

[0136] The camera frustum is obtained based on the camera field of view angle, the camera optical axis direction and the camera posture. The unit direction of the camera frustum edge i in the world coordinate system is:

[0137] e i =(R w2c ) T e i0 (42)

[0138] Among them, e io is the initial unit orientation of the camera's view frustum edge i in the world coordinate system.

[0139] The quadrilateral range where the camera's view cone intersects the working plane is the camera's field of view. The Z-axis coordinate of the camera's field of view vertex i in the camera coordinate system is:

[0140]

[0141] in, w z c Represents the Z-axis coordinate of the optical camera in the world coordinate system; α i represents the angle between the cone edge i and the optical axis, β i Represents the angle between the view cone edge i and the downward vector of the vertical working plane, satisfying:

[0142]

[0143] Among them, e o is the unit direction of the camera optical axis, is the normal vector pointing upward on the working plane.

[0144] From equations (5), (6) and (15), we can get: j The camera field of view vertex P at the moment i Position coordinates in the world coordinate system:

[0145]

[0146] in,[ w x i w y i ] T The camera's field of view vertex Pi Position coordinates on the working plane, P pi =[u i v i ] T is the vertex P of the imaging plane pi Coordinates in the image plane coordinate system.

[0147] From the field of view formula (17), we can get t j The coordinates of the four vertices of the camera field of view on the landing plane at the moment. 1 , P 2 , P 3 , P 4 They are the camera field of view vertices at the upper left corner, upper right corner, lower right corner, and lower left corner. Connect the four vertices P 1 , P 2 , P 3 , P 4 Get j The field of view of the camera at this moment is S j The camera parameters are shown in Table 2.

[0148] Table 2 Navigation camera parameters

[0149]

[0150] The geometric relationship between the camera frustum and the field of view is as follows: Figure 4 shown.

[0151] Step 3: Determine the noise value range based on the disturbances suffered by the detector in the actual mission, that is, consider the uncertainty of the detector position and attitude, and combine it with the field of view range formula in step 2 to obtain the field of view range intersection area as the estimated field of view range for navigation feature selection at the current moment.

[0152] Introducing position uncertainty in the detector's three-axis position:

[0153]

[0154] Among them, ε l represents the three-axis position uncertainty of the detector,

[0155] Introducing attitude uncertainty:

[0156]

[0157] Among them, ε a represents the uncertainty of the detector attitude, and takes

[0158] The rotation matrix from the world coordinate system to the camera coordinate system with uncertainty is expressed as:

[0159]

[0160] in:

[0161]

[0162] t j Substituting a series of uncertain detection positions and attitudes at each moment into equation (17), it is simplified to:

[0163]

[0164] in, Represents the edge vertex P of the field of view i The uncertainty of the Z-axis coordinate in the camera coordinate system.

[0165] From formula (22), we can get the extreme value of the uncertainty of the x and y coordinates of each vertex, denoted by P i xmax , P i ymax , P i xmin , P i ymin They are the camera field of view vertices P after considering uncertainty. i The x- and y-coordinates of the extreme points.

[0166] straight line Intersection Point That is t j The vertex of the intersection area of ​​the field of view at the moment. Connection point Get j Estimated field of view for navigation feature optimization at this moment

[0167] According to the intersection area of ​​the field of view considering uncertainty obtained in step 3, Figure 5 shown.

[0168] Step 4: Before the detection task begins, use the Cramér-Rao lower bound as the performance indicator to estimate the field of view at different sampling times The optical navigation features in the are planned and optimized to obtain the optical navigation features that meet the requirements of autonomous operations on the surface of extraterrestrial bodies, that is, to achieve the optimization of optical navigation feature planning.

[0169] The Fisher information matrix F of the navigation feature point observations about the detector position is shown in formula (23):

[0170]

[0171] Among them, R i satisfy σ i It represents the standard deviation of observation noise. The standard deviation of observation noise of each feature point is consistent. n is the number of observation landmark points.

[0172] When the number of observed landmarks is n, assuming that different landmarks are at the same time C z j The coordinate components in the direction are approximately equal, that is, C z 1 ≈…≈ C z n = C z, then the Fisher information matrix of the detector position is:

[0173]

[0174] According to the Cramér-Rao inequality and the Fisher information matrix, the common lower bound of the variance of the estimated state estimator is determined, and the estimated error variance matrix P * The trace of is as follows (25):

[0175]

[0176] In the formula, λ i represents the eigenvalue of the Fisher information matrix.

[0177] Combined with the properties of the rotation matrix, the lower limit of the detector position error in the case of n feature points is obtained as:

[0178]

[0179] The above formula is used to optimize the selection of feature points in the image. The feature points in the estimated field of view of the sequence obtained in step 3 are optimized and selected, and n feature points are selected as the performance index of the observed features as shown in formula (27):

[0180]

[0181] Taking the selection of three feature points as an example, the intersection area of ​​the sequence field of view obtained by step 3 planning and the sequence features obtained by step 4 optimization are as follows: Figure 6 shown.

[0182] Step 5: Use the navigation feature points selected in step 4 to perform navigation, eliminate the impact of the detector's position and attitude uncertainty on navigation performance, improve the detector's autonomous positioning accuracy, and improve the robustness and efficiency of the autonomous operation navigation algorithm on the surface of extraterrestrial bodies.

[0183] At this point, the planning and optimization of the optical navigation features required in the navigation system of autonomous probes on the surface of extraterrestrial bodies has been completed.

[0184] The specific description above further illustrates the purpose, technical solutions and beneficial effects of the invention in detail. It should be understood that the above 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 in the scope of protection of the present invention.

Claims

1. A method for optimizing optical navigation feature planning for autonomous operations on the surface of an extraterrestrial body, characterized in that: The following steps are included: Step 1: Establish an optical navigation observation model to plan the trajectory and attitude of the detector; The implementation method of step one is: Define the world coordinate system O w X w Y w Z w :To simplify the calculation, the origin O w Defined as the detector position at the initial moment, O w X w Axis is the direction of motion of the detector at the initial moment, O w Z w Perpendicular to the lunar surface operation plane, O w Y w The axis is determined by the right-hand rule; define the detector body coordinate system O b X b Y b Z b : The origin is located at the center of mass of the detector, O b X b Parallel to the chassis plane and pointing forward, O b Z b Parallel to the chassis plane and pointing upward, O b Y b The axis is determined by the right-hand rule; define the camera coordinate system O c X c Y c Z c :The focal center of the pinhole camera model, i.e. the optical center, is taken as the origin, and the optical axis of the camera is taken as O c Z c The three-dimensional rectangular coordinate system established by the axis; define the camera image coordinate system Ouv: the origin is the intersection of the camera optical axis and the imaging plane, and the u axis and the v axis are parallel to the camera coordinate system O c X c Axis and O c Y c axis; Without considering the rotation of the celestial body, the dynamic equation of the detector in the world coordinate system is simplified as follows: in, w r and w v is the position and velocity of the probe in the world system, a is the resultant acceleration of all forces acting on the probe, is the attitude quaternion from the world coordinate system to the detector body coordinate system, abbreviated as q, b ω represents the angular velocity of the detector relative to the world coordinate system in the detector body coordinate system. For any angular velocity ω, Ω(ω) represents the antisymmetric matrix, which is defined as: In order to achieve accurate navigation of the probe, the navigation system needs to determine the position, velocity and attitude information of the probe in the world coordinate system; the state equation of the navigation system is defined as: x=[ w r Tw v T q T ] T (3) According to formula (1), the state equation of the navigation system is as follows: Among them, R w2b represents the transformation matrix from the world coordinate system to the detector body coordinate system, a c represents the control acceleration in the detector body coordinate system, g represents the gravitational acceleration in the world system, and n represents the navigation system noise, whose components are independent Gaussian white noise; The navigation landmarks are used as observation feature points; the navigation camera model adopts the pinhole imaging model, and any navigation landmark point P on the surface of the celestial body i The measurement of feature points detected in any camera image is: Among them, (u i ,v i ) represents the feature point P i The pixel coordinates in the image coordinate system, f represents the focal length of the camera, C x i , C y i , C z i Represents navigation landmark point P i The three-axis components of the position vector in the camera coordinate system satisfy: in, w P i Represents navigation landmark point P i The position vector in the world coordinate system, c P i is an intermediate variable; The observation equation for the observed feature points is: U i =h u (x)+v u (7) Among them, v u Represents the measurement error of the feature point, which is uncorrelated Gaussian white noise, h u (x) is the measurement value of the observed feature point; With Rodrigues parameter ξ=θu=[ξ1ξ2ξ3] T Represents the posture of the detector, where the unit vector u=[u1 u2 u3] T represents the axis of rotation between coordinate systems, θ is the rotation angle with the same rotation direction as u, then the relationship between the quaternion and the Rodriguez parameter is: According to the Rodriguez formula, the rotation matrix from the world coordinate system to the detector body coordinate system represented by the Rodriguez parameter is: R w2b =exp(ξ × )=cosθI3+(1-cosθ)uu T +sinθu × (9) Where I3 is a 3×3 identity matrix; The motion trajectory of the detector is obtained according to the task and obstacle avoidance requirements. The camera optical axis points to the direction of the detector's movement, so the rotation matrix from the world coordinate system to the camera coordinate system is: R w2c =R w2b R b2c (10) According to the motion trajectory of the detector, the detector planning posture represented by the rotation matrix is ​​obtained Step 2: Based on the planned trajectory and posture, combined with the camera parameters, the field of view range formula is established according to the geometric relationship between the imaging plane and the field of view range projection, and the visible field of view range of the optical camera at each sampling moment is calculated; The implementation method of step 2 is: The sampling time sequence of the optical camera is t0, t1, ..., t n , the expression is shown in formula (11); t j+1 -t j =Δt (j=1,2,...,n-1) (11) Wherein, Δt represents the sampling time interval; When the imaging plane is a square, its side length is expressed in the image coordinate system as: d=2ftan(fov / 2) (12) Among them, f represents the focal length of the camera, fov represents the field of view of the camera; the four vertices P of the imaging plane p1 ,P p2 ,P p3 ,P p4 In the image coordinate system, they are: The camera cone is obtained according to the camera field of view angle, the direction of the camera optical axis and the camera posture; the unit direction of the camera cone edge i in the world coordinate system is: And i =(R w2c ) T And i0 (14) Among them, e io is the initial unit orientation of the camera's view frustum i in the world coordinate system; The quadrilateral range where the camera's view cone intersects the working plane is the camera's field of view; the Z-axis coordinate of the camera's field of view vertex i in the camera coordinate system is: in, w z c Represents the Z-axis coordinate of the optical camera in the world coordinate system; α i represents the angle between the cone edge i and the optical axis, β i Represents the angle between the view cone edge i and the downward vector of the vertical working plane, satisfying: Among them, e o is the unit direction of the camera optical axis, is the normal vector pointing upwards on the working plane; From equations (5), (6) and (15), we can get: j The camera field of view vertex P at the moment i Position coordinates in the world coordinate system: in, The camera's field of view vertex P i Position coordinates on the working plane, P pi =[u i v i ] T is the vertex P of the imaging plane pi Coordinates in the image plane coordinate system; From the field of view formula (17), we can get t j The coordinates of the four vertices of the camera field of view on the working plane at the moment; P1, P2, P3, and P4 are the camera field of view vertices at the upper left corner, upper right corner, lower right corner, and lower left corner respectively; connect the four vertices P1, P2, P3, and P4 to get t j The field of view of the camera at this moment is S j ; Step 3: Determine the noise value range according to the disturbance suffered by the detector in the actual mission, that is, consider the uncertainty of the detector position and attitude, and combine it with the field of view range formula in step 2 to obtain the field of view range intersection area as the estimated field of view range for navigation feature selection at the current moment; Step 4: Before the detection task begins, use the Cramér-Rao lower bound as the performance indicator to estimate the field of view at different sampling times The optical navigation features in the are planned and optimized to obtain the optical navigation features that meet the requirements of autonomous operations on the surface of extraterrestrial bodies, that is, to achieve the optimization of optical navigation feature planning.

2. The method for optimizing optical navigation feature planning for autonomous operation on the surface of an extraterrestrial body as claimed in claim 1, characterized in that: The method also includes step five, which uses the preferred navigation feature points planned in step four to perform navigation, eliminate the influence of the uncertainty of the detector's position and attitude on the navigation performance, improve the detector's autonomous positioning accuracy, and improve the robustness and efficiency of the autonomous operation navigation algorithm on the surface of extraterrestrial bodies.

3. The method for optimizing optical navigation feature planning for autonomous operation on the surface of an extraterrestrial body as claimed in claim 1, characterized in that: The implementation method of step three is: Introducing position uncertainty in the detector's three-axis position: Among them, ε l It represents the uncertainty of the three-axis position of the detector, and its components are mutually uncorrelated Gaussian white noises determined according to the disturbances suffered by the detector in the actual mission; Introducing the detector attitude uncertainty: Among them, ε a represents the uncertainty of the detector's attitude, and its components are mutually uncorrelated Gaussian white noises determined according to the disturbances experienced by the detector in the actual mission; The rotation matrix from the world coordinate system to the camera coordinate system with uncertainty is expressed as: in: t j Substituting a series of uncertain detection positions and attitudes at each moment into equation (17), it is simplified to: in, Represents the edge vertex P of the field of view i The uncertainty of the Z-axis coordinate in the camera coordinate system; From formula (22), we can get the extreme value of the uncertainty of the x and y coordinates of each vertex, denoted by P i xmax , P i ymax , P i xmin , P i ymin They are the camera field of view vertices P after considering uncertainty. i The extreme points of x-coordinates and y-coordinates; straight line Intersection point P1 Λ , P3 Λ , That is t j The vertex of the intersection area of ​​the field of view at the moment; connection point P1 Λ , P3 Λ , Get j Estimated field of view for navigation feature optimization at this moment 4. The method for optimizing optical navigation feature planning for autonomous operation on the surface of an extraterrestrial body as claimed in claim 1, characterized in that: The implementation method of step 4 is: The Fisher information matrix F of the navigation feature point observations about the detector position is shown in formula (23): Among them, R i satisfy σ i It represents the standard deviation of the observed noise. The standard deviation of the observed noise of each feature point is consistent. n is the number of observed landmark points. When the number of observed landmarks is n, different landmarks at the same time C z j The coordinate components in the direction are approximately equal, that is, C z1≈…≈ C z n = C z, then the Fisher information matrix of the detector position is: According to the Cramér-Rao inequality and the Fisher information matrix, the common lower bound of the variance of the estimated state estimator is determined, and the estimated error variance matrix P * The trace of is as follows (25): In the formula, λ i represents the eigenvalue of the Fisher information matrix; Combined with the properties of the rotation matrix, the lower limit of the detector position error in the case of n feature points is obtained as: The above formula is used to optimize the selection of feature points in the image; the feature points in the estimated field of view of the sequence obtained in step 3 are optimized and selected. The performance index is as shown in formula (27). n feature points are selected as observation features: The observation characteristics shown in formula (27) are the optical navigation characteristics that meet the requirements for autonomous operation on the surface of extraterrestrial bodies, thereby achieving the optimization of optical navigation feature planning.

Images