Unmanned aerial vehicle attitude estimation method and system based on cooperative beacon and reprojection optimization

Abstract

The invention discloses an unmanned aerial vehicle attitude estimation method and system based on cooperation beacon and re-projection optimization, and the method comprises the steps: carrying out the reverse calculation of a pixel position of a camera target surface of a cooperation beacon in a current unmanned aerial vehicle attitude estimation state through a re-projection mode, and comparing the re-projection pixel position with an actual imaging pixel coordinate, calculating to obtain a re-projection error; based on the re-projection error, the attitude estimation problem is converted into a nonlinear least square optimization problem, an objective function is constructed, and a variable to be solved in the objective function is an unmanned aerial vehicle attitude estimation state vector; iteratively solving the attitude estimation state vector of the unmanned aerial vehicle by adopting an L-M algorithm, and obtaining the attitude estimation state vector of the unmanned aerial vehicle when the target function is minimum as the optimal estimation of the attitude angle of the unmanned aerial vehicle; the method has the advantages that the problem of optimal attitude angle high-precision estimation under the condition of strong magnetic interference or incapability of acquiring accurate attitude information by an attitude angle measurement sensor is effectively solved, and data support is provided for effectively improving target positioning precision.

Description

Technical Field

[0001] This invention relates to the field of UAV attitude estimation, specifically to a method and system for UAV attitude estimation based on cooperative beacon and reprojection optimization. Background Technology

[0002] Unmanned aerial vehicle (UAV) electro-optical reconnaissance platforms, with their superior situational awareness and target localization capabilities, have become crucial equipment for acquiring critical intelligence in modern warfare. Utilizing airborne visual sensors, UAVs can provide command systems with real-time, precise target coordinates, which is essential for achieving a closed-loop operational process of "observation-location-decision-strike." Target localization methods based on electro-optical sensors typically achieve this by constructing an air-to-ground coordinated coordinate transformation chain. This system uses airborne sensors to acquire UAV attitude, position, and ranging information, and combines this with the gimbal angle of the electro-optical pod and target imaging data. Through transformation relationships between multiple coordinate systems, it ultimately achieves precise visual localization of the target. In recent years, visual localization schemes based on monocular pods have attracted considerable attention due to their simple hardware, low power consumption, and ability to independently track targets.

[0003] Common vision-pod positioning methods can be mainly divided into the following two categories: (1) IMU-Vision-Based Coupled Filtering Method: This method integrates the pod encoder and the inertial measurement unit (IMU), and uses extended Kalman filtering (EKF), moving average filtering, or particle filtering for attitude estimation. For example, Chinese Patent Publication No. CN115371673A discloses a binocular camera target localization method based on Bundle Adjustment in an unknown environment. The UAV acquires airborne sensor data, including binocular image information and IMU information, and then constructs a visual inertial odometry based on the acquired sensor data. Finally, the UAV's own pose information is obtained based on the constructed visual inertial odometry. This type of method can suppress IMU drift to a certain extent. However, this method still relies on magnetic sensors to provide yaw reference, and its performance degrades significantly in battlefield environments with strong magnetic disturbances. (2) Algebraic analytical method based on pod angle-pixel observation: Under the premise that the aircraft attitude is known, the target position is calculated by intersecting the gimbal angle and image point information. This method has a small computational load, but the aircraft attitude error (especially the yaw error of more than 1°) will lead to a large ground positioning error at a height of 100 meters, which becomes the main source of system error.

[0004] Therefore, "known aircraft attitude" is a fundamental assumption of existing pod-based positioning frameworks. Traditionally, aircraft attitude is obtained through RTK-GNSS / INS integrated navigation systems, and high accuracy can be achieved by fusing carrier phase differential technology with an IMU. However, in complex battlefield environments such as those with strong magnetic interference, magnetic compasses are prone to yaw angle drift, leading to inaccurate UAV attitude acquisition and a rapid increase in positioning errors. Although lidar and stereo vision can avoid dependence on magnetic fields, their additional payload and computational requirements severely limit their application on small UAV platforms. Summary of the Invention

[0005] The technical problem to be solved by this invention is that the attitude estimation accuracy of existing UAV attitude estimation methods is low under strong magnetic interference or when the precise value is unknown.

[0006] This invention solves the above-mentioned technical problems through the following technical means: a UAV attitude estimation method based on cooperative beacon and reprojection optimization, comprising the following steps: a. The pixel positions of the pre-deployed dual cooperative beacons on the camera target surface under the current UAV attitude estimation state are obtained by back-calculating through reprojection. Then, the reprojected pixel positions are compared with the ideal imaging pixel coordinates to calculate the joint reprojection error. b. Based on the joint reprojection error, the attitude estimation problem is transformed into a nonlinear least squares optimization problem, and an objective function is constructed. The variable to be solved in the objective function is the UAV attitude estimation state vector. c. The LM algorithm is used to iteratively solve the UAV attitude estimation state vector, and the UAV attitude estimation state vector with the minimum objective function is obtained as the optimal estimate of the UAV attitude angle.

