Master-slave multi-month-based equipment autonomous operation cooperative navigation method

Abstract

The master-slave type multi-lunar base equipment autonomous operation cooperative navigation method disclosed by the application belongs to the technical field of deep space exploration. The method of the application is as follows: analyzing the navigation completeness required by the lunar surface autonomous cooperative operation, modeling the movement dynamics and navigation sensors of the lunar base equipment, designing the master-slave type lunar base equipment cooperative navigation method, taking the information obtained by the motion state interaction among the multiple lunar base equipments as the measurement value to reconstruct the state equation and the measurement equation of the multiple lunar base equipment system, introducing the information distribution factor to realize the calculation of the primary filter, and using the extended Kalman filter to obtain the more accurate relative position of the lunar base equipment and improve the positioning accuracy of the target. Through the information cooperation of the multiple lunar base equipments, the joint filtering model is built, the master-slave type multiple lunar base equipment cooperative navigation is realized, the accuracy and reliability of the cooperative navigation are improved, and the precision and robustness of the master-slave type multiple lunar base equipment autonomous operation cooperative navigation system are improved. The application is not affected by time and space factors.

Description

Technical Field

[0001] This invention relates to a master-slave multi-lunar base equipment autonomous operation and collaborative navigation method, belonging to the field of deep space exploration technology. Background Technology

[0002] Lunar exploration is a crucial component of deep space exploration, and establishing a base on the lunar surface is a major trend in lunar exploration. The lunar environment is complex and uncertain. The payload and data processing capabilities of navigation equipment on a single lunar-based instrument are limited, and individual lunar-based instruments suffer from weak environmental awareness and poor self-positioning accuracy, making it difficult to meet the requirements for high-precision and high-reliability navigation. Therefore, collaborative navigation among multiple lunar-based instruments can achieve higher navigation accuracy and improve exploration efficiency. Collaborative navigation utilizes shared navigation information to calculate and correct its own position, velocity, attitude, and other navigation information in real time, ensuring the smooth maintenance of coordinated formation, formation reconfiguration, and subsequent collaborative tasks, effectively meeting the requirements for long-duration, long-distance, and high-precision operations. Summary of the Invention

[0003] To address the problem that lunar bases cannot provide accurate and complete navigation and communication systems, and that the complex lunar environment causes significant interference to navigation, the master-slave multi-lunar-based equipment autonomous operation and cooperative navigation method disclosed in this invention aims to solve the following technical problems: lunar bases cannot provide accurate and complete navigation and communication systems, and the complex lunar environment causes significant interference to navigation. The master-slave multi-lunar-based equipment cooperative navigation scheme can improve the accuracy and reliability of lunar surface navigation. It also has the following advantages: (1) Navigation information fusion uses a joint filtering model, which can obtain a more accurate relative position of the lunar-based equipment. (2) Information is transmitted between lunar-based equipment via radio communication, unaffected by time and space factors.

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

[0005] This invention discloses a master-slave multi-lunar-based equipment autonomous operation and cooperative navigation method. Since the lunar surface lacks an atmosphere and is significantly affected by radiation from space and external objects such as meteorites, this invention analyzes the navigation completeness required for autonomous cooperative operations on the lunar surface. It models the motion dynamics of lunar-based equipment and navigation sensors, designs a master-slave lunar-based equipment cooperative navigation method, and reconstructs the state equations and measurement equations of the multi-lunar-based equipment system using information obtained from the interaction of motion states among the multiple lunar-based equipment as measurement values. An information allocation factor is introduced to calculate the primary filter, and an extended Kalman filter is used to obtain a more accurate relative position of the lunar-based equipment, improving the positioning accuracy of the target. Through information coordination among the multiple lunar-based equipment, a joint filtering model is built to realize master-slave multi-lunar-based equipment cooperative navigation, improving the accuracy and reliability of cooperative navigation, and thus enhancing the accuracy and robustness of the master-slave multi-lunar-based equipment autonomous operation and cooperative navigation system.

[0006] The master-slave multi-lunar base equipment autonomous operation and cooperative navigation method disclosed in this invention includes the following steps:

[0007] Step 1: Establish the coordinate systems used in the lunar-based equipment navigation process and the transformation relationships between the coordinate systems. The coordinate systems include the lunar-based equipment coordinate system, the navigation coordinate system, and the camera coordinate system.

[0008] Lunar base equipment coordinate system O c -X c Y c Z c Take the center of mass of the lunar base equipment as the origin O. c O c X c The axis is parallel to the bottom plane of the lunar base equipment and points forward, O c Z c The axis is perpendicular to the bottom plane of the lunar base equipment and points upwards, O c Y c The orientation of the axis and the first two coordinate axes satisfy the right-handed orthogonal coordinate system relationship.

[0009] Navigation coordinate system O d -X d Y d Z d The navigation coordinate system is defined as the initial lunar-based equipment coordinate system, with its basic direction pointing in the same direction as the lunar-based equipment coordinate system and stationary relative to the Moon. It is a Cartesian coordinate system in three-dimensional space, and the position coordinates of all three lunar-based equipment units are represented in the navigation coordinate system. The matrix A for transforming the navigation coordinate system to the lunar-based equipment coordinate system is expressed as follows:

[0010]

[0011] In equation (1), ψ, θ, and γ represent the navigation coordinate system's orbital distance around Y. d Z d X d The angle of rotation of the shaft.

[0012] Camera coordinate system O s -X s Y s Z s Assume the optical center of the left camera is fixedly connected to the lunar rover's center of mass O, the lens is perpendicular to the lunar base equipment and mounted in front, and the optical center of the camera is perpendicular to the coordinate origin O. s Coincidence, O s X s The axis is parallel to the plane of the lunar base equipment chassis and points to the right, O s Y s The axis is perpendicular to the plane of the lunar base equipment and points downwards, O s Z sThe axis coincides with the optical axis of the vision camera and points forward.

[0013] Step 2: Establish a dynamic model of lunar-based equipment movement. The movement of lunar-based equipment on the lunar surface can be considered as three-dimensional motion. During its journey, the equipment will encounter various terrain conditions, including craters, rocks, and other unique topography. By modeling the dynamics of lunar-based equipment movement, the process of state parameter changes during its movement on the lunar surface can be intuitively reflected.

[0014] The continuous change process of the lunar base equipment state is accurately described by the kinematic model of the lunar base equipment as shown in Equation (2). In order to perform subsequent digital signal processing, the continuous process needs to be discretized and a simplified kinematic model of the lunar base equipment is used to approximate the change process of the lunar base equipment state.

[0015]

[0016] In equation (2), v x v y v z These represent the velocities of the lunar-based equipment moving in different directions along the coordinate axes, a. x a y a z These represent the accelerations of the lunar-based equipment moving in different directions along the coordinate axes.

[0017] Lunar-based equipment can be considered as a rigid body. The rotational motion of a rigid body about any fixed point has only 3 degrees of freedom. Therefore, to describe the attitude motion of lunar-based equipment, 6 independent state variables are needed.

[0018]

[0019] In equation (3), w x w y w z These represent the angular velocities of the lunar-based equipment around different coordinate axes.

[0020]

[0021] The forces acting on the lunar-based equipment along the coordinate axes include thrust along the lateral axis, rolling around the longitudinal axis, and vertical oscillation along the vertical axis.

[0022] In the simplified physical model of the lunar environment, the forces acting on the wheels of the lunar-based equipment in each direction are shown in equation (5). The force relationships of the lunar-based equipment are as follows:

[0023]

[0024] In equation (5), The longitudinal forces F on the right and left front wheels are respectively. lfThe longitudinal resultant force of the two front wheels, The lateral forces F are the lateral forces on the right front wheel and the left front wheel, respectively. cf The resultant lateral force of the two front wheels, F represents the longitudinal forces on the right rear wheel and the left rear wheel, respectively. lr The longitudinal resultant force of the two rear wheels, F represents the lateral forces on the right rear wheel and the left rear wheel, respectively. cr Let M be the resultant lateral force of the two rear wheels, M be the torque acting on the lunar-based equipment, and I be the moment of inertia of the lunar-based equipment. The turning angle during the movement of lunar-based equipment.

[0025] Step 3: Through information collaboration among multiple lunar base equipment, master-slave collaborative navigation among multiple lunar base equipment is achieved, improving the accuracy and reliability of collaborative navigation among multiple lunar base equipment, and thus improving the accuracy and robustness of the master-slave collaborative navigation system for autonomous operation.

[0026] Lunar-based equipment is divided into primary lunar-based equipment and member lunar-based equipment. The primary lunar-based equipment carries sensors including strapdown inertial navigation systems (INS), laser rangefinders, and binocular vision cameras. Member lunar-based equipment carries sensors including strapdown INS and radio equipment. The primary lunar-based equipment uses the laser rangefinder to measure the distance to a pre-set beacon, and combines this with azimuth information provided by the binocular vision cameras to determine the positions of the two member lunar-based equipment relative to the primary equipment. The strapdown INS carried by each member lunar-based equipment obtains its own position information and transmits it to the primary lunar-based equipment via radio. The primary lunar-based equipment then performs filtering and fusion to obtain a more accurate position for the member lunar-based equipment.

[0027] The attitude of lunar-based equipment is primarily described using quaternions. Quaternions avoid singularities when representing attitude coordinates and are relatively simple to calculate. The transformation quaternion for converting the navigation frame n to the carrier coordinate system b is q. The attitude differential equation can be expressed as:

[0028]

[0029] In equation (6), To represent quaternion multiplication, It can be calculated using the following formula:

[0030]

[0031] In equation (7), For lunar-based equipment relative to the inertial frame O d -X d Y d Z d The projection of the corresponding angular velocity onto the system; This represents the angular velocity vector of the fixed frame of reference relative to the inertial frame due to the Moon's rotation. The navigation coordinate system O is affected by the change in the position of the lunar base equipment. c -X c Y c Z c The rotational angular velocity vector relative to the Moon's fixed connection, This is the attitude transformation matrix from the navigation system to the carrier system.

[0032] The velocity v of the lunar-based equipment in a coordinate system fixed to the moon. e for:

[0033]

[0034] velocity v e Transferring to the navigation system, we get:

[0035]

