Collaborative navigation method and system for unmanned aerial vehicle satellite image matching based on factor graph optimization

Abstract

The invention discloses an unmanned aerial vehicle satellite image matching collaborative navigation method and system based on factor graph optimization, and the method specifically comprises the steps: building a front-end module and a rear-end module, achieving a visual odometer through inter-frame image feature matching at the front-end module, and completing the relative positioning; a visual positioning system AS-VPS based on satellite image matching is constructed in the rear-end module; constructing a position as a state variable based on an FGO algorithm, representing a cost function by using a residual vector, and calculating state increment estimation by using a least square method based on the minimum optimal state residual; establishing a CO-AS-VPS and FGO integrated model, coding by taking a front-end visual odometer, rear-end measurement and collaborative distance measurement as factors, and constructing a cost function to obtain optimal estimation; and for the CO-AS-VPS and FGO integrated model, the optimal estimation is iteratively solved. According to the invention, the positioning precision and reliability of the unmanned aerial vehicle navigation system in a complex environment are improved, and the continuity and stability of the system are enhanced.

Description

Technical Field

[0001] The present invention relates to the field of sensor data fusion, and particularly to a cooperative navigation method and system for UAV satellite image matching based on factor graph optimization. Background Art

[0002] With the rapid development of UAV technology, UAVs are increasingly widely used in multiple fields, such as surveying and mapping, agriculture, environmental protection, etc. However, in complex environments, high-precision navigation of UAVs still faces many challenges, especially when GNSS signals are limited or unavailable. Existing positioning methods usually rely on a single sensor positioning algorithm, making it difficult to ensure stability and accuracy in dynamic environments.

[0003] Satellite images, as a reliable external reference information, can provide precise absolute positioning for UAVs. However, existing UAV navigation technologies often have problems of insufficient image matching accuracy and unstable positioning when using satellite images for positioning. To make up for this deficiency, in recent years, the factor graph optimization (FGO) algorithm has gradually become a research hotspot in UAV navigation due to its advantages in multi-sensor fusion and state estimation. Summary of the Invention

[0004] The purpose of the present invention is to provide a cooperative navigation method and system for UAV satellite image matching based on factor graph optimization, which has high positioning accuracy, strong stability, and strong environmental adaptability.

[0005] The technical solution for achieving the purpose of the present invention is as follows: A cooperative navigation method for UAV satellite image matching based on factor graph optimization includes the following steps:

[0006] Step 1: Establish a front-end module and a back-end module. In the front-end module, visual odometry is achieved through inter-frame image feature matching to complete relative positioning; in the back-end module, the aerial photography information of the UAV is matched with satellite images to achieve absolute positioning, and then the absolute positioning information is fed back to the front-end visual odometry to construct a visual positioning system AS-VPS based on satellite image matching.

[0007] Step 2: Based on the FGO algorithm, construct the position as a state variable, use the residual vector to represent the cost function, and based on the minimum of the optimal state residual, use the least squares method to calculate the state increment estimation.

[0008] Step 3: Establish a CO-AS-VPS and FGO integrated model, encode the front-end visual odometry, back-end measurement, and cooperative distance measurement as factors, and construct a cost function to obtain the optimal estimation; where CO-AS-VPS represents a multi-UAV cooperative visual positioning system based on satellite image matching.

[0009] Step 4: Set initial values for the CO-AS-VPS and FGO integrated model and iteratively solve for the optimal estimate.

[0010] A cooperative navigation system for UAV satellite image matching based on factor graph optimization, which is used to implement the cooperative navigation method for UAV satellite image matching based on factor graph optimization. The system includes a first module to a fourth module, and the functions of each module are as follows:

[0011] The first module: Establish a front-end module and a back-end module. In the front-end module, visual odometry is achieved through inter-frame image feature matching to complete relative positioning. In the back-end module, the UAV aerial photography information is matched with satellite images to achieve absolute positioning, and then the absolute positioning information is fed back to the front-end visual odometry to construct a visual positioning system AS-VPS based on satellite image matching.