[0007] This invention treats all three-axis attitudes of the aircraft as optimizable variables and directly minimizes the reprojection error using the LM algorithm, thus fundamentally solving the problem of inaccurate three-axis attitude angle estimation for UAVs in traditional methods. Compared with existing technologies, it no longer treats the aircraft attitude as precise prior information, but as an unknown state variable to be solved, and estimates it in an integrated manner with the positioning process. This effectively reduces the positioning error caused by inaccurate attitude angle estimation. By constructing a complete transformation chain from the world coordinate system to the image pixel coordinate system, and using the single-image point reprojection error of the cooperative beacon on the camera target surface as the optimization objective, the problem is transformed into a nonlinear least squares problem, which is then efficiently solved using the LM algorithm optimization algorithm, further improving positioning accuracy.

[0008] Further, step a includes: The formula for calculating reprojection error is:

[0009] in, Indicates the number of iterations. and Indicates the first The actual pixel coordinates of the cooperative beacons projected onto the CCD through a coordinate transformation chain. ; and Indicates the first Each beacon is based on the current iterative attitude estimation state. The pixel coordinates of the lower cooperative beacon are calculated by projection through a coordinate transformation chain. The joint reprojection error vector of the two beacons is: .

[0010] Furthermore, the coordinate transformation chain is

[0011] in, This is a transformation matrix from geographic to geodetic rectangular coordinates containing latitude and longitude information. For the absolute position of the target, For the pod angle rotation matrix, This is the rotation matrix for the machine's attitude. In the image, (0, 0) represents the physical coordinates of the target point located at the center of the image in the camera coordinate system (derived from the pixel coordinates u and v of the target point combined with the homogeneous matrix of the camera intrinsic parameters). Indicates the distance from the drone to the target. For the UAV's geodetic coordinates, , , These are the pitch angle, roll angle, and yaw angle of the drone.

[0012] Furthermore, step b includes: For a single frame image, the objective function is:

[0013] Among them, coefficient It is the introduced scaling factor.

[0014] Furthermore, step c includes: Sc1. In each iteration, the LM algorithm solves the following system of linear equations to determine the parameter update amount. :

[0015] in, It is the joint reprojection error vector of the two beacons. At the current iteration point Jacobian matrix at the location, The damping factor, It is the identity matrix; Sc2, the parameter update rule is:

[0016] Sc3, Updated UAV Attitude Estimation State Substitute the values ​​into the objective function and calculate the objective function value. If the current update increases the objective function value, the iteration is considered successful, and the damping factor is reduced to one-tenth of its original value in this iteration direction. If the current update increases the objective function value, the iteration is considered unsuccessful, and the damping factor is increased to ten times its original value. The iteration returns to Sc1 and continues until the convergence condition is met. The estimated UAV attitude state corresponding to the time when the iteration stops is output as the optimal estimate of the UAV attitude angle.

[0017] Furthermore, the formula for calculating the Jacobian matrix is ​​as follows:

[0018] in, Let be the element in the m-th row and n-th column of the Jacobi matrix, where m is the component index of the error vector and n is the component index of the state vector. Let n be the unit vector of the nth component. This represents the magnitude of the m-th reprojection error component after only fine-tuning the n-th attitude angle (e.g., roll angle). e m This represents the magnitude of the m-th reprojection error component.

[0019] Furthermore, the convergence condition is that the following conditions are met simultaneously: (1) , The threshold for parameter update amount; (2) , This is the threshold for changes in the objective function value.

[0020] This invention also provides a UAV attitude estimation system based on cooperative beacon and reprojection optimization, comprising: The error calculation module is used to calculate the pixel position of the cooperative beacon on the camera target surface under the current UAV attitude estimation state by reprojection, and then compare the reprojected pixel position with the actual imaging pixel coordinates to calculate the reprojection error. The optimization objective construction module is used to transform the attitude estimation problem into a nonlinear least squares optimization problem based on the reprojection error, and to construct the objective function. The variable to be solved in the objective function is the UAV attitude estimation state vector. The optimization solution module is used for optimal estimation using LM computation.

[0021] Furthermore, the error calculation module is also used for: The formula for calculating reprojection error is:

[0022] in, Indicates the number of iterations. and Indicates the first The ideal pixel coordinates of the cooperative beacons are projected onto the CCD through a coordinate transformation chain. ; and Indicates the first Each beacon is based on the current iterative attitude estimation state. The pixel coordinates obtained by projecting the lower cooperative beacon through a coordinate transformation chain; The joint reprojection error vector of the two beacons is: .

[0023] Furthermore, the coordinate transformation chain of the target localization process is as follows:

[0024] in, This is a transformation matrix from geographic to geodetic rectangular coordinates containing latitude and longitude information. For the absolute position of the target, For the pod angle rotation matrix, This is the rotation matrix for the machine's attitude. In the image, (0, 0) represents the physical coordinates of the target point located at the center of the image in the camera coordinate system (derived from the pixel coordinates u and v of the target point combined with the homogeneous matrix of the camera intrinsic parameters). Indicates the distance from the drone to the target. For the UAV's geodetic coordinates, , , These are the pitch angle, roll angle, and yaw angle of the drone.

[0025] It is worth noting that the complete transformation chain constructed from the world coordinate system to the image pixel coordinate system is the inverse transformation calculation of the target positioning coordinate transformation chain mentioned above, so it will not be described in detail here.

[0026] Furthermore, optimizing the target building module is also used for: For a single frame image, the objective function is:

[0027] Among them, coefficient It is the introduced scaling factor.

[0028] Furthermore, the optimized solver module is also used for: Sc1. In each iteration, the LM algorithm solves the following system of linear equations to determine the parameter update amount. :

[0029] in, It is the joint reprojection error vector of the two beacons. At the current iteration point Jacobian matrix at the location, The damping factor, It is the identity matrix; Sc2, the parameter update rule is:

[0030] Sc3, Updated UAV Attitude Estimation State Substitute the values ​​into the objective function and calculate the objective function value. If the current update increases the objective function value, the iteration is considered successful, and the damping factor is reduced to one-tenth of its original value in this iteration direction. If the current update increases the objective function value, the iteration is considered unsuccessful, and the damping factor is increased to ten times its original value. The iteration returns to Sc1 and continues until the convergence condition is met. The estimated UAV attitude state corresponding to the time when the iteration stops is output as the optimal estimate of the UAV attitude angle.

[0031] Furthermore, the formula for calculating the Jacobian matrix is ​​as follows:

[0032] in, Let be the element in the m-th row and n-th column of the Jacobi matrix, where m is the component index of the error vector and n is the component index of the state vector. Let n be the unit vector of the nth component. This represents the magnitude of the m-th reprojection error component after only fine-tuning the n-th attitude angle (e.g., roll angle). e m This represents the magnitude of the m-th reprojection error component.

[0033] Furthermore, the convergence condition is that the following conditions are met simultaneously: (1) , The threshold for parameter update amount; (2) , This is the threshold for changes in the objective function value.

[0034] The advantages of this invention are: (1) This invention treats all three-axis attitudes of the aircraft as optimizable variables and uses the LM algorithm to directly minimize the reprojection error, thereby fundamentally solving the problem of inaccurate attitude error estimation. Compared with the prior art, the aircraft attitude is no longer regarded as precise input prior information, but as an unknown state variable to be solved, and is estimated in an integrated manner with the positioning process. This can effectively reduce the positioning error caused by attitude error in the subsequent target positioning calculation. By constructing a complete transformation chain from the world coordinate system to the image pixel coordinate system, and taking the single-image point reprojection error of the cooperative beacon on the camera target surface as the optimization target, the problem is transformed into a nonlinear least squares problem, and the LM algorithm is used to optimize the solution efficiently, further improving the positioning accuracy.

[0035] (2) This invention incorporates aircraft attitude, gimbal angle, camera intrinsic parameters, etc., into the optimization model by constructing a complete transformation chain from the world coordinate system to the image pixel coordinate system. A nonlinear least squares model is constructed based on the joint reprojection error of single-image points of the dual cooperative beacon, and the LM algorithm is used to achieve robust UAV attitude estimation that does not rely on high-precision initial values. The method of this invention not only effectively suppresses the coupling amplification effect of attitude errors and achieves high-precision target positioning and attitude inversion, but also provides a new approach for error control and accuracy improvement of electro-optical reconnaissance systems in complex battlefield environments. Attached Figure Description

[0036] Figure 1 This is a schematic diagram of the UAV target localization process in the UAV attitude estimation method based on cooperative beacon and reprojection optimization disclosed in the embodiments of the present invention; Figure 2 This is a schematic diagram of the coordinate transformation relationship in the target localization process of the UAV attitude estimation method based on cooperative beacon and reprojection optimization disclosed in the embodiments of the present invention; Figure 3 This is a schematic diagram of the three-axis attitude angle rotation in the UAV attitude estimation method based on cooperative beacon and reprojection optimization disclosed in the embodiments of the present invention; Figure 4 This is a schematic diagram illustrating the coupling effect between yaw angle error and various angle measurements in the UAV attitude estimation method based on cooperative beacon and reprojection optimization disclosed in the embodiments of the present invention. Figure 5 This is a flowchart of a UAV attitude estimation method based on cooperative beacon and reprojection optimization disclosed in an embodiment of the present invention. Detailed Implementation