[0036] In equation (9), v n The speed is the speed under the navigation system.

[0037] Differentiating the velocity in the navigation system and simplifying, we get:

[0038]

[0039] According to Newton's second law of motion, the acceleration of an object is related to the external force acting on it. f i For the specific force in the inertial coordinate system, G i The gravitational acceleration of the moon (G) i ≈1.63m / s 2 According to the formula for calculating the gravity on the lunar surface. The differential equation for the velocity of lunar-based equipment can be derived as follows:

[0040]

[0041] In equation (11), f b For comparison under the condition of solid connection on the lunar surface, g n This refers to the lunar gravitational acceleration in the inertial navigation frame.

[0042] Step 4: The information obtained from each subsystem is fused using a joint filtering model. The sensor of the strapdown inertial navigation system is processed by discrete Kalman filtering, and the angle and distance information obtained from the binocular stereo camera and laser rangefinder are processed by extended Kalman filtering. Finally, the information is input into the master filter for time updating to obtain the optimal fusion of master-slave cooperative navigation.

[0043] The main filter fuses information from the accelerometer and gyroscope, using the positions of these two internal sensors as data to estimate the equipment's coordinates at that moment. The other two sub-filters estimate the target's position using cooperative sensor localization. They observe the target using the two external sensors and fuse the resulting information to correct autonomous localization errors. The target's localization information from the external vision sensor serves as the input to the sub-filters, and an extended Kalman filter is used to estimate the target's localization state for the vision sensors.

[0044] The information allocation factors of each sub-filter are calculated and allocated by the main filter. By using an extended Kalman filter, the position and attitude information obtained by the sub-filters are estimated and fused according to the allocation factor weights, making the target localization more accurate.

[0045] This invention uses discrete Kalman filtering to fuse data from accelerometers and gyroscopes, treating it as a linear system, and taking the state variable X = [a x a y a z w g ] T System state equation X k+1 =A k+1 X k+1 W k A k+1 W represents the state transition matrix from time k to k+1. k This is used to represent Gaussian white noise in a system with a variance matrix of Q. The transition matrix can be set as follows: β is a correction coefficient determined based on experimental debugging.

[0046] The observation equation is Z k =H k X k +V k The observation data can be obtained from accelerometers and gyroscopes, so the measurement matrix H k Let I be the identity matrix.

[0047] Based on the model of the laser rangefinder, by placing two beacons, the distance between the target and the sensor can be obtained, thereby obtaining the distance of the member lunar base equipment relative to the main lunar base equipment at the current moment.

