Cluster cooperative navigation method based on Lie group EKF and SW verifiable optimization

Abstract

This invention discloses a swarm cooperative navigation method based on a verifiable optimization of the Lie group EKF and SW, employing a front-end-back-end coupled architecture. At the front end, each UAV independently runs a Lie group right-invariant extended Kalman filter for state prediction, and updates its state by fusing ranging observations from other UAVs and / or external position observations fed back from the back end, obtaining the odometry trajectory of each UAV. At the back end, the odometry trajectories output from all UAVs' front ends and the ranging data between UAVs are received. A quadratic constrained quadratic programming problem is constructed and solved within a fixed-size sliding window to optimize the poses of all UAVs within the sliding window. Simultaneously, the optimization results from the back end are fed back to the Lie group right-invariant extended Kalman filter as external position observations. This invention is applicable to the navigation field, enabling high-precision real-time cooperative navigation in large-scale, anchor-free outdoor scenarios.

Description

Technical Field

[0001] This invention relates to the field of navigation technology, specifically a cluster cooperative navigation method based on Lie group EKF and SW verifiable optimization. Background Technology

[0002] Multi-UAV cooperative navigation technology has broad application prospects in search and rescue, inspection, and logistics delivery. Inertial Measurement Units (IMUs), as the core sensors for autonomous navigation, calculate UAV attitude and position through integrated angular velocity and linear acceleration. However, noise and bias in IMU measurements can cause positioning errors to accumulate over time, resulting in unbounded drift. Cooperative navigation, by introducing relative measurements between UAVs (such as communication ranging), constrains the trajectory drift of each UAV, and is an effective way to solve this problem. Range-Aided SLAM (Range-Assisted Simultaneous Localization and Mapping) technology has received widespread attention in the field of robot navigation in recent years, with applications covering various environments including underwater, aerial, underground, and planetary exploration.

[0003] In outdoor environments where visual sensors are unavailable, GNSS is denied, and there are no anchor points (such as in swarm cross-strait scenarios), multi-UAV collaborative navigation and positioning faces severe challenges. These scenarios are characterized by: (1) visual sensors being unavailable due to weather, lighting, or other factors; (2) GNSS signals being interfered with or blocked, unable to provide absolute positioning information; and (3) the absence of pre-set anchor points for reference. In such scenarios, UAVs can only rely on the relative ranging information between the IMU and the UAVs for collaborative positioning.

[0004] Currently, existing technical solutions for the above scenarios mainly include the following categories: Option 1: Traditional Extended Kalman Filter (EKF) Co-localization Method. This method uses a 15-dimensional state vector (including attitude, velocity, position, and IMU bias) to perform co-localization by fusing IMU and ranging information through EKF. Representative works include the polar coordinate EKF method and the distributed EKF co-localization algorithm based on UWB. This method has high computational efficiency, but it has the following problems: (1) There is a singularity problem in the rotation parameterization in the traditional EKF; (2) The processing of nonlinear ranging observations is not optimal; (3) There is a lack of systematic modeling of the clock error of the communication ranging module, which limits the positioning accuracy in large-scale scenarios.

[0005] Option 2: Filtering methods based on Lie group EKF, such as the right-invariant extended Kalman filter framework based on Lie group SE(3). This method avoids the singularity problem caused by rotation parameterization in traditional EKF by directly defining the error state on the Lie group manifold, while ensuring the right-invariant property of the error state being independent of the reference coordinate system. Lie group EKF has shown good convergence and robustness in scenarios such as UAV pose estimation and ground vehicle localization. However, this framework is mainly designed for single-carrier applications, and its distributed implementation and consistency guarantee in multi-UAV collaborative scenarios still need further research, and it does not consider the clock error modeling of the communication ranging module.

[0006] Option 3: Sliding window optimization based on nonlinear least squares. Represented by VINS-Mono and GTSAM, this method compresses historical information through sliding window optimization and edge detection techniques, controlling the computational complexity to O(W) (where W is the window size). VINS-Mono successfully applied sliding window optimization to visual inertial navigation systems, achieving excellent performance in mobile robot navigation. While filtering optimization hybrid methods such as MSCKF improve computational efficiency, these methods all face the problem of local optima, only obtaining locally optimal solutions and lacking guarantees of global optimality.