[0037] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below in conjunction with the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0038] Example 1 To address the technical problems of existing technologies listed in the background section, this invention proposes a joint estimation method for UAV attitude and target based on coupled modeling of vision-beacon-reprojection error. By constructing a complete transformation chain from the world coordinate system to the image pixel coordinate system, the method incorporates the aircraft attitude, gimbal angle, camera intrinsic parameters, and other parameters into the optimization model. A nonlinear least squares model is constructed based on the joint reprojection error of single-image points from two beacons, and the Levenberg-Marquardt algorithm is used to achieve robust UAV attitude estimation that does not rely on high-precision initial values. To better understand the technical principles of this invention, the principle of photoelectric imaging target localization is first introduced, followed by a detailed explanation of the UAV attitude optimization estimation method based on this principle.

[0039] S1. Principle of photoelectric imaging target localization S11. Overall Structure of the Positioning System The purpose of UAV target localization is to obtain the target's three-dimensional coordinates or latitude and longitude information in a Cartesian coordinate system. Figure 1 shows a schematic diagram of the UAV electro-optical imaging localization process. During localization, electro-optical images and telemetry data are transmitted to the ground station in real time and displayed. The operator controls the flight system and electro-optical imaging equipment to search for and track the target. When the target appears within the imaging field of view, the target is stabilized and held in the center of the image. At this time, an image frame is captured, and key information such as the aircraft's attitude measurement data, the aircraft's GPS receiver information, the line-of-sight position of the electro-optical payload, and its laser rangefinder value are sent to the ground station computer. After receiving this data, the ground station computer calculates the target's three-dimensional coordinates in space, thereby achieving precise target localization.

[0040] S12, Air-Ground Cooperative Optoelectronic Imaging Target Localization Model Within the framework of spatial geometry, given the coordinates of one point and its direction and distance to another point, the three-dimensional spatial coordinates of that other point can be calculated. The rapid target localization of UAV electro-optical reconnaissance is based on this principle. This localization process requires defining the following coordinate system: Camera physical coordinate system The origin is the optical center of the camera. O C Xc The axis is the direction of the optical axis.

[0041] Photoelectric pod coordinate system The origin is the intersection of the azimuth and pitch axes of the electro-optical pod, determined by the pod's angle. α，β definition.

[0042] Body geographic coordinate system The origin is defined at the drone's center of mass, rigidly connected to the fuselage, and its attitude is determined by the pitch angle. Roll angle and yaw angle A joint decision.

[0043] Geodetic rectangular coordinate system Fixed to a global geodetic datum, the drone's location is provided by GNSS. It can be converted to and from the geodetic coordinate system (latitude and longitude coordinate system).

[0044] Once the position, attitude angle, and target imaging point of the UAV are determined, the coordinates of the target point in the Cartesian coordinate system can be calculated by combining the distance from the UAV to the target measured by the laser rangefinder. As mentioned earlier, in the locked tracking state, the target is usually stabilized at the center of the image plane, and its coordinates in the camera's physical coordinate system are (R, 0, 0).

[0045] The transformation process of the target localization calculation model is shown in Figure 2. The specific steps are as follows: 1) In the camera coordinate system, the coordinates of the target point locked at the image center are determined by the laser ranging value R. ; 2) The target point is transformed from the camera coordinate system to the electro-optical pod body axis coordinate system using the pod's azimuth angle α and pitch angle β. The pod's angular rotation matrix is: ; 3) Utilizing the drone's attitude angle ( , , The target point is transformed from the pod's body axis coordinate system to the aircraft's geographic coordinate system. The aircraft attitude rotation matrix is: ; 4) Combining the UAV's geodetic coordinates Finally, the target point is transformed into a geodetic rectangular coordinate system to obtain the target's absolute position. .

[0046] The mathematical model of this process can be simplified to the following form: (1) in, This is a transformation matrix from geographic to geodetic rectangular coordinates containing latitude and longitude information.

[0047] S13. Analysis of the Influence of UAV Attitude on Target Positioning Error 1) Theoretical model of attitude error propagation The attitude angles of a UAV define the rotational relationship between its carrier coordinate system (fixed to the airframe) and the navigation coordinate system (usually the local geographic coordinate system). For example... Figure 3 As shown, through three ordered Euler angle rotations (yaw angles) The pitch angle (θ) and roll angle (γ) allow the carrier coordinate system to coincide with the navigation coordinate system; these three Euler angles are the attitude angles of the UAV. O - X T Y T Z T Platform pod body axis coordinate system O - X G Y G Z G This is the geographic coordinate system of the organism.

[0048] The conversion relationship is as follows: (2) The UAV heading and attitude measurement system uses a variety of sensors, so various errors are inevitable. In the coordinate transformation chain of target positioning in formula (1), the accuracy of the aircraft attitude angle is one of the key factors affecting the final positioning result. The attitude measurement error will be nonlinearly transmitted and amplified to the target position calculation through the coordinate rotation matrix. In order to quantify the impact of attitude angle error on target positioning accuracy, an error propagation model is established. By taking the total differential of positioning formula (1) with respect to the attitude angle, the target position error caused by the attitude angle error can be obtained: (3) in, The symbol for total differential is . These represent the roll angles respectively. Pitch angle and yaw angle The target position error caused by the error.