[0012] The second module: Based on the FGO algorithm, construct the position as the state variable, represent the cost function using the residual vector, and calculate the state increment estimate using the least squares method based on the minimum of the optimal state residual.

[0013] The third module: Establish a CO-AS-VPS and FGO integrated model, encode the front-end visual odometry, back-end measurement, and cooperative distance measurement as factors, and construct a cost function to obtain the optimal estimate. Among them, CO-AS-VPS represents a multi-aircraft cooperative visual positioning system based on satellite graphic matching.

[0014] The fourth module: Set initial values for the CO-AS-VPS and FGO integrated model and iteratively solve for the optimal estimate.

[0015] A mobile terminal includes a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the program, it implements the cooperative navigation method for UAV satellite image matching based on factor graph optimization.

[0016] A computer-readable storage medium stores a computer program, and when the program is executed by a processor, it implements the steps in the cooperative navigation method for UAV satellite image matching based on factor graph optimization.

[0017] Compared with the prior art, the significant advantages of the present invention are as follows: (1) By integrating the front-end visual odometer and the back-end absolute positioning information, continuous, stable and high-precision positioning is achieved; (2) The factor graph optimization method is adopted to effectively encode and fuse the measurement data from multiple sensors and UAVs, enhancing the navigation ability of the system in complex environments; (3) By introducing the ranging factor and the cost function, the measurement uncertainty is quantified, and the state estimation is further optimized, ensuring the stability and accuracy of the system under complex and dynamic conditions and improving the cooperative navigation ability of the UAV swarm. Brief Description of the Drawings

[0018] Figure 1 It is a flowchart of the cooperative navigation method for UAV satellite image matching based on factor graph optimization of the present invention.

[0019] Figure 2 It is the basic schematic diagram of CO-AS-VPS in the present invention.

[0020] Figure 3 It is a schematic structural diagram of the integrated model of CO-AS-VPS and FGO in the present invention.

[0021] Figure 4 It is a schematic flowchart of the iterative solution of the integrated model of CO-AS-VPS and FGO in the present invention. Detailed Description of the Preferred Embodiment

[0022] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0023] As Figure 1 shown, a cooperative navigation method for UAV satellite image matching based on factor graph optimization includes the following steps:

[0024] Step 1: Establish a front-end module and a back-end module. In the front-end module, visual odometry is achieved through inter-frame image feature matching to complete relative positioning; in the back-end module, the UAV aerial photography information is matched with the satellite image to achieve absolute positioning, and then the absolute positioning information is fed back to the front-end visual odometer to construct a visual positioning system AS-VPS based on satellite image matching.

[0025] Step 2: Based on the FGO algorithm, construct the position as the state variable, use the residual vector to represent the cost function, and calculate the state increment estimation based on the least squares method with the optimal state residual being the smallest.

[0026] Step 3: Establish an integrated model of CO-AS-VPS and FGO, encode the front-end visual odometer, back-end measurement and cooperative distance measurement as factors, and construct a cost function to obtain the optimal estimation; where CO-AS-VPS represents a multi-UAV cooperative visual positioning system based on satellite image matching.

[0027] Step 4: Set the initial values for the CO-AS-VPS and FGO integrated model and iteratively solve for the optimal estimate.

[0028] As a specific example, in Step 1, a front-end module and a back-end module are established. In the front-end module, visual odometry is achieved through inter-frame image feature matching to complete relative positioning. In the back-end module, the aerial photography information of the drone is matched with satellite images to achieve absolute positioning, and then the absolute positioning information is fed back to the front-end visual odometry to construct a visual positioning system AS-VPS based on satellite image matching, as Figure 2 shown below:

[0029] Step 1.1: In the front-end vision module, feature matching is completed through the LightGlue framework, and the transformation between two consecutive frames of the front-end visual odometry is modeled as df = (dx dy) T and dθ using a function with two parameters. The relationship between two consecutive frames can be expressed as:

[0030]

[0031] where f i is the coordinate of the previous frame in the pixel coordinate system, f i+1 is the coordinate of the current frame in the pixel coordinate system, dx is the translation increment in the x-axis direction, dy is the translation increment in the y-axis direction, and dθ is the change in the rotation angle;

[0032] The transformation from pixel coordinates to UTM coordinates is:

[0033] f W = β · (R Wu · f u + t Wu )

[0034] where R Wu and t Wu are the rotation and displacement matrices respectively, f u represents the point in the pixel coordinates, f w represents the point corresponding to f u in the UTM coordinates, and β is the scale factor from the pixel coordinate system to the body coordinate system;

[0035] Step 1.2: In the back-end vision module, after LightGlue completes feature point matching using satellite tile images, the formula for transformation to UTM coordinates is:

[0036]

[0037] where (La t, Lo t ), (La b , Lo b ) represent the geographical locations of the upper-left and lower-right corners of the satellite image respectively. C x and C y represent the pixel positions of the UAV image. W and H are the height and width of the satellite image; La, Lo are the geographical coordinates of the feature points of the UAV image;

[0038] Step 1.3: Feed the absolute positioning information back to the correct front-end visual odometer to construct a visual positioning system AS-VPS based on satellite image matching.

[0039] As a specific example, in Step 2, based on the FGO algorithm, the position is constructed as the state variable, the cost function is represented by the residual vector, and based on the minimum of the optimal state residual, the least squares method is used to calculate the state increment estimation, which is as follows:

[0040] Step 2.1: Solve the state estimation by searching for the maximum value of the global probability function

[0041]

[0042] where L is the position state, and G(L1, L2, …, L N ) is the global probability function. Therefore, the state estimation is:

[0043]

[0044] where p() is the joint probability density;

[0045] Step 2.2: Represent the cost function using the residual vector as:

[0046]

[0047] where E is the residual vector;

[0048] Step 2.3: Since the optimal state needs to satisfy the minimum residual, the state increment is obtained using the least squares method:

[0049]

[0050] where is the Jacobian matrix, is the residual.

[0051] As a specific example, in Step 3, a CO-AS-VPS and FGO integrated model is established. The front-end visual odometer, the back-end measurement, and the collaborative distance measurement are encoded as factors, and the cost function is constructed to obtain the optimal estimation, which is as follows:

[0052] Step 3.1: Select the front-end visual odometry factor, the back-end measurement factor, and the cooperative ranging measurement factor as the factors for integrating CO-AS-VPS and FGO; assume that there are three drones for cooperative navigation, then the position vector p is expressed as:

[0053]

[0054] where the superscripts Usr 11 , Usr2, and Usr3 are the indices of the drones, and the subscript k represents the epoch, represents the state of the Usr i -th drone at time k.

[0055] Step 3.2: For the Usr i -th drone, the front-end visual odometry factor and the cost function are respectively:

[0056]

[0057] where is the front-end measurement increment from time k to time k + 1, N(·) is the Gaussian distribution, is the covariance matrix of the Usr i drone at time k + 1, is the front-end odometry function, is the front-end cost function of the Usr i drone at time k + 1;

[0058] Step 3.3: The back-end measurement factor and the cost function are respectively:

[0059]

[0060] where is the back-end measurement position, is the covariance matrix, is the back-end cost function of the Usr i drone at time k + 1, is the back-end odometry function;

[0061] Step 3.4: The cooperative ranging factor and the loss function are defined as:

[0062]

[0063] where is the covariance matrix, is the distance ranging factor between the Usr i and Usr j drones, is the cost function of the cooperative ranging factor;

[0064] Step 3.5. In summary, the global cost function is as follows:

[0065]