[0007] Option 4: Verifiable Batch Optimization Method (CORA). For example, the existing CORA (Certifiably Correct Range-Aided SLAM) framework formalizes the range-aided SLAM problem using quadratic constraint quadratic programming (QCQP), transforms it into semidefinite programming (SDP) using Shor's relaxation technique, and efficiently solves it using the Riemann echelon method. Its core advantage lies in handling non-convex constraints through semidefinite programming relaxation, providing an Optimality Certificate to verify whether the current solution is globally optimal. Theoretically, it has been proven that when the certificate matrix satisfies positive semidefiniteness, the current solution is globally optimal. However, CORA processes the entire trajectory in batches, resulting in computational complexity that increases linearly with trajectory length, lacking real-time guarantees and making it difficult to apply to long-running multi-UAV systems. Furthermore, this method does not consider clock error modeling of the ranging module, limiting positioning accuracy in large-scale scenarios.

[0008] Based on the above analysis, the existing technology has the following drawbacks: (1) The traditional EKF method is not optimal in handling nonlinear ranging observations and lacks systematic modeling of ranging errors. The traditional 15-dimensional EKF does not include clock bias and clock drift rate in state estimation. In large-scale scenarios, the clock drift error of the communication ranging module can reach 3-15 m / s and accumulates over time, which seriously affects the positioning accuracy.

[0009] (2) Existing ranging-assisted SLAM methods are mostly limited to small-scale environments, rely on visual information and UWB ranging devices, and fail in large-scale scenarios where vision is unavailable. The effective distance of traditional UWB ranging modules is usually less than 100 meters, which cannot meet the needs of large-scale multi-UAV collaborative applications.

[0010] (3) Existing sliding window optimization methods (such as GTSAM and VINS-Mono) can only obtain local optimal solutions and lack global optimality guarantees. These methods are based on nonlinear least squares, which can only guarantee local optima in essence and cannot verify the quality of the solution. They are prone to getting trapped in local optima when the initialization is poor or the observation constraints are insufficient.

[0011] (4) Batch processing verifiable optimization methods (such as CORA) have high computational complexity and are difficult to apply to real-time multi-UAV cooperative navigation systems. CORA's batch processing method requires processing the complete trajectory, and the dimension of the optimization variables increases linearly with the trajectory length, which cannot meet the real-time requirements. Summary of the Invention

[0012] To address the shortcomings of the existing technologies, this invention provides a cluster cooperative navigation method based on Lie group EKF and SW verifiable optimization, which can achieve high-precision real-time cooperative navigation in large-scale outdoor anchor-free scenarios.

[0013] To achieve the above objectives, this invention provides a cluster cooperative navigation method based on Lie group EKF and SW verifiable optimization, which adopts a front-end-back-end coupled architecture; At the front end, each UAV independently runs a Lie group right-invariant extended Kalman filter to predict its state, and integrates communication ranging observations with other UAVs and / or external position observations fed back from the back end to update its state, thus obtaining the odometry trajectory of each UAV. At the backend, the odometry trajectories and distance measurement data between all UAVs output from the frontend are received. A quadratic constrained quadratic programming problem is constructed and solved within a fixed-size sliding window to optimize the pose of all UAVs within the sliding window. At the same time, the optimization results obtained from the backend are fed back to the Lie group right-invariant extended Kalman filter as external position observations.

[0014] Compared with the prior art, the present invention has the following beneficial technical effects: 1. This invention applies the Lie group right-invariant error EKF framework to multi-UAV ranging cooperative navigation, and designs a 17-dimensional extended Kalman filter method with state vectors. By directly defining the error state on the Lie group manifold, it avoids the singularity problem caused by rotation parameterization in the traditional EKF, while ensuring the right-invariant property of the error state being independent of the reference coordinate system. 2. This invention is the first to introduce the semidefinite programming relaxation and optimality verification mechanism of the CORA framework into sliding window optimization, breaking through the limitations of traditional sliding window optimization methods that can only obtain local optimal solutions, and the limitations of the CORA method that only supports offline computation. By performing edge-mapping operations at the window points, historical information is compressed into prior terms and passed to the next window, achieving effective utilization of historical information while ensuring real-time performance. Furthermore, window-level optimality verification is performed through a certificate matrix, realizing high-precision real-time positioning for multi-UAV cooperative navigation in large-scale outdoor anchor-free scenarios. Attached Figure Description