[0049] Based on error theory, for formula (1), respectively... Taking the partial derivative, we have: (4) (5) (6) Since the coordinate system rotation matrix does not change the length of the space vector, we can obtain the following: (7) (8) (9) in, , , These represent the roll angle error, pitch angle error, and yaw angle error, respectively.

[0050] As can be seen from the above error analysis, in the coordinate transformation chain of target positioning (see formula (1)), the accuracy of the body attitude angle is one of the key factors affecting the final positioning result.

[0051] 2) Simulation verification analysis To investigate the mechanism by which UAV attitude affects positioning accuracy, yaw angle was used as the starting point. Taking a positioning model as an example, this study analyzes its coupling relationship with positioning measurements from other angles. Using the controlled variable method, while keeping other angle measurements constant, the positioning error is obtained by changing only the UAV's yaw angle in 0.1-degree increments. Then, while changing the UAV's yaw angle in 0.1-degree increments, the pod's pitch angle, pod's azimuth angle, UAV's roll angle, and UAV's pitch angle are individually and quantitatively changed separately, and the target positioning error is recorded for each. The final relationship between the yaw angle and the positioning error is shown in Figure 4.

[0052] Depend on Figure 4 It is evident that the positioning error increases with the increase of the yaw angle error. The magnified view shows that as other fixed angle values ​​change, the error growth curve does not completely coincide with the curve when only the yaw angle error is changed. The ratio of the positioning error deviation caused by the coupling effect to the incremental angle is defined as the incremental angle coupling coefficient. Therefore, taking the pod pitch angle as an example, for... When the change in pitch angle is 0.5°, the yaw angle coupling coefficient reaches a maximum of 0.744, verifying the state dependence of attitude angle error. Therefore, the yaw angle measurement error is coupled with the electro-optical pod's pitch angle, azimuth angle, and the UAV's three-axis attitude angles, and is influenced by other angle measurements, indicating that the impact of yaw angle error on target positioning measurement is related to the measurement state. Similarly, the relationship between the target positioning error introduced by platform roll angle error and platform pitch angle error and the measurement state can be obtained using the same method. Figure 4 As shown in equations (7) to (9), the attitude measurement error is nonlinearly transmitted and amplified to the target position calculation through the coordinate rotation matrix. This complex error coupling mechanism makes it difficult for traditional differential GNSS technology to effectively compensate for, and it must be controlled from the source of attitude estimation.

[0053] S2. UAV Attitude Optimization Estimation Method Given the strong coupling characteristics of attitude errors, traditional step-by-step compensation or linear filtering methods are difficult to handle effectively. Therefore, this invention adopts a nonlinear optimization framework to jointly optimize multiple attitude angles as a whole state vector, fundamentally solving the coupling error problem. Based on the above analysis, this invention constructs an efficient UAV attitude estimation optimization framework based on cooperative beacons. Since the position of the cooperative beacon is accurately measured in advance by military-grade high-precision positioning equipment, its pixel position on the camera target surface can be calculated backward by reprojection. Then, the reprojected pixel position is compared with the actual imaging pixel coordinates to calculate the reprojection error. Through the Levenberg-Marquardt optimization algorithm, based on known parameter conditions, continuous optimization and iteration are performed to obtain the attitude angle information with the minimum reprojection error, resulting in a higher-precision optimal estimate of the UAV attitude angle.

[0054] S21. Optimization Model Construction To overcome the estimation instability caused by insufficient constraints from a single beacon, and the operational and computational complexity of multiple beacons, this invention employs a non-collinear deployment scheme of dual cooperative beacons to construct an optimized model. This method significantly reduces the complexity of field deployment while maintaining estimation accuracy.

[0055] Define the UAV's attitude information, including pitch, roll, and yaw angles, as state vectors. .

[0056] The known parameters are the UAV position parameters. Cooperative beacon location parameters ,in, Indicates the first A cooperative beacon Parameters of the photoelectric pod, pod azimuth angle With pitch angle Visual observation parameters, and the actual pixel coordinates of the cooperative beacon on the camera target surface during iterative optimization. .

[0057] Based on the known parameters and state vectors, a dual-beacon reprojection error function is constructed to describe the difference between the estimated position and the actual observed position during iterative optimization. The pixel position of the cooperative beacon on the camera target surface is obtained by back-calculating through reprojection. Then, the reprojected pixel position is compared with the actual imaging pixel coordinates to calculate the reprojection error. For the ... For each beacon, the reprojection error at the pixel level is:

[0058] in, Indicates the number of iterations. and Indicates the first The actual pixel coordinates of each cooperative beacon are projected onto the CCD image plane through a coordinate transformation chain. and Indicates the first Each beacon is estimated based on the current iteration's attitude. The pixel coordinates of the beacon, projected through the coordinate transformation chain, are derived by reversing Equation 1, i.e., from the world coordinates of the target (on the left side of the equals sign), to calculate the pixel coordinates on the image plane. It is worth noting that in Equation (1), the target is imaged at the center of the image, and the matrix is ​​[R,0,0]. T In this method, (0,0) represents the target pixel coordinates in the physical coordinate system of the image after imaging transformation (with the origin at the image center). If the cooperative beacon is not imaged at the image center, [R,x',y'] can be obtained through a single homogeneous transformation using the camera intrinsic parameter matrix. T , where (x', y') are the physical coordinates of the target image. Therefore and The physical coordinate system of the image is obtained by inverse operation of formula (1) and then combined with homogeneous inverse transformation based on camera intrinsic parameters.