[0066] where p * is the global cost function, argmin(·) is the minimum value of the surrogate cost function, is the backend cost function of the i-th UAV at time j + 1, is the frontend cost function of the i-th UAV at time j + 1, represents the distance cost function between UAVs 1 and 2, represents the distance cost function between UAVs 1 and 3, represents the distance cost function between UAVs 2 and 3.

[0067] As a specific example, for the CO-AS-VPS and FGO integrated model described in Step 4, set the initial values and iteratively solve for the optimal estimate, as Figure 4 shown below:

[0068] Step 4.1. After obtaining the initial values according to Step 3 , update the Jacobian matrix and the residual limit

[0069] Step 4.2. Use the least squares equation to calculate ΔL i , and update the state to

[0070]

[0071] Step 4.3. Repeat Step 4.1 to Step 4.2. If the residual is lower than the set threshold, it is considered converged, terminate the iteration and obtain the optimal estimate If the maximum number of iterations N max is reached and it still has not converged, terminate the calculation and use the current state vector as the final estimation result.

[0072] The present invention also provides a cooperative navigation system for UAV satellite image matching based on factor graph optimization. This system is used to implement the cooperative navigation method for UAV satellite image matching based on factor graph optimization. The system includes a first module to a fourth module, and the functions of each module are as follows:

[0073] The first module establishes a front-end module and a back-end module. In the front-end module, visual odometry is achieved through inter-frame image feature matching to complete relative positioning. In the back-end module, the aerial photography information of the drone is matched with satellite images to achieve absolute positioning, and then the absolute positioning information is fed back to the front-end visual odometry to construct a visual positioning system AS-VPS based on satellite image matching.

[0074] The second module constructs the position as a state variable based on the FGO algorithm, represents the cost function using the residual vector, and calculates the state increment estimation using the least squares method based on the minimum of the optimal state residual.

[0075] The third module establishes an integrated model of CO-AS-VPS and FGO, encodes the front-end visual odometry, back-end measurement, and cooperative distance measurement as factors, and constructs a cost function to obtain the optimal estimation. Here, CO-AS-VPS represents a multi-robot cooperative visual positioning system based on satellite image matching.

[0076] The fourth module sets the initial value for the integrated model of CO-AS-VPS and FGO and iteratively solves for the optimal estimation.

[0077] The present invention also provides a mobile terminal, including a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the program, it implements the cooperative navigation method for drone satellite image matching based on factor graph optimization.

[0078] The present invention also provides a computer-readable storage medium, on which a computer program is stored. When the program is executed by a processor, it implements the steps in the cooperative navigation method for drone satellite image matching based on factor graph optimization.

[0079] The following further elaborates on the present invention with specific embodiments.

[0080] Embodiment

[0081] In this embodiment, the present invention's CO-AS-VPS and the traditional FGO-AS-VPS are respectively used for cooperative navigation of drone satellite image matching. The horizontal errors of CO-AS-VPS and FGO-AS-VPS are shown in Table 1:

[0082] Table 1 Comparison table of horizontal errors between CO-AS-VPS and FGO-AS-VPS

[0083]

[0084] As can be seen from Table 1, compared with the traditional FGO-AS-VPS non-cooperative navigation method, the horizontal average errors of Trajectory 1 and Trajectory 2 of the CO-AS-VPS of the present invention are reduced by 24.25% and 32.50% respectively; the maximum errors are reduced from 46.06 m to 39.16 m and from 39.05 m to 36.02 m respectively. The experimental results show that the cooperative navigation method for UAV satellite image matching based on factor graph optimization provided by the present invention effectively improves the positioning accuracy.

[0085] The above are only the preferred embodiments of the present invention, and do not limit the scope of implementation of the present invention. Without departing from the principle of the present invention, several improvements and retouches can be made, and these improvements and retouches should also be regarded as the protection scope of the present invention.

Claims