[0015] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the structures shown in these drawings without creative effort.

[0016] Figure 1 This is a framework diagram of a cluster cooperative navigation method based on verifiable optimization of Lie group EKF and SW in an embodiment of the present invention; Figure 2 This is a schematic diagram of the sensor parameter settings in the simulation experiment of this invention. Figure 3 This is a schematic diagram comparing the trajectories of UAV 1 in an embodiment of the present invention; Figure 4 This is a schematic diagram comparing the trajectories of UAV 2 in an embodiment of the present invention; Figure 5 This is a schematic diagram comparing the trajectories of three drones in an embodiment of the present invention; Figure 6 This is a schematic diagram comparing the four trajectories of the UAV in an embodiment of the present invention; Figure 7 This is a schematic diagram illustrating the change of the RMSE of the UAV 1 position over time in an embodiment of the present invention; Figure 8 This is a schematic diagram illustrating the change of RMSE of the UAV 2 position over time in an embodiment of the present invention; Figure 9 This is a schematic diagram illustrating the change of RMSE at position 3 of the UAV over time in an embodiment of the present invention; Figure 10 This is a schematic diagram illustrating the change of RMSE at position 4 of the UAV over time in an embodiment of the present invention; Figure 11 This is a schematic diagram comparing the average position RMSE of all drones in this embodiment of the invention.

[0017] The realization of the objective, functional features and advantages of the present invention will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0018] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present invention.

[0019] Furthermore, the technical solutions of the various embodiments of the present invention can be combined with each other, but only if they are feasible for those skilled in the art. If the combination of technical solutions is contradictory or cannot be implemented, it should be considered that such combination of technical solutions does not exist and is not within the scope of protection claimed by the present invention.

[0020] This embodiment discloses a cluster cooperative navigation method (LG_SW_CORA) based on Lie group EKF and SW verifiable optimization. It adopts a front-end-back-end coupled architecture, consisting of a Lie group right-invariant error EKF (LG-EKF-R) front-end and a sliding window verifiable optimization (SW_CORA) back-end. The front-end provides real-time state estimation, while the back-end performs cooperative optimization to refine the trajectory within a sliding window. An edge-shifting mechanism is used to effectively utilize historical information and control bounded computational complexity. The optimization results are passed to the front-end through a feedback correction mechanism. The back-end does not process IMU pre-integration measurements but directly uses the odometry information output by the front-end to avoid information duplication and covariance underestimation. (Reference) Figure 1 The UAV cooperative navigation method in this embodiment specifically includes: At the front end, each UAV independently runs a distributed Lie group right-invariant extended Kalman filter for state prediction, and integrates high-frequency IMU data (200Hz), communication ranging observations with other UAVs, and / or external position observations fed back from the back end for state updates, to obtain the odometry trajectory of each UAV. At the back end, the odometry trajectories and distance measurement data between all UAVs are received from the front end of all UAVs. A quadratic constrained quadratic programming problem is constructed and solved within a fixed-size sliding window to optimize the pose of all UAVs within the sliding window. At the same time, the optimization results obtained from the back end are fed back to the Lie group right-invariant extended Kalman filter as external position observations.

[0021] In this embodiment, the communication ranging observation between UAVs is obtained by an integrated communication ranging module. The integrated communication ranging module is based on the bilateral two-way ranging (ALTDS-TWR) protocol and has the following advantages compared with the traditional UWB ranging module: (1) the effective ranging distance is greater than 5km, which is suitable for large-scale multi-UAV collaborative application scenarios; (2) the integrated communication ranging design reduces the weight of the payload and power consumption; (3) the module has autonomous clock synchronization capability, which supports the synchronous ranging and data communication between multiple UAVs.