[0059] The joint reprojection error vector of the two beacons is:

[0060] S22, Nonlinear Optimization Framework Based on the reprojection error, the pose estimation problem is transformed into a nonlinear least squares optimization problem. The objective function for a single frame image is:

[0061] coefficient This scaling factor is introduced for the convenience of subsequent derivative calculations and does not affect the optimal solution. That is, to estimate the optimal attitude angle. , so that the objective function Minimum.

[0062] To address the aforementioned nonlinear least squares problem, this invention employs the Levenberg-Marquardt (LM) algorithm for efficient solution. The LM algorithm adaptively adjusts the damping factor. It smoothly transitions between the Gauss-Newton method and the steepest descent method, exhibiting good convergence and stability.

[0063] In each iteration, the algorithm solves the following system of linear equations to determine the parameter update amount. :

[0064] in: It is the joint reprojection error vector of the two beacons. At the current iteration point The Jacobian matrix at that location. The damping factor is a key parameter that controls the behavior of the algorithm. It is an identity matrix.

[0065] The parameter update rules are as follows:

[0066] The damping factor adaptive strategy is the core of the LM algorithm, and its update logic is as follows: If the current update reduces the value of the objective function, i.e. If the iteration is successful, the iteration factor is reduced in this iteration direction. This makes the algorithm behave more like the Gauss-Newton method, taking advantage of its fast local convergence. If the current update increases the objective function value, the iteration is considered to have failed, and the damping factor is increased. This makes the algorithm behave more like the steepest descent method, thereby enhancing stability and ensuring global convergence.

[0067] To avoid tedious analytical differentiation, a numerical differentiation method is used to calculate the Jacobian matrix, and its formula is as follows:

[0068] in, Let be the element in the m-th row and n-th column of the Jacobi matrix, where m is the component index of the error vector (m = 1, 2, ..., 2N, where N is the number of beacons), and n is the component index of the state vector (n = 1, 2, 3 corresponding to pitch, yaw, and roll). For small perturbation step size, It is the unit vector of the nth component. This represents the magnitude of the m-th reprojection error component after only fine-tuning the n-th attitude angle (e.g., roll angle). e m This represents the magnitude of the m-th reprojection error component.

[0069] An algorithm is considered convergent when it simultaneously satisfies the following conditions: (1) The parameter update norm is less than the threshold: In this embodiment, This threshold corresponds to approximately The angle change is far lower than the measurement accuracy of actual sensors.

[0070] (2) The change in the objective function value is less than the threshold: This embodiment takes This threshold is far below the resolution of the image sensor, ensuring that further optimization of the reprojection error is of no practical significance.

[0071] This optimization framework transforms the UAV attitude estimation problem into a numerical optimization problem. Through iterative adjustments to the attitude angles, it ultimately drives the reprojection error to converge to a global minimum, thus obtaining high-precision attitude estimation results. The solution process of this method is as follows: Figure 5 As shown.

[0072] Compared to traditional UAV electro-optical positioning methods, this invention represents a fundamental technological innovation and breakthrough. Traditional methods are generally based on the core assumption of "known aircraft attitude," and their positioning process is an "open-loop," chain-like geographic coordinate transformation process. They heavily rely on a high-precision GNSS / INS integrated navigation system to provide attitude references. In particular, yaw angle information is easily affected by the inaccuracy of the magnetic compass in complex battlefield environments, causing attitude errors to be non-linearly transmitted and amplified through the rotation matrix, becoming one of the main sources of positioning errors.

[0073] To address this core challenge, this invention abandons the traditional step-by-step solution model and proposes a novel joint optimization estimation method based on coupled modeling of cooperative beacons and reprojection errors. The innovation lies in its approach: it no longer treats the aircraft attitude as precise prior input information, but rather as an unknown state variable to be solved, estimating it integrally with the localization process. By constructing a complete inverse transformation chain from the world coordinate system to the image pixel coordinate system, and using the joint reprojection error of single-image points on the camera target surface of the two cooperative beacons as the optimization objective, the problem is transformed into a nonlinear least squares problem.

[0074] In terms of technical means, this invention employs the Levenberg-Marquardt (LM) optimization algorithm for efficient solution. This algorithm can adaptively adjust the iteration step size and robustly converge to the vicinity of the global optimum even without explicit attitude information, thereby simultaneously outputting high-precision target position and UAV attitude angle. This process essentially achieves active estimation and self-compensation of multi-source system errors such as airframe attitude error and pod angle error, fundamentally cutting off the path of error coupling amplification.