[0048] Let the current position coordinates of the main lunar base equipment be (x... k ,y k ,z k The distance equation for a laser rangefinder is:

[0049] The lunar-based equipment measurement matrix is ​​as follows:

[0050]

[0051] In the formula,

[0052] According to equation (13), the overall observation equation can be obtained as Z. J =H J X J +V J .

[0053] The distance between the target and the vision sensor can be obtained through measurement. Let the coordinates of the main lunar base equipment during observation be (x...). k ,y k ,z k The distance observation equation for the main lunar-based equipment is derived as follows:

[0054]

[0055] In equation (14), For Gaussian white noise, derive the Jacobian measurement matrix:

[0056]

[0057] The inertial navigation system obtained by fusing the accelerometers and gyroscopes of each piece of equipment is used as the reference state. Visual positioning and laser ranging positioning are two independent sub-filtering systems of the main equipment. Extended Kalman filtering is used for state estimation. The above two sub-filters pass the estimation structure of each step to the main filter. The main filter completes the optimal fusion of information. The overall state estimate is obtained by taking the arithmetic mean of the optimal estimates of the states of the two subsystems.

[0058] The filtering system solves for the information allocation factors β1 and β2, and the information allocation coefficient β i It obeys the principle of information conservation, that is... β m These are the coefficients assigned to the main filter information. Where β1,…,β N The determination of the coefficients will affect the performance of the joint filter. Generally, larger coefficient values ​​are assigned to sensors with higher measurement accuracy. This invention uses coefficients based on the variance matrix P. i eigenvalue decomposition for β i The values ​​are calculated in real time. i Decomposed into P according to the eigenvalues ​​of the variance matrix i =L i ∧L T Λ i =diag{λ i1 ,λi2 ,...,λ iN}, we can deduce that:

[0059]

[0060] Extended Kalman filtering marginalizes all past states and obtains real-time performance by estimating the current state. The fusion filtering algorithm is specifically divided into the following steps:

[0061] Initialize global state estimator Covariance matrix P i0 And the navigation information is proportioned according to the information factor β obtained in equation (16). i Assigned to two sub-filters and the main filter:

[0062]

[0063]

[0064] Since cooperative navigation systems require spatiotemporal synchronization, time correction is performed simultaneously on each sub-filter and the main filter. Common noise information is adjusted according to the proportion β of the matched information factor. i Assigned to each sub-filter:

[0065]

[0066] After time correction of the filter, we get:

[0067]

[0068] P i (k+1|k)=A(k+1|k)P i (k)+Γ(k)Q i (k)Γ T (k) (20)

[0069] Where Γ is the noise matrix of the system, and A is the state transition matrix of the system.

[0070] Each sub-filter utilizes its own local observation Z. i Correct the observed values:

[0071]

[0072]

[0073] Among them, H i This is the measurement matrix.

[0074] Information fusion of the main filter:

[0075]

[0076]

[0077] The sub-filter is reset using the fusion result for time updates, effectively controlling positioning errors.

[0078] Beneficial effects:

[0079] 1. The master-slave multi-lunar base equipment autonomous operation collaborative navigation method disclosed in this invention uses the information obtained from the interaction of motion states among multiple lunar base equipment as measurement values ​​to reconstruct the state equation and measurement equation of the multi-lunar base equipment system, reducing the interference of environmental factors on the navigation mode of a single lunar base equipment, thereby providing reliable navigation information for the lunar base equipment navigation system.

[0080] 2. In the master-slave multi-monthly base equipment autonomous operation cooperative navigation method disclosed in this invention, the filter fused by the joint filtering model is not only from the positioning information of a certain equipment itself, but also from the observation information of other equipment relative to a certain equipment. An information allocation factor is introduced to realize the calculation of the primary filter. The cooperative positioning method among multiple equipment effectively controls the magnitude of the error and improves the accuracy. Attached Figure Description

[0081] Figure 1 This is a schematic diagram of the master-slave multi-lunar base equipment autonomous operation and collaborative navigation method of the present invention.

[0082] Figure 2 This is a schematic diagram of the coordinate system used in the multi-lunar base equipment cooperative navigation of the present invention.

[0083] Figure 3 This is a schematic diagram of the force model of the lunar base equipment of the present invention.

[0084] Figure 4 This is a schematic diagram of the forces acting on the tires of the lunar base equipment of the present invention.

[0085] Figure 5 This is a block diagram of the multi-lunar base equipment cooperative navigation scheme of the present invention.

[0086] Figure 6 This is a block diagram of the multi-lunar base equipment joint filtering model of the present invention.

[0087] Figure 7 The results are simulation analysis of the master-slave multi-lunar base equipment autonomous operation and collaborative navigation method. Figures (a), (b), and (c) show the position changes of the master lunar base equipment over time, while figures (d), (e), (f), (g), (h), and (i) show the position changes of the member lunar base equipment over time. Detailed Implementation

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

[0089] Example 1:

[0090] To verify the feasibility of the present invention, mathematical simulation was performed under the given parameter environment, as shown in Table 1.

[0091] Table 1. Parameter Settings for Master-Slave Multi-Lunar Base Equipment Navigation System

[0092]

[0093] The master-slave multi-lunar base equipment autonomous operation and cooperative navigation method disclosed in this embodiment has the following specific implementation steps:

[0094] Step 1 is implemented as follows:

[0095] Establish the coordinate systems used in the navigation process of lunar-based equipment and the transformation relationships between the coordinate systems. The coordinate systems include the lunar-based equipment coordinate system, the navigation coordinate system, and the camera coordinate system.

[0096] Lunar base equipment coordinate system O c -X c Y c Z c Take the center of mass of the lunar base equipment as the origin O. c O c X c The axis is parallel to the bottom plane of the lunar base equipment and points forward, O c Z c The axis is perpendicular to the bottom plane of the lunar base equipment and points upwards, O c Y c The orientation of the axis and the first two coordinate axes satisfy the right-handed orthogonal coordinate system relationship.

[0097] Navigation coordinate system O d -X d Y d Z d The navigation coordinate system is defined as the initial lunar-based equipment coordinate system, with its basic direction pointing in the same direction as the lunar-based equipment coordinate system and stationary relative to the Moon. It is a Cartesian coordinate system in three-dimensional space, and the position coordinates of all three lunar-based equipment units are represented in the navigation coordinate system. The matrix A for transforming the navigation coordinate system to the lunar-based equipment coordinate system is expressed as follows:

[0098]

[0099] In equation (25), ψ, θ, and γ represent the navigation coordinate system's orbital distance around Y. d Z d X d The angle of rotation of the shaft.

[0100] Camera coordinate system O s -X s Y s Z s Assume the optical center of the left camera is fixedly connected to the lunar rover's center of mass O, the lens is perpendicular to the lunar base equipment and mounted in front, and the optical center of the camera is perpendicular to the coordinate origin O. s Coincidence, O s X s The axis is parallel to the plane of the lunar base equipment chassis and points to the right, O s Y s The axis is perpendicular to the plane of the lunar base equipment and points downwards, O s Z s The axis coincides with the optical axis of the vision camera and points forward.

[0101] Step 2: Establish a dynamic model of lunar-based equipment movement. The movement of lunar-based equipment on the lunar surface can be considered as three-dimensional motion. During its journey, the equipment will encounter various terrain conditions, including craters, rocks, and other unique topography. By modeling the dynamics of lunar-based equipment movement, the process of state parameter changes during its movement on the lunar surface can be intuitively reflected.

[0102] The continuous change process of the lunar base equipment state is accurately described by the kinematic model of the lunar base equipment as shown in Equation (26). In order to perform subsequent digital signal processing, the continuous process needs to be discretized and a simplified kinematic model of the lunar base equipment is used to approximate the change process of the lunar base equipment state.

[0103]

[0104] In equation (26), v x v y v z These represent the velocities of the lunar-based equipment moving in different directions along the coordinate axes, a. x a y a z These represent the accelerations of the lunar-based equipment moving in different directions along the coordinate axes.

[0105] Lunar-based equipment can be considered as a rigid body. The rotational motion of a rigid body about any fixed point has only 3 degrees of freedom. Therefore, to describe the attitude motion of lunar-based equipment, 6 independent state variables are needed.

[0106]

[0107] In equation (27), w x w y w z These represent the angular velocities of the lunar-based equipment around different coordinate axes.

[0108]

[0109] The forces acting on the lunar-based equipment along the coordinate axes include thrust along the lateral axis, rolling around the longitudinal axis, and vertical oscillation along the vertical axis.

[0110] In the simplified physical model of the lunar environment, the forces acting on the wheels of the lunar-based equipment in each direction can be obtained as shown in equation (29). The force relationships of the lunar-based equipment are as follows:

[0111]

[0112] In equation (29), The longitudinal forces F on the right and left front wheels are respectively. lf The longitudinal resultant force of the two front wheels, The lateral forces F are the lateral forces on the right front wheel and the left front wheel, respectively. cf The resultant lateral force of the two front wheels, F represents the longitudinal forces on the right rear wheel and the left rear wheel, respectively. lr The longitudinal resultant force of the two rear wheels, F represents the lateral forces on the right rear wheel and the left rear wheel, respectively. cr Let M be the resultant lateral force of the two rear wheels, M be the torque acting on the lunar-based equipment, and I be the moment of inertia of the lunar-based equipment. The turning angle during the movement of lunar-based equipment.

[0113] Step 3: Through information collaboration among multiple lunar base equipment, master-slave collaborative navigation among multiple lunar base equipment is achieved, improving the accuracy and reliability of collaborative navigation among multiple lunar base equipment, and thus improving the accuracy and robustness of the master-slave collaborative navigation system for autonomous operation.

[0114] Lunar-based equipment is divided into primary lunar-based equipment and member lunar-based equipment. The primary lunar-based equipment carries sensors including strapdown inertial navigation systems (INS), laser rangefinders, and binocular vision cameras. Member lunar-based equipment carries sensors including strapdown INS and radio equipment. The primary lunar-based equipment uses the laser rangefinder to measure the distance to a pre-set beacon, and combines this with azimuth information provided by the binocular vision cameras to determine the positions of the two member lunar-based equipment relative to the primary equipment. The strapdown INS carried by each member lunar-based equipment obtains its own position information and transmits it to the primary lunar-based equipment via radio. The primary lunar-based equipment then performs filtering and fusion to obtain a more accurate position for the member lunar-based equipment.

[0115] The attitude of lunar-based equipment is primarily described using quaternions. Quaternions avoid singularities when representing attitude coordinates and are relatively simple to calculate. The transformation quaternion for converting the navigation frame n to the carrier coordinate system b is q. The attitude differential equation can be expressed as:

[0116]

[0117] In equation (30), To represent quaternion multiplication, It can be calculated using the following formula:

[0118]

[0119] In equation (31), For lunar-based equipment relative to the inertial frame O d -X d Y d Z d The projection of the corresponding angular velocity onto the system; This represents the angular velocity vector of the fixed frame of reference relative to the inertial frame due to the Moon's rotation. The navigation coordinate system O is affected by the change in the position of the lunar base equipment. c -X c Y c Z c The rotational angular velocity vector relative to the Moon's fixed connection, This is the attitude transformation matrix from the navigation system to the carrier system.

[0120] The velocity v of the lunar-based equipment in a coordinate system fixed to the moon. e for:

[0121]

[0122] velocity v e Transferring to the navigation system, we get:

[0123]

[0124] In equation (33), v n The speed is the speed under the navigation system.

[0125] Differentiating the velocity in the navigation system and simplifying, we get:

[0126]

[0127] According to Newton's second law of motion, the acceleration of an object is related to the external force acting on it. f i For the specific force in the inertial coordinate system, G i The gravitational acceleration of the moon (G) i ≈1.63m / s 2 According to the formula for calculating the gravity on the lunar surface. The differential equation for the velocity of lunar-based equipment can be derived as follows:

[0128]

[0129] In equation (35), fb For comparison under the condition of solid connection on the lunar surface, g n This refers to the lunar gravitational acceleration in the inertial navigation frame.

[0130] Step 4: The information obtained from each subsystem is fused using a joint filtering model. The sensor of the strapdown inertial navigation system is processed by discrete Kalman filtering, and the angle and distance information obtained from the binocular stereo camera and laser rangefinder are processed by extended Kalman filtering. Finally, the information is input into the master filter for time updating to obtain the optimal fusion of master-slave cooperative navigation.

[0131] The main filter fuses information from the accelerometer and gyroscope, using the positions of these two internal sensors as data to estimate the equipment's coordinates at that moment. The other two sub-filters estimate the target's position using cooperative sensor localization. They observe the target using the two external sensors and fuse the resulting information to correct autonomous localization errors. The target's localization information from the external vision sensor serves as the input to the sub-filters, and an extended Kalman filter is used to estimate the target's localization state for the vision sensors.

[0132] The information allocation factors of each sub-filter are calculated and allocated by the main filter. By using an extended Kalman filter, the position and attitude information obtained by the sub-filters are estimated and fused according to the allocation factor weights, making the target localization more accurate.

[0133] This invention uses discrete Kalman filtering to fuse data from accelerometers and gyroscopes, treating it as a linear system, and taking the state variable X = [a x a y a z w g ] T System state equation X k+1 =A k+1 X k+1 W k A k+1 W represents the state transition matrix from time k to k+1. k This is used to represent Gaussian white noise in a system with a variance matrix of Q. The transition matrix can be set as follows: β is a correction coefficient determined based on experimental debugging.

[0134] The observation equation is Z k =H k X k +V k The observation data can be obtained from accelerometers and gyroscopes, so the measurement matrix H k Let I be the identity matrix.

[0135] Based on the model of the laser rangefinder, by placing two beacons, the distance between the target and the sensor can be obtained, thereby obtaining the distance of the member lunar base equipment relative to the main lunar base equipment at the current moment.

[0136] Let the current position coordinates of the main lunar base equipment be (x... k ,y k ,z k The distance equation for a laser rangefinder is:

[0137] The lunar-based equipment measurement matrix is ​​as follows:

[0138]

[0139] In the formula,

[0140] According to equation (36), the overall observation equation can be obtained as Z. J =H J X J +V J .

[0141] The distance between the target and the vision sensor can be obtained through measurement. Let the coordinates of the main lunar base equipment during observation be (x...). k ,y k ,z k The distance observation equation for the main lunar-based equipment is derived as follows:

[0142]

[0143] In equation (37), For Gaussian white noise, derive the Jacobian measurement matrix:

[0144]

[0145] The inertial navigation system obtained by fusing the accelerometers and gyroscopes of each piece of equipment is used as the reference state. Visual positioning and laser ranging positioning are two independent sub-filtering systems of the main equipment. Extended Kalman filtering is used for state estimation. The above two sub-filters pass the estimation structure of each step to the main filter. The main filter completes the optimal fusion of information. The overall state estimate is obtained by taking the arithmetic mean of the optimal estimates of the states of the two subsystems.

[0146] The filtering system solves for the information allocation factors β1 and β2, and the information allocation coefficient β i It obeys the principle of information conservation, that is... β m These are the coefficients assigned to the main filter information. Where β1,…,β NThe determination of the coefficients will affect the performance of the joint filter. Generally, larger coefficient values ​​are assigned to sensors with higher measurement accuracy. This invention uses coefficients based on the variance matrix P. i eigenvalue decomposition for β i The values ​​are calculated in real time. i Decomposed into P according to the eigenvalues ​​of the variance matrix i =L i ∧L T Λ i =diag{λ i1 ,λ i2 ,...,λ iN}, we can deduce that:

[0147]

[0148] Extended Kalman filtering marginalizes all past states and obtains real-time performance by estimating the current state. The fusion filtering algorithm is specifically divided into the following steps:

[0149] Initialize global state estimator Covariance matrix P i0 And the navigation information is proportioned according to the information factor β obtained in equation (39). i Assigned to two sub-filters and the main filter:

[0150]

[0151]

[0152] Since cooperative navigation systems require spatiotemporal synchronization, time correction is performed simultaneously on each sub-filter and the main filter. Common noise information is adjusted according to the proportion β of the matched information factor. i Assigned to each sub-filter:

[0153]

[0154] After time correction of the filter, we get:

[0155]

[0156] P i (k+1|k)=A(k+1|k)P i (k)+Γ(k)Q i (k)Γ T (k) (44)

[0157] Where Γ is the noise matrix of the system, and A is the state transition matrix of the system.

[0158] Each sub-filter utilizes its own local observation Z.i Correct the observed values:

[0159]

[0160]

[0161] Among them, H i This is the measurement matrix.

[0162] Information fusion of the main filter:

[0163]

[0164]

[0165] The fused results are used to reset the sub-filters for time updates, effectively controlling positioning errors. The final navigation error is as follows: Figure 7 As shown, the image of the primary lunar base equipment, after being fused through collaborative navigation filtering using multiple sensors, can eliminate some of the errors introduced by the strapdown inertial navigation system (SINS). These errors accumulate over time, while interference noise gradually weakens, thus bringing the image closer to the true value. The image of the two member lunar base equipment, after being fused through collaborative navigation filtering using SINS and radio signals transmitted from the primary lunar base equipment, shows that the corrected result can eliminate some of the errors accumulated by the SINS over time, bringing it closer to the true value. These images fully demonstrate the necessity and reliability of the master-slave multi-lunar base equipment autonomous operation collaborative navigation method.

[0166] 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 master-slave multi-lunar base equipment autonomous operation and cooperative navigation method, characterized in that: Includes the following steps, Step 1: Establish the coordinate systems used in the navigation process of lunar-based equipment and the transformation relationships between the coordinate systems; the coordinate systems include the lunar-based equipment coordinate system, the navigation coordinate system, and the camera coordinate system; Step 2: The motion of lunar-based equipment on the lunar surface is equivalent to three-dimensional motion. By modeling the motion dynamics of lunar-based equipment, the process of state parameter changes when lunar-based equipment moves on the lunar surface can be intuitively represented. Step 3: Through information coordination among multiple lunar base equipment, master-slave multi-lunar base equipment cooperative navigation is achieved, thereby improving the accuracy and reliability of multi-lunar base equipment cooperative navigation; Step 3 is implemented as follows: Lunar-based equipment is divided into primary lunar-based equipment and member lunar-based equipment. The primary lunar-based equipment carries sensors including strapdown inertial navigation systems (SINS), laser rangefinders, and binocular vision cameras. The member lunar-based equipment carries sensors including strapdown inertial navigation systems (SINS) and wireless equipment. The primary lunar-based equipment uses laser rangefinders to measure the distance to pre-set beacons and combines this with orientation information provided by binocular vision cameras to determine the positions of the two member lunar-based equipment relative to the primary lunar-based equipment. The member lunar-based equipment uses its strapdown inertial navigation system to obtain its own position information and transmits the information to the primary lunar-based equipment via wireless equipment. The primary lunar-based equipment then performs filtering and fusion to obtain a more accurate position of the member lunar-based equipment. The attitude of lunar-based equipment is primarily described using quaternions. Quaternions avoid singularities when representing attitude coordinates and are relatively simple to calculate. The transformation quaternion for converting the navigation frame n to the carrier coordinate system b is q. The attitude differential equation is expressed as: （6） In formula (6), " " represents quaternion multiplication, Calculate using the following formula: （7） In equation (7), For lunar-based equipment relative to inertial frames The projection of the corresponding angular velocity onto the system; This represents the angular velocity vector of the fixed frame of reference relative to the inertial frame due to the Moon's rotation. The navigation coordinate system caused by the change in the position of lunar base equipment The rotational angular velocity vector relative to the Moon's fixed connection, This represents the attitude transformation matrix from the navigation system to the carrier system. Velocity of lunar-based equipment in a coordinate system fixed to the moon for: （8） speed Transferring to the navigation system, we get: （9） In equation (9), Speed ​​under navigation system; Differentiating the velocity in the navigation system and rearranging, we get: （10） According to Newton's second law of motion, the acceleration of an object is related to the external force acting on it. , Specific force in inertial coordinate system For the Moon's gravitational acceleration, ; According to the formula for calculating gravity on the lunar surface The differential equation for the velocity of lunar-based equipment is derived as follows: （11） In equation (11), For comparison under the condition of solid connection on the lunar surface, The lunar gravitational acceleration in the inertial navigation frame; Step 4: The information obtained from each subsystem is fused using a joint filtering model. The sensor of the strapdown inertial navigation system is processed by discrete Kalman filtering, and the angle and distance information obtained by the binocular stereo vision camera and laser rangefinder are processed by extended Kalman filtering and input into the main filter for time updating. The optimal fusion result of the master-slave cooperative navigation is obtained. Based on the optimal fusion result, the accuracy and robustness of the master-slave multi-lunar base equipment autonomous operation cooperative navigation system are improved.

2. The master-slave multi-lunar base equipment autonomous operation and cooperative navigation method as described in claim 1, characterized in that: Step 1 is implemented as follows: Lunar-based equipment coordinate system Taking the center of mass of the lunar-based equipment as the origin. , The axis is parallel to the bottom plane of the lunar base equipment and points forward. The axis is perpendicular to the bottom plane of the lunar base equipment and points upwards. The orientation of the axis relative to the first two coordinate axes satisfies the right-handed orthogonal coordinate system relationship; Navigation coordinate system The navigation coordinate system is defined as the initial lunar-based equipment coordinate system, with its basic direction pointing in the same direction as the lunar-based equipment coordinate system and stationary relative to the Moon. It is a Cartesian coordinate system in three-dimensional space, and the position coordinates of all three lunar-based equipment are represented in the navigation coordinate system. The matrix for transforming the navigation coordinate system to the lunar-based equipment coordinate system is A, expressed as: （1） In equation (1), , , respectively around the navigation coordinate system , , The angle of rotation of the shaft; Camera coordinate system When the optical center of the left camera aligns with the center of mass of the lunar rover The camera is fixed in place, with the lens perpendicular to the lunar base equipment and mounted in front of it. The optical center of the camera is aligned with the origin of the coordinate system. coincide, The axis is parallel to the plane of the lunar base equipment chassis and points to the right. The axis is perpendicular to the plane of the lunar base equipment and points downwards. The axis coincides with the optical axis of the vision camera and points forward.

3. The master-slave multi-lunar base equipment autonomous operation and cooperative navigation method as described in claim 2, characterized in that: Step 2 is implemented as follows: The continuous change process of the lunar base equipment state is accurately described by the kinematic model of the lunar base equipment as shown in Equation (2). In order to perform subsequent digital signal processing, the continuous process needs to be discretized and a simplified kinematic model of the lunar base equipment is adopted to approximate the change process of the lunar base equipment state. （2） In equation (2), v x v y v z These represent the velocities of the lunar-based equipment moving in different directions along the coordinate axes, a. x a y a z These represent the accelerations of the lunar-based equipment moving in different directions along the coordinate axes; Lunar-based equipment is equivalent to a rigid body. The rotational motion of a rigid body about any fixed point has only 3 degrees of freedom. That is, to describe the attitude motion of lunar-based equipment, 6 independent state variables are needed. （3） In equation (3), w x w y w z These represent the angular velocities of the lunar-based equipment around different coordinate axes; （4） The forces acting on lunar-based equipment along the coordinate axes include thrust along the lateral axis, rolling around the longitudinal axis, and vertical oscillation along the vertical axis. In the simplified physical model of the lunar environment, the force conditions of the lunar base equipment wheels in each direction are shown in Equation (5); the force relationship of the lunar base equipment is as follows: （5） In equation (5), , These are the longitudinal forces on the right front wheel and the left front wheel, respectively. The longitudinal resultant force of the two front wheels, , These are the lateral forces on the right front wheel and the left front wheel, respectively. The resultant lateral force of the two front wheels, , These are the longitudinal forces on the right rear wheel and the left rear wheel, respectively. The longitudinal resultant force of the two rear wheels, , These are the lateral forces on the right rear wheel and the left rear wheel, respectively. denoted as the resultant lateral force of the two rear wheels, M as the torque acting on the lunar base equipment, I as the moment of inertia of the lunar base equipment, and ϑ as the steering angle of the lunar base equipment during its motion.

4. The master-slave multi-lunar base equipment autonomous operation and cooperative navigation method as described in claim 3, characterized in that: Step 4 is implemented as follows: The main filter fuses information from the accelerometer and gyroscope, using the positions of the two internal sensors as data to estimate the equipment's own position coordinates. The other two sub-filters estimate the target's positioning information through sensor cooperative localization, observe the target using two external sensors, and then fuse the obtained information to correct autonomous positioning errors. The target positioning information obtained from the external vision sensor is used as the observation input of the sub-filters, and then the extended Kalman filter is used to estimate the positioning state of the target by the aforementioned external vision sensor. The information allocation factors of each sub-filter are calculated and allocated by the main filter. By using the extended Kalman filter, the position and attitude information obtained by the sub-filters are estimated and fused according to the allocation factor weights, so that the target localization is more accurate. Discrete Kalman filtering is used to fuse the data from the accelerometer and gyroscope, treating it as a linear system, with the state variable X=[a x a y a z w g ] T System state equation X k+1 =A k+1 X k+1 W k A k+1 W represents the state transition matrix from time k to k+1. k This is used to represent Gaussian white noise in a system with variance matrix Q; where the transition matrix is ​​set to... β is a correction coefficient determined based on experimental debugging; The observation equation is Z k =H k X k +V k The observation data is obtained from accelerometers and gyroscopes, so the measurement matrix H k I is the identity matrix; Based on the laser rangefinder model, by placing two beacons, the distance between the target and the sensor is obtained, thereby obtaining the distance of the member lunar base equipment relative to the main lunar base equipment at the current moment; Define the current position coordinates of the primary lunar base equipment as (x... k ,y k ,z k The distance equation for a laser rangefinder is: ; The lunar-based equipment measurement matrix is ​​as follows: （12） In equation (12), ; According to equation (12), the overall observation equation is Z. J =H J X J +V J ; The distance between the target and the visual sensor is obtained by measurement; the coordinate position of the main lunar base equipment during observation is defined as (x... k ,y k ,z k The distance observation equation for the main lunar-based equipment is derived as follows: （13） In equation (14), For Gaussian white noise, derive the Jacobian measurement matrix: （14） The inertial navigation system obtained by fusing the accelerometers and gyroscopes of each piece of equipment is used as the reference state. Visual positioning and laser ranging positioning are two independent sub-filtering systems of the main equipment. Extended Kalman filtering is used for state estimation. The above two sub-filters pass the estimation structure of each step to the main filter. The main filter completes the optimal fusion of information. The overall state estimate is obtained by taking the arithmetic mean of the optimal estimates of the states of the two subsystems. The filtering system solves for the information allocation factors β1 and β2, and the information allocation coefficient β i It obeys the principle of information conservation, that is... ,β m The coefficients for allocating information to the main filter; where β1,…,β N The determination of the coefficients will affect the performance of the joint filter; P i Decomposed into P according to the eigenvalues ​​of the variance matrix i =L i ∧L T , The derivation yields: （15） Extended Kalman filtering marginalizes all past states and obtains real-time performance by estimating the current state. The fusion filtering algorithm is specifically divided into the following steps: Initialize global state estimator covariance matrix And the navigation information is proportioned according to the information factor β obtained in equation (16). i Assigned to two sub-filters and the main filter: （16） （17） Since cooperative navigation systems require spatiotemporal synchronization, time correction is performed simultaneously on each sub-filter and the main filter. Common noise information is adjusted according to the proportion β of the matched information factor. i Assigned to each sub-filter: （18） After time correction of the filter, we get: （19） （20） Where Γ is the noise matrix of the system, and A is the state transition matrix of the system; Each sub-filter utilizes its own local observation Z. i Correct the observed values: （21） （22） Among them, H i For measurement matrix; Information fusion of the main filter: （23） （24） The sub-filter is reset using the fusion result for time updates, effectively controlling positioning errors.

Images