[0022] In this embodiment, the ranging error of the integrated communication and ranging module mainly comes from three aspects: clock drift error, thermal noise, and multipath effect. Clock drift is the most significant source of error for the integrated communication and ranging module. The stability of the device's crystal oscillator frequency determines the upper limit of ranging accuracy. The typical crystal oscillator drift rate is 10-50 ppm, corresponding to a distance error of 3-15 meters / second, which accumulates over time. Assuming a drone... The clock reading is ,in For real time, To account for equivalent clock skew, in a bilateral two-way ranging protocol, the UAV... and drones The timing relationship for exchanging ranging messages is as follows: ; in, , For drones The local transmission time when the first ranging request message is sent. , For drones Received drone The local reception time of the first ranging request message sent. , For drones The local sending time for sending a reply message. , For drones The local reception time when this reply is received. For drones The actual moment the first ranging request message was sent. For message flight time, For drones Response delay; Through derivation, the residual drift error after removing the fixed clock bias is: ; in, m / s is the speed of electromagnetic wave propagation; Equivalent clock bias It satisfies the random walk model, that is: ; in, To drive noise, Clock drift rate (unit: seconds / second), clock drift rate It also changes slowly over time, satisfying the following: ; in, This is the process noise representing the rate of change of clock drift. Power-on error is treated as an initial fixed component of the equivalent clock skew, i.e. Includes power-on error The impact.

[0023] Radio signals may travel along multiple paths to reach the receiver, leading to ranging errors. Multipath effects can cause errors of 1-20 meters in severe multipath scenarios such as indoor environments and urban areas. Multipath errors exhibit non-Gaussian properties, making them difficult to accurately model using simple probability distributions. In real-world open outdoor environments, multipath effects are relatively weak, and some outliers can be eliminated based on filter predictions. Based on the above error analysis, UAVs... With drones The communication ranging measurement model between them is as follows: ; in, For drones With drones Position vector in the navigation coordinate system Thermal noise, This is a multipath error, which can be ignored or approximated by a Gaussian in outdoor environments. After approximating the multipath error as a Gaussian distribution, the ranging observation noise can be uniformly expressed as: ; in, To reduce noise in ranging observations, This represents the variance of the distance measurement observation noise.

[0024] In this embodiment, the state vector of the Lie group right-invariant extended Kalman filter is 17-dimensional, including Lie group pose error components, IMU bias error components, and clock error components. The Lie group pose error components include attitude error, velocity error, and position error; the IMU bias error components include gyroscope bias error and accelerometer bias error; and the clock error components include equivalent clock skew and clock drift rate. That is, the UAV... 17-dimensional error state vector Defined as: ; in, For attitude error, specifically, attitude estimation error using axis angle parameterization, satisfying... , This is an estimate of the attitude matrix. It is the identity matrix. The true value of the attitude matrix; The velocity error, specifically the velocity disturbance component in the right-invariant error form of the Lie group, satisfies... , For drones Speed ​​error; The position error is specifically the position disturbance component in the right-invariant error form of the Lie group, which satisfies... , For drones Position error; This refers to the gyroscope bias error, i.e. , This is an estimate of the gyroscope bias. This is the actual value of the gyroscope bias; This is the accelerometer bias error, i.e. , This is an estimate of the accelerometer bias error. This represents the true value of the accelerometer bias error; The equivalent clock bias includes the combined effects of fixed bias and drift accumulation; This refers to the clock drift rate, which is the relative deviation of the clock frequency. Regarding the state The right-invariant error is defined as This definition ensures that the distribution of error states in the state space is independent of the reference coordinate system, thus improving the convergence and numerical stability of the filter.

[0025] In the right-invariant extended Kalman filter with Lie group, the error dynamics equation after the extended state is: ; Extended error dynamics matrix Extended noise driving matrix , Extended process noise vector They are respectively: ; ; ; in, This is the projection of the Earth's rotational angular velocity onto coordinate system e. This is the projection of gravitational acceleration onto coordinate system e. and For drones The projection of the estimated velocity and position values ​​onto coordinate system e. , , , , These are gyroscope white noise, accelerometer white noise, gyroscope bias process noise, accelerometer bias process noise, and clock drift rate process noise, respectively.