[0075] Ultimately, the technical benefits of this invention are reflected in the synergistic improvement of accuracy and robustness. It significantly reduces reliance on airborne high-precision attitude sensors, enabling the system to maintain excellent positioning performance even in harsh environments such as those with strong magnetic interference. This approach of "compensating for hardware shortcomings with software algorithms" not only overcomes the industry pain point of poor reliability in complex scenarios using traditional solutions, but also opens up a completely new technical path for small unmanned aerial vehicle (UAV) platforms to achieve low-cost, high-precision reconnaissance and positioning.

[0076] Example 2 Based on Embodiment 1, Embodiment 2 of the present invention also provides a UAV attitude estimation system based on cooperative beacon and reprojection optimization, including: The error calculation module is used to calculate the pixel position of the pre-deployed dual cooperative beacons on the camera target surface under the current UAV attitude estimation state by reprojection. Then, the reprojected pixel position is compared with the ideal imaging pixel coordinates to calculate the joint reprojection error. The optimization objective construction module is used to transform the attitude estimation problem into a nonlinear least squares optimization problem based on the joint reprojection error, and to construct the objective function. The variable to be solved in the objective function is the UAV attitude estimation state vector. The optimization and solution module is used to iteratively solve the UAV attitude estimation state vector using the LM algorithm, and obtain the UAV attitude estimation state vector with the minimum objective function as the optimal estimate of the UAV attitude angle.

[0077] Specifically, the error calculation module is also used for: The formula for calculating reprojection error is:

[0078] in, Indicates the number of iterations. and Indicates the first The actual projected pixel coordinates of each cooperative beacon onto the CCD image plane are projected through a coordinate transformation chain. ; and Indicates the first Each beacon is based on the current iterative attitude estimation state. The pixel coordinates of the lower cooperative beacon are calculated by projection through a coordinate transformation chain. The joint reprojection error vector of the two beacons is: .

[0079] More specifically, the coordinate transformation chain is

[0080] in, This is a transformation matrix from geographic to geodetic rectangular coordinates containing latitude and longitude information. For the absolute position of the target, For the pod angle rotation matrix, This is the rotation matrix for the machine's attitude. In the image, (0, 0) represents the physical coordinates of the target point located at the center of the image in the camera coordinate system (derived from the pixel coordinates u and v of the target point combined with the homogeneous matrix of the camera intrinsic parameters). Indicates the distance from the drone to the target. For the UAV's geodetic coordinates, , , These are the pitch angle, roll angle, and yaw angle of the drone.

[0081] More specifically, optimizing the target building module is also used for: For a single frame image, the objective function is:

[0082] Among them, coefficient It is the introduced scaling factor.

[0083] More specifically, the optimized solver module is also used for: Sc1. In each iteration, the LM algorithm solves the following system of linear equations to determine the parameter update amount. :

[0084] in, It is the joint reprojection error vector of the two beacons. At the current iteration point Jacobian matrix at the location, The damping factor, It is the identity matrix; Sc2, the parameter update rule is:

[0085] Sc3, Updated UAV Attitude Estimation State Substitute the values ​​into the objective function and calculate the objective function value. If the current update increases the objective function value, the iteration is considered successful, and the damping factor is reduced to one-tenth of its original value in this iteration direction. If the current update increases the objective function value, the iteration is considered unsuccessful, and the damping factor is increased to ten times its original value. The iteration returns to Sc1 and continues until the convergence condition is met. The estimated UAV attitude state corresponding to the time when the iteration stops is output as the optimal estimate of the UAV attitude angle.

[0086] More specifically, the formula for calculating the Jacobian matrix is:

[0087] in, Let be the element in the m-th row and n-th column of the Jacobi matrix, where m is the component index of the error vector and n is the component index of the state vector. Let n be the unit vector of the nth component. This represents the magnitude of the m-th reprojection error component after only fine-tuning the n-th attitude angle (e.g., roll angle). e mThis represents the magnitude of the m-th reprojection error component.

[0088] More specifically, the convergence condition is that the following conditions are met simultaneously: (1) , The threshold for parameter update amount; (2) , This is the threshold for changes in the objective function value.

[0089] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A UAV attitude estimation method based on cooperative beacon and reprojection optimization, characterized in that, Includes the following steps: a. The pixel positions of the pre-deployed dual cooperative beacons on the camera target surface under the current UAV attitude estimation state are obtained by back-calculating through reprojection. Then, the reprojected pixel positions are compared with the ideal imaging pixel coordinates to calculate the joint reprojection error. b. Based on the joint reprojection error, the attitude estimation problem is transformed into a nonlinear least squares optimization problem, and an objective function is constructed. The variable to be solved in the objective function is the UAV attitude estimation state vector. c. The LM algorithm is used to iteratively solve the UAV attitude estimation state vector, and the UAV attitude estimation state vector with the minimum objective function is obtained as the optimal estimate of the UAV attitude angle.