1. A cooperative navigation method for UAV satellite image matching based on factor graph optimization, characterized in that It includes the following steps: Step 1: Establish a front-end module and a back-end module. In the front-end module, visual odometry is achieved through inter-frame image feature matching to complete relative positioning; In the back-end module, the aerial photography information of the drone is matched with the satellite image to achieve absolute positioning, and then the absolute positioning information is fed back to the front-end visual odometry to construct a visual positioning system AS-VPS based on satellite image matching; Step 2: Based on the FGO algorithm, construct the position as a state variable, use the residual vector to represent the cost function, and based on the minimum of the optimal state residual, use the least squares method to calculate the state increment estimation; Step 3: Establish an integrated model of CO-AS-VPS and FGO, encode the front-end visual odometry, back-end measurement, and collaborative distance measurement as factors, and construct a cost function to obtain the optimal estimation; where CO-AS-VPS represents a multi-aircraft collaborative visual positioning system based on satellite image matching; Step 4: For the integrated model of CO-AS-VPS and FGO, set the initial value and iteratively solve the optimal estimation.

2. The collaborative navigation method for UAV satellite image matching optimized based on factor graph according to claim 1, characterized in that In Step 1, a front-end module and a back-end module are established. In the front-end module, visual odometry is achieved through inter-frame image feature matching to complete relative positioning; in the back-end module, the aerial photography information of the drone is matched with the satellite image to achieve absolute positioning, and then the absolute positioning information is fed back to the front-end visual odometry to construct a visual positioning system AS-VPS based on satellite image matching, specifically as follows: Step 1.

1. In the front-end vision module, feature matching is completed through the LightGlue framework, and the transformation between two consecutive frames of the front-end visual odometer is modeled as df = (dx dy) T and dθ, and the relationship between two consecutive frames is expressed as: Among them, f i is the coordinate of the previous frame in the pixel coordinate system, f i+1 is the coordinate of the current frame in the pixel coordinate system, dx is the translation increment in the x-axis direction, dy is the translation increment in the y-axis direction, and dθ is the change in the rotation angle in the rotation angle; The transformation from pixel coordinates to UTM coordinates is: f W = β·(R Wu ·f u + t Wu ) where R Wu and t Wu are the rotation and displacement matrices respectively, f u represents a point in pixel coordinates, f w represents the corresponding point of f u in UTM coordinates, and β is the scale factor from the pixel coordinate system to the body coordinate system; Step 1.2: In the back-end visual module, after LightGlue completes feature point matching using satellite tile images, the formula for transforming to UTM coordinates is: Among them, (La t , Lo t ), (La b , Lo b ) respectively represent the geographical locations of the upper left corner point and the lower right corner point of the satellite image, C x and C y represent the pixel positions of the UAV image, W and H are the height and width of the satellite image; La, Lo are the geographical coordinates of the feature points of the UAV image; Step 1.3: Feed the absolute positioning information back to the correct front-end visual odometry to construct a visual positioning system AS-VPS based on satellite image matching.

3. The collaborative navigation method for UAV satellite image matching optimized based on factor graph according to claim 2, characterized in that In Step 2, based on the FGO algorithm, construct the position as a state variable, use the residual vector to represent the cost function, and based on the minimum of the optimal state residual, use the least squares method to calculate the state increment estimation, specifically as follows: Step 2.1, solve the state estimation by searching for the maximum value of the global probability function where L is the position state, and G(L1, L2, …, L N ) is the global probability function, so the state estimate is: Among them, p() is the joint probability density; Step 2.2: Use the residual vector to represent the cost function as: Among them, E is the residual vector; Step 2.3: Since the optimal state needs to satisfy the minimum residual, the state increment is obtained using the least squares method: Among them, is the Jacobian matrix, is the residual.