[0026] In the Ligun right-invariant extended Kalman filter, the drone With drones The distance measurement equation between them is: ; in, , For drones drones The estimated position vector, For ranging, use Gaussian white noise; Linearize the ranging observation equation at the estimated state and define the unit line-of-sight vector. You can get Observation residuals ,for: ; in, , For drones drones Position error, The speed of electromagnetic wave propagation. , For drones drones Estimation error of equivalent clock offset; The position error is expressed in right-invariant error form. , The observation matrix can then be obtained. , for: ; ; in, It is a zero vector. For the left Jacobian matrix of the Lie group, , For drones drones The positional disturbance component. , For drones drones The attitude error.

[0027] In this embodiment, each UAV independently performs state prediction and update based on a distributed Lie group right-invariant extended Kalman filter. The state prediction process is as follows: ; in, For drones exist Prior state estimation at time 10:00 For drones The discrete state transition matrix, For drones exist Posterior state estimation at time 10:00. for The corresponding prediction covariance matrix, for The corresponding prediction covariance matrix, For drones The discrete process noise covariance matrix, This is the transpose of the matrix; During the update process, the drone In broadcast mode And receive neighbor's drone The information, and calculate the observation residuals. and observation matrix , Then, the CI (Conservative Inference) method is used to handle state correlations and perform fusion updates, specifically as follows: ; ; ; ; in, The residual covariance matrix is ​​based on CI fusion. For CI, the optimal weights For drones The observation matrix of its own state, For drones The observation matrix of its own state, For drones Neighbor Drone State prediction covariance matrix For drones With drones The noise covariance matrix of the distance measurement observations between them. For drones The Kalman gain matrix, For drones exist Posterior state estimation at time 10:00. for The corresponding prediction covariance matrix, It is the identity matrix; After the error state is updated, attitude, velocity, and position can be synchronized using the Lie group state update formula, which is: ; in, , , For the updated attitude, velocity, and position.

[0028] In this embodiment, the core idea of ​​the sliding window verifiable optimization (SW_CORA) backend is to transform the entire process of "QCQP formulation - SDP relaxation - Riemann echelon solution - certificate matrix authentication" of the CORA framework from offline batch processing to an online fixed lag estimation framework. While ensuring real-time performance, it inherits the optimality verification capability of CORA, mainly including the following four points: (1) Windowed state management: Only the K most recent state blocks are retained in the active optimization window, with a fixed hysteresis length of K; (2) Half-window overlap mechanism: Each time the window is updated, the oldest K / 2 state blocks are marginalized, and the newest K / 2 state blocks are retained as the overlapping region with the next window. (3) Marginalization at the QCQP level: By performing marginalization operations at the level of quadratic constraint quadratic programming (QCQP), historical information is compressed into prior terms and passed to the next window, instead of operating at the level of SDP or Riemann ladder, thus fully preserving the structural characteristics of CORA. (4) Window-level optimality verification: The optimization problem within each window still maintains the QCQP form, and Shor's type SDP relaxation can be used, and posterior optimality verification is performed through the certificate matrix.