2. The UAV attitude estimation method based on cooperative beacon and reprojection optimization according to claim 1, characterized in that, Step a includes: The formula for calculating reprojection error is as follows: in, Indicates the number of iterations. and Indicates the first The actual pixel coordinates of the cooperative beacons projected onto the CCD through a coordinate transformation chain. ; and Indicates the first Each beacon is based on the current iterative attitude estimation state. The pixel coordinates of the lower cooperative beacon are calculated by projection through a coordinate transformation chain. The joint reprojection error vector of the two beacons is: 。 3. The UAV attitude estimation method based on cooperative beacon and reprojection optimization according to claim 2, characterized in that, The coordinate transformation chain for the target localization process is as follows: in, This is a transformation matrix from geographic to geodetic rectangular coordinates containing latitude and longitude information. For the absolute position of the target, For the pod angle rotation matrix, This is the body attitude rotation matrix. In the image, (0, 0) represents the physical coordinates of the target point located at the center of the image in the camera coordinate system. Indicates the distance from the drone to the target. For the UAV's geodetic coordinates, , , These are the pitch angle, roll angle, and yaw angle of the drone.

4. The UAV attitude estimation method based on cooperative beacon and reprojection optimization according to claim 2, characterized in that, Step b includes: For a single frame image, the objective function is: Among them, coefficient It is the introduced scaling factor.

5. The UAV attitude estimation method based on cooperative beacon and reprojection optimization according to claim 4, characterized in that, Step c includes: Sc1. In each iteration, the LM algorithm solves the following system of linear equations to determine the parameter update amount. : in, It is the joint reprojection error vector of the two beacons. At the current iteration point Jacobian matrix at the location, The damping factor, It is the identity matrix; Sc2, the parameter update rule is: Sc3, Updated UAV Attitude Estimation State Substitute the values ​​into the objective function and calculate the objective function value. If the current update increases the objective function value, the iteration is considered successful, and the damping factor is reduced to one-tenth of its original value in this iteration direction. If the current update increases the objective function value, the iteration is considered unsuccessful, and the damping factor is increased to ten times its original value. The iteration returns to Sc1 and continues until the convergence condition is met. The estimated UAV attitude state corresponding to the time when the iteration stops is output as the optimal estimate of the UAV attitude angle.

6. The UAV attitude estimation method based on cooperative beacon and reprojection optimization according to claim 5, characterized in that, The formula for calculating the Jacobian matrix is ​​as follows: in, Let be the element in the m-th row and n-th column of the Jacobi matrix, where m is the component index of the error vector and n is the component index of the state vector. Let n be the unit vector of the nth component. This indicates the magnitude of the m-th reprojection error component after adjusting only the n-th attitude angle. e m This represents the magnitude of the m-th reprojection error component.

7. The UAV attitude estimation method based on cooperative beacon and reprojection optimization according to claim 6, characterized in that, The convergence condition is that the following conditions must be met simultaneously: (1) , The threshold for parameter update amount; (2) , This is the threshold for changes in the objective function value.

8. A UAV attitude estimation system based on cooperative beacon and reprojection optimization, characterized in that, include: The error calculation module is used to calculate the pixel position of the pre-deployed dual cooperative beacons on the camera target surface under the current UAV attitude estimation state by reprojection. Then, the reprojected pixel position is compared with the ideal imaging pixel coordinates to calculate the joint reprojection error. The optimization objective construction module is used to transform the attitude estimation problem into a nonlinear least squares optimization problem based on the joint reprojection error, and to construct the objective function. The variable to be solved in the objective function is the UAV attitude estimation state vector. The optimization and solution module is used to iteratively solve the UAV attitude estimation state vector using the LM algorithm, and obtain the UAV attitude estimation state vector with the minimum objective function as the optimal estimate of the UAV attitude angle.

9. The UAV attitude estimation system based on cooperative beacon and reprojection optimization according to claim 8, characterized in that, The error calculation module is also used for: The formula for calculating reprojection error is as follows: in, Indicates the number of iterations. and Indicates the first The actual pixel coordinates of the cooperative beacons projected onto the CCD through a coordinate transformation chain. ; and Indicates the first Each beacon is based on the current iterative attitude estimation state. The pixel coordinates of the lower cooperative beacon are calculated by projection through a coordinate transformation chain. The joint reprojection error vector of the two beacons is: 。 10. The UAV attitude estimation system based on cooperative beacon and reprojection optimization according to claim 9, characterized in that, The coordinate transformation chain for the target localization process is as follows: in, This is a transformation matrix from geographic to geodetic rectangular coordinates containing latitude and longitude information. For the absolute position of the target, For the pod angle rotation matrix, This is the body attitude rotation matrix. In the image, (0, 0) represents the physical coordinates of the target point located at the center of the image in the camera coordinate system. Indicates the distance from the drone to the target. For the UAV's geodetic coordinates, , , These are the pitch angle, roll angle, and yaw angle of the drone.

Images