4. The collaborative navigation method for UAV satellite image matching optimized based on factor graph according to claim 3, characterized in that In Step 3, establish an integrated model of CO-AS-VPS and FGO, encode the front-end visual odometry, back-end measurement, and collaborative distance measurement as factors, and construct a cost function to obtain the optimal estimation, specifically as follows: Step 3.1: Select the front-end visual odometry factor, back-end measurement factor, and collaborative ranging measurement factor as the factors for integrating CO-AS-VPS and FGO; assume there are three drones for collaborative navigation, then the position vector p is expressed as: where the superscripts Usr1, Usr2, and Usr3 are the indices of the UAVs, and the subscript k represents the epoch, indicating the state of the i Usr-th UAV at time k; Step 3.

2. For the Usr i th drone, the front-end visual odometry factor and the cost function are respectively: wherein, is the front-end measurement increment from time k to time k + 1, N(·) is the Gaussian distribution, is the covariance matrix of the Usr i UAV at time k + 1, is the front-end odometry function, is the front-end cost function of the Usr i UAV at time k + 1; Step 3.3: The back-end measurement factor and the cost function are respectively: Among them, is the backend measurement position, is the covariance matrix, is Usr at time k + 1 i is the backend cost function of the UAV, is the backend odometry function; Step 3.4: The collaborative ranging factor and the loss function are defined as: Among them, is the covariance matrix, is the distance ranging factor between the drone Usr i and Usr j ; is the cost function of the cooperative ranging factor; Step 3.5: In summary, the global cost function is: where p * is the global cost function, and argmin(·) is the minimum value of the surrogate cost function, is the backend cost function of the i-th UAV at the (j + 1)-th moment, is the frontend cost function of the i-th UAV at the (j + 1)-th moment, represents the distance cost function between UAVs 1 and 2, represents the distance cost function between UAVs 1 and 3, represents the distance cost function between UAVs 2 and 3.

5. The collaborative navigation method for UAV satellite image matching optimized based on factor graph according to claim 4, characterized in that, For the integrated model of CO-AS-VPS and FGO described in Step 4, set the initial value and iteratively solve the optimal estimation, specifically as follows: Step 4.

1. Obtain the initial value according to Step 3 After that, update the Jacobian matrix and the residual limit Step 4.2: Use the least squares equation to calculate ΔL i , and update the status to Step 4.3: Repeat steps 4.1 to 4.

2. If the residual is lower than the set threshold, it is considered converged, the iteration is terminated, and the optimal estimate is obtained. If the maximum number of iterations N is reached max and it still has not converged, the calculation is terminated, and the current state vector is used as the final estimation result.

6. A cooperative navigation system for UAV satellite image matching based on factor graph optimization, characterized in that This system is used to implement the cooperative navigation method for UAV satellite image matching based on factor graph optimization according to any one of claims 1 to 5. The system includes a first module to a fourth module, and the functions of each module are as follows: The first module establishes a front-end module and a back-end module. In the front-end module, visual odometry is achieved through inter-frame image feature matching to complete relative positioning. In the back-end module, the aerial photography information of the UAV is matched with the satellite image to achieve absolute positioning, and then the absolute positioning information is fed back to the front-end visual odometry to construct a visual positioning system AS-VPS based on satellite image matching; The second module constructs the position as a state variable based on the FGO algorithm, represents the cost function using the residual vector, and calculates the state increment estimation using the least squares method based on the minimum of the optimal state residual; The third module establishes a CO-AS-VPS and FGO integrated model, encodes the front-end visual odometry, back-end measurement, and cooperative distance measurement as factors, and constructs a cost function to obtain the optimal estimation; where CO-AS-VPS represents a multi-UAV cooperative visual positioning system based on satellite image matching; The fourth module sets an initial value for the CO-AS-VPS and FGO integrated model and iteratively solves the optimal estimation.

7. A mobile terminal, comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that When the processor executes the program, it implements the cooperative navigation method for UAV satellite image matching based on factor graph optimization according to any one of claims 1 to 5.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the steps in the cooperative navigation method for UAV satellite image matching based on factor graph optimization according to any one of claims 1 to 5.

Images