[0029] In this embodiment, the optimization variables of the quadratic constrained quadratic programming problem include position variables and an auxiliary unit vector introduced by distance measurement. Each pose variable is defined as... ,in for 3D rotation matrix (when It is a three-dimensional rotation. (a special orthogonal group) for 3D translation vector. Simultaneously, using... Denotes the auxiliary unit vector introduced by distance measurement, where for 3D spherical manifold.

[0030] The goal of backend sliding window optimization is to find an optimal set of pose and auxiliary variables to minimize the measurement residual cost function. The standard CORA QCQP objective function is defined as follows: ; in, These are the weight parameters for the relatively rotated edges. The values ​​are relative rotation measurements. For the set of relative pose edges, The weight parameters for the relative translation edges, The measured value is the relative translation. The standard deviation of the distance to the side. This is the distance measurement from the edge. Let the set of edges be relative distances. Describing the Frobenius norm, Represents the Euclidean norm; Backend sliding window optimization needs to satisfy two types of quadratic equality constraints: ,in It is a d-dimensional identity matrix.

[0031] In this embodiment, after optimizing a sliding window, the earliest partial states in the sliding window are removed from the active optimization variables through a marginalization operation, and the information carried by the marginalized states is compressed into a secondary prior term and passed to the optimization problem of the next sliding window.

[0032] In the specific implementation process, the state division and scheduling rules of the sliding window are set as follows: Window size and shift step: Let the active window size be... This represents the total number of state blocks contained in each window; the window shift step is defined as: That is, each time the window is updated, it is panned. A state block ensures that there are states between adjacent windows. The overlapping area of ​​state blocks; Window state set partitioning: dividing the window Divided into two mutually exclusive subsets, (1) the oldest to be marginalized Marginalized set of states (2) The latest version to be retained Stitched / overlap set ; Measurement factor processing rules: (1) Internal factor: The variable associated with the measurement factor belongs entirely to the internal variable family. (2) Coupling factor: The measurement factors are simultaneously associated. and The variables within. During the marginalization process, the information of this factor is absorbed and transformed into effects on... (3) Retained factor: The variable associated with the measurement factor belongs entirely to the prior terms; The factor remains active in the next window; (4) New factor: The measurement factor involves newly added state blocks, which are explicitly added when the next window is constructed.

[0033] Considering the optimization problem of compressing the information carried by the marginalized state into a quadratic prior and passing it to the next sliding window, this embodiment uses the objective function of the quadratic constraint quadratic programming problem. Set to: ; in, For the first The sum of the costs of pose measurement residuals and distance measurement residuals explicitly represented in each sliding window. For from the first The window is passed to the first The secondary priors of each window; The QCQP form of a sliding window is: The quadratic equality constraint remains unchanged, that is: .in, To optimize variables, It is a symmetric quadratic matrix that contains measurement cost and prior cost. For a linear term vector, For constant terms, For the first A constrained symmetric matrix, Let be the total number of constraints for the s-th window; In this embodiment, a semidefinite programming relaxation technique is used to handle the non-convex constraints of the quadratic programming problem, and the Riemann step method is used to solve it, thus obtaining candidate solutions. With dual variables Based on this, and using dual variables Construct and define the certificate matrix ,for: ; in, For the first A sliding window optimizes the dimensions of the variables; if (Positive semidefinite), then candidate solutions This represents the global optimal solution to a quadratic constrained quadratic programming problem.

[0034] In the optimization problem of compressing the information carried by the marginalized state into a quadratic prior and passing it to the next sliding window, the quadratic prior is specifically: ; ; in, It is a second-order prior. This refers to the state of the overlapping region within the current sliding window. The optimal solution for the overlapping region state after optimization of the previous sliding window. For the environmental space prior matrix, The marginal Schur complement matrix is ​​calculated according to the standard Schur method. Let be the constraint matrix of the tangent space basis matrix in the overlapping region. This indicates the Moore-Penrose pseudo-inverse.

[0035] The merged complete QCQP formal parameter update is as follows: ; ; ; in, For the merged first The quadratic cost matrix of each window. This is the quadratic coefficient matrix of the measurement terms. To select the matrix (extract the overlapping state from the window variable). For the merged first The linear term coefficient vector of each window, The linear coefficient vector of the measurement terms. For the merged first The constant term for each window, This is the constant part of the measurement term.

[0036] In this embodiment, the sliding window CORA problem retains the exact same QCQP structure as the batch CORA problem, only summarizing and passing marginalized historical information through rolling quadratic priors. Therefore, each window can still use Shor's type SDP relaxation, the Riemann echelon solution technique is fully applicable, and the authentication logic of the certificate matrix does not need to be modified. The solution to the problem can be obtained using the standard Shor's relaxation Riemann echelon solution, and the certificate matrix is ​​used to verify whether the solution is optimal. If it is not optimal, homogeneous boosting variables are introduced for further iteration.

[0037] In the LG_SW_CORA ensemble framework of this embodiment, feedback correction of the back-end optimization result is used to pass the refined position information to the front-end filter, improving the real-time state estimation accuracy. After the back-end optimization is completed, the refined position at the latest time is used as an external position observation, and the front-end filter state is corrected through the observation update step of the Lie group EKF. The refined position output by the back-end optimization is introduced as an external position observation into the Lie group EKF update, that is, based on the observation matrix... , Based on the initial state update, further state updates are performed using external location observations. The external location observation matrix is ​​as follows: ; in, The difference between the position after SW optimization and the position before optimization. This refers to the error quantity corresponding to the state variables of the UAV.

[0038] It is worth noting that, although this embodiment Figure 1 The steps are shown sequentially as indicated by the arrows, but they are not necessarily executed in the order indicated by the arrows. Unless otherwise specified in this document, there is no strict order in which these steps are performed; they can be executed in other orders. Figure 1 At least some of the steps in the process may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily completed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed in turn or alternately with other steps or at least some of the sub-steps or stages of other steps.

[0039] The following section provides further explanation of the verifiable and optimized cluster cooperative navigation method based on Lie group EKF and SW in this embodiment, using specific simulation experiments.

[0040] The simulation scenario includes four drones, each flying along a predetermined trajectory. The flight trajectories of the drones in the simulation experiment are as follows: Figure 2 As shown in Table 1.

[0041] Table 1 Sensor parameter settings for simulation experiment

[0042] The power-on error is treated as an initial fixed component of the clock bias, which can cancel out the zero-bias portion in the two-way ranging protocol, thus eliminating the need for separate modeling. Table 2 shows the comparison of positioning errors for each method in the simulation experiments. This indicates that the entire trajectory obtained based on the baseline 4 method is optimized through offline post-processing and cannot be run in real time.

[0043] Table 2 Comparison of positioning errors of various methods in simulation experiments (unit: meters)

[0044] Figures 3 to 6 The simulation experiment demonstrates a comparison of the two-dimensional trajectories of various drones. Figure 7 and Figure 11 The simulation results show the RMSE variation curves of each UAV's position over time and a comparison of the average RMSE of all UAVs. The experimental results demonstrate that, while ensuring real-time operation, the method of this invention achieves an average APE-RMSE of 22.20 meters: a reduction of approximately 27.3% compared to the offline CORA optimization method (30.54 meters), a reduction of approximately 38.1% compared to the offline GTSAM optimization method (35.89 meters), a reduction of approximately 45.7% compared to the pure filtering 17-dimensional LG_EKF method (40.87 meters), and a reduction of approximately 67.2% compared to dead reckoning before collaboration (67.60 meters).

[0045] The above description is only a preferred embodiment of the present invention and does not limit the scope of protection of the present invention. All equivalent structural transformations made under the inventive concept of the present invention using the contents of the present invention specification and drawings, or direct / indirect applications in other related technical fields, are included within the scope of protection of the present invention.

Claims

1. A cluster cooperative navigation method based on verifiable optimization of Lie group EKF and SW, characterized in that, Adopting a front-end-back-end coupled architecture; At the front end, each UAV independently runs a Lie group right-invariant extended Kalman filter to predict its state, and integrates communication ranging observations with other UAVs and / or external position observations fed back from the back end to update its state, thus obtaining the odometry trajectory of each UAV. At the backend, the odometry trajectories and distance measurement data between all UAVs output from the frontend are received. A quadratic constrained quadratic programming problem is constructed and solved within a fixed-size sliding window to optimize the pose of all UAVs within the sliding window. At the same time, the optimization results obtained from the backend are fed back to the Lie group right-invariant extended Kalman filter as external position observations.

2. The cluster cooperative navigation method based on verifiable optimization of Lie group EKF and SW as described in claim 1, characterized in that, The state vector of the Lie group right-invariant extended Kalman filter is 17-dimensional, including Lie group pose error components, IMU bias error components, and clock error components. The Lie group pose error components include attitude error, velocity error and position error; The IMU bias error components include gyroscope bias error and accelerometer bias error; The clock error components include the equivalent clock bias and the clock drift rate.

3. The cluster cooperative navigation method based on verifiable optimization of Lie group EKF and SW as described in claim 2, characterized in that, The equivalent clock bias satisfies the random walk model, as follows: in, For equivalent clock offset, Clock drift rate, To drive noise; The clock drift rate It changes slowly over time and satisfies: in, This refers to the process noise caused by the rate of change of clock drift.

4. The cluster cooperative navigation method based on verifiable optimization of Lie group EKF and SW according to claim 1, 2, or 3, characterized in that, The state prediction process of the Lie group right-invariant extended Kalman filter is as follows: in, For drones exist Prior state estimation at time 10:00 For drones The discrete state transition matrix, For drones exist Posterior state estimation at time 10:

00. for The corresponding prediction covariance matrix, for The corresponding prediction covariance matrix, For drones The discrete process noise covariance matrix, This is the transpose of the matrix; The process of using the Lie group right-invariant extended Kalman filter to fuse communication ranging observations with other UAVs for state updates is as follows: in, For based on The fused residual covariance matrix for Optimal weights For drones The observation matrix of its own state, For drones The observation matrix of its own state, For drones Neighbor Drones The state prediction covariance matrix For drones With drones The noise covariance matrix of the distance measurement observations between them. For drones The Kalman gain matrix, For drones exist Posterior state estimation at time 10:

00. for The corresponding prediction covariance matrix, To observe the residuals, It is an identity matrix.

5. The cluster cooperative navigation method based on verifiable optimization of Lie group EKF and SW as described in claim 4, characterized in that, The observation residual Specifically: in, Unit line-of-sight vector, , For drones drones The estimated position vector, , For drones drones Positional error, The speed of electromagnetic wave propagation. , For drones drones Estimation error of equivalent clock bias For ranging, use Gaussian white noise; The position error is expressed in right-invariant error form. The observation matrix can then be obtained. , for: in, It is a zero vector. For the left Jacobian matrix of the Lie group, , For drones drones The positional disturbance component. , For drones drones The attitude error.

6. The cluster cooperative navigation method based on verifiable optimization of Lie group EKF and SW according to claim 1, 2, or 3, characterized in that, After completing the optimization of one sliding window, the earliest partial states in the sliding window are removed from the active optimization variables through a marginalization operation, and the information carried by the marginalized states is compressed into a quadratic prior term and passed to the optimization problem of the next sliding window.

7. The cluster cooperative navigation method based on verifiable optimization of Lie group EKF and SW as described in claim 6, characterized in that, The specific secondary prior terms are: in, It is a second-order prior. This refers to the state of the overlapping region within the current sliding window. The optimal solution for the overlapping region state after optimization of the previous sliding window. For the environmental space prior matrix, To marginalize the Schur complement matrix, Let be the constraint matrix of the tangent space basis matrix in the overlapping region. This indicates the Moore-Penrose pseudo-inverse.

8. The cluster cooperative navigation method based on verifiable optimization of Lie group EKF and SW as described in claim 7, characterized in that, The specific steps for constructing and solving a quadratic constrained quadratic programming problem within a fixed-size sliding window are as follows: Within the sliding window, construct using pose variables And the auxiliary unit vector introduced by distance measurement To optimize a quadratic programming problem with quadratic constraints, where, for 3D rotation matrix, for 3D translation vector, for 3D spherical manifold; The objective function of the quadratic constrained quadratic programming problem for: in, For the first The sum of the costs of pose measurement residuals and distance measurement residuals explicitly represented in each sliding window. For from the first The window is passed to the first The secondary priors of each window.

9. The cluster cooperative navigation method based on verifiable optimization of Lie group EKF and SW as described in claim 8, characterized in that, The non-convex constraints of the quadratic programming problem with quadratic constraints are handled using semidefinite programming relaxation techniques, and candidate solutions are obtained using the Riemann step method. with Lagrange multiplier vectors And construct and define the certificate matrix based on the Lagrange multiplier vectors. ,for: in, Let be the quadratic cost matrix of the s-th window. For the first The optimal Lagrange multipliers corresponding to each constraint For the first A quadratic equality constraint matrix For the first The number of quadratic equality constraints in a window For the first A sliding window optimizes the dimensions of the variables; like Then the candidate solution This is the global optimal solution to the quadratic constrained quadratic programming problem.

10. The cluster cooperative navigation method based on verifiable optimization of Lie group EKF and SW according to claim 1, 2, or 3, characterized in that, The external location observation specifically refers to: in, For external location observation, The difference between the position after SW optimization and the position before optimization. This refers to the error quantity corresponding to the state variables of the UAV.

Images