A cooperative localization method for unmanned swarms based on fusion factor graphs
By constructing a multi-source sensor collaborative acquisition system and optimizing the factor graph model, high-precision collaborative positioning of unmanned swarms in GNSS-denied environments is achieved, solving the problems of low positioning accuracy and error accumulation of unmanned swarms in complex urban environments, and providing reliable location awareness and collaborative operation capabilities.
Patent Information
- Application Number
- CN202512024066.5
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2025-12-30
- Publication Date
- 2026-11-17
- Estimated Expiration
- 2045-12-30
AI Technical Summary
When GNSS signals are denied in complex urban environments, unmanned swarms suffer from low positioning accuracy and the accumulation of positioning errors over time. It becomes difficult for each UAV node to maintain a precise relative pose relationship, which affects the efficiency of collaborative operations.
An unmanned swarm cooperative localization method based on fusion factor graphs is adopted. By constructing a multi-source sensor cooperative acquisition system, a unified spatiotemporal reference is established. Using UWB ranging, IMU inertial data and visual image data, a tightly coupled optimization model is constructed and the factor graph model is optimized to achieve online localization of unknown single anchor points and suppress the cumulative drift of visual inertial odometry.
Significantly improves the collaborative positioning accuracy of unmanned swarms in GNSS-denied environments, provides reliable location awareness, ensures the collaborative operation capability of unmanned swarms in complex scenarios, and eliminates dependence on pre-calibrated infrastructure.
Smart Images

Figure CN121898372B_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of artificial intelligence, and specifically refers to an unmanned cluster cooperative localization method based on fusion factor graph. Background Technology
[0002] With the rapid development of unmanned systems technology and the deepening of its cluster applications, unmanned swarms, with their advantages of distributed collaboration and parallel execution of multiple tasks, have been widely used in various fields and have become one of the core directions for the development of intelligent unmanned systems. Precise and continuous positioning and navigation capabilities are prerequisites for unmanned swarms to achieve autonomous flight, formation control, and mission collaboration, directly determining the operational efficiency and mission reliability of the swarm system.
[0003] The positioning and navigation of unmanned swarms rely heavily on the absolute position reference provided by the Global Navigation Satellite System (GNSS). However, in complex urban environments, factors such as dense building obstruction, electromagnetic radiation shielding, and electronic countermeasures interference can easily lead to GNSS signal rejection, causing individual UAVs within the swarm to lose a continuous and reliable absolute positioning reference. It is difficult for each UAV node to maintain a precise relative pose relationship, which seriously restricts the collaborative operation efficiency of unmanned swarms. Summary of the Invention
[0004] To overcome the problems of low positioning accuracy and cumulative positioning errors over time in GNSS-denied environments for unmanned swarms, this invention proposes a cooperative positioning method for unmanned swarms based on a fusion factor graph. This method constructs a fusion factor graph optimization model, tightly coupling multi-source information such as visual observation, inertial measurement, and UWB ranging. Employing an online positioning strategy with an unknown single anchor point, it establishes a global position reference without requiring pre-calibrated infrastructure. Simultaneously, this method effectively suppresses the cumulative drift of visual-inertial odometry, significantly improving cooperative positioning accuracy in satellite navigation-denied environments, and providing reliable position awareness for cooperative operations of unmanned swarms in complex scenarios.
[0005] The technical solution adopted in this invention is as follows:
[0006] A method for unmanned swarm cooperative localization based on fusion factor graphs includes the following steps:
[0007] Step 1: Construct a multi-source sensor collaborative acquisition system to synchronously acquire UWB ranging data, IMU inertial data and visual image data at each UAV node, and complete the spatiotemporal calibration between sensors to establish a unified spatiotemporal reference.
[0008] Step 2: Establish a tightly coupled optimization model based on a sliding window, incorporating the pose state of each UAV, sensor bias, and unknown single anchor point into a unified state space.
[0009] Step 3: Construct a tightly coupled optimization objective function that includes IMU pre-integration residuals, visual reprojection residuals, UWB inter-machine ranging residuals, and UWB machine-anchor ranging residuals;
[0010] Step 4: Estimate the initial position of the anchor point in the global coordinate system by fusing multiple UAV VIO trajectories with UWB drone-anchor ranging data;
[0011] Step 5: Use a factor graphical model to structurally represent the tightly coupled optimization problem, taking state variables as nodes and multi-source observation constraints as factors, and establish a probabilistic graphical model.
[0012] Step 6: Iteratively optimize the fusion factor diagram by dynamically adjusting the damping factor to balance the convergence speed and stability during the iterative process.
[0013] Step 7: Introduce a sliding window edge-out strategy to retain historical observation information while maintaining optimization efficiency, thus ensuring the temporal consistency of state estimation.
[0014] Step 8: Output the optimal pose estimate of each UAV in the global coordinate system in real time to complete the high-precision collaborative positioning of the UAV swarm.
[0015] Furthermore, in step 1, the time calibration adopts a soft synchronization scheme, using the key frame timestamp of the visual inertial odometry as a unified reference, aligning the UWB ranging data timestamps through linear interpolation, and using the DS-TWR scheme to compensate for the clock offset between cluster nodes; the spatial calibration solves the pose transformation matrix from the camera to the IMU through the hand-eye calibration principle, and at the same time uses nonlinear least squares optimization to complete the position vector calibration of the UWB antenna relative to the IMU, realizing the spatial unification of the coordinate system of the multi-source sensors.
[0016] Furthermore, in step 2, the unified state space includes the position, velocity, and attitude quaternions of each UAV in the world coordinate system, the zero bias of the IMU accelerometer and gyroscope, the inverse depth value of the visual feature point in the camera coordinate system when it is first observed, and the three-dimensional spatial coordinates of the unknown UWB single anchor point in the world coordinate system. The unified state space of the UAV nodes is dynamically maintained by a sliding window.
[0017] Furthermore, in step 3, the tightly coupled optimization objective function includes a multivariate constraint system, specifically including: IMU pre-integration residuals constraining the relative motion between adjacent keyframes; visual reprojection residuals constraining the consistency of feature point projection in multi-view images; UWB inter-machine ranging residuals providing relative distance constraints between UAVs; UWB UAV-anchor ranging residuals providing absolute distance constraints between UAVs and anchor points; marginalization prior residuals maintaining the consistency of historical observation information of the system by transforming the old keyframe states removed from the sliding window into probabilistic prior constraints; and constructing the tightly coupled optimization objective function by weighted combination of each residual term.
[0018] Furthermore, in step 4, the local coordinate system of any UAV in the cluster is selected as the dominant coordinate system. The remaining UAVs establish a rigid transformation relationship between their own local coordinate system and the dominant coordinate system through UWB inter-UAV ranging observation, so as to realize the coordinate unification of the VIO trajectory of all UAVs. Based on the multi-UAV trajectory under the unified coordinate, combined with the multi-time UWB ranging data of each UAV and the unknown single anchor point, the initial three-dimensional position estimate of the anchor point is obtained.
[0019] Furthermore, in step 5, the global state of each UAV key frame moment and the three-dimensional position of the unknown anchor point within the sliding window are used as state nodes of the factor graph. The IMU pre-integration residual, visual reprojection residual, UWB inter-UAV ranging residual, and UWB UAV-anchor ranging residual are converted into corresponding observation factors. Each observation factor is incorporated into the global probability estimation framework in the form of a negative log-likelihood function, thus transforming the tightly coupled optimization problem into a factor graph optimization problem.
[0020] Furthermore, in step 6, the Levenberg-Marquardt algorithm is used to perform nonlinear iterative optimization on the fusion factor graph. The objective function is expanded into a second-order Taylor series at the current state estimate and an incremental equation is constructed. The damping factor is dynamically adjusted according to the changes in the iterative residuals. The residuals are reduced to improve the convergence speed and increased to ensure convergence stability.
[0021] Furthermore, in step 7, when the addition of a new keyframe causes the sliding window length to exceed a set threshold, the oldest keyframe is marginalized. By converting the state information of the old keyframe into prior constraints on the remaining state, the kinematic constraints of historical observations are preserved while removing the old keyframe to control computational complexity.
[0022] Furthermore, in step 8, the optimal pose estimation includes the three-dimensional position, attitude quaternion, velocity vector, and IMU sensor bias estimation of each UAV in the global coordinate system. The system uses the state estimation at the latest moment of the sliding window as the output to provide accurate and consistent state information for the cooperative localization and decision control of the UAV swarm.
[0023] The beneficial effects of this invention are as follows:
[0024] 1. This invention proposes a collaborative localization method for unmanned swarms based on fusion factor graphs. This method establishes a unified spatiotemporal reference by constructing a multi-source sensor collaborative acquisition system; designs a tightly coupled optimization model based on a sliding window to jointly optimize the UAV pose, sensor bias, and unknown anchor point coordinates in a unified state space; constructs a multi-factor constraint system including IMU pre-integration, visual reprojection, and UWB ranging, and achieves deep fusion of multi-source observation information through a factor graph model; and adopts an online localization strategy for unknown single anchor points to establish a global position reference without the need for pre-calibrated infrastructure.
[0025] 2. This invention effectively solves the problems of accumulated positioning errors and lack of global reference in unmanned swarm cooperative positioning under satellite-guided denial environments. It gets rid of the dependence of traditional UWB positioning on pre-calibrated anchor points, takes into account both positioning accuracy and real-time performance, significantly improves the cooperative positioning capability of unmanned swarms in complex urban environments, provides reliable guarantee for autonomous flight and cooperative mission execution of unmanned swarms, and has broad engineering application prospects. Attached Figure Description
[0026] Figure 1 This is a schematic diagram of the timestamps of data collected by various sensors in an embodiment of the present invention.
[0027] Figure 2 This is a schematic diagram illustrating the relative spatial relationships of various sensor coordinate systems in embodiments of the present invention.
[0028] Figure 3 This is a schematic diagram illustrating the estimation of the position of an unknown single anchor point in an embodiment of the present invention.
[0029] Figure 4 This is a schematic diagram of the fusion factor graph optimization model in an embodiment of the present invention. Detailed Implementation
[0030] 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 some embodiments of the present invention, and not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention.
[0031] A method for unmanned swarm cooperative localization based on fusion factor graphs includes the following steps:
[0032] Step 1: Construct a multi-source sensor collaborative acquisition system to synchronously acquire UWB ranging data, IMU inertial data and visual image data at each UAV node, and complete the spatiotemporal calibration between sensors to establish a unified spatiotemporal reference.
[0033] Step 2: Establish a tightly coupled optimization model based on a sliding window, incorporating the pose state of each UAV, sensor bias, and unknown single anchor point into a unified state space.
[0034] Step 3: Construct a tightly coupled optimization objective function that includes IMU pre-integration residuals, visual reprojection residuals, UWB inter-machine ranging residuals, and UWB machine-anchor ranging residuals;
[0035] Step 4: Use a factor graphical model to structurally represent the tightly coupled optimization problem, taking state variables as nodes and multi-source observation constraints as factors, and establish a probabilistic graphical model.
[0036] Step 5: Iteratively optimize the fusion factor diagram by dynamically adjusting the convergence speed and stability during the damping factor balancing iteration process.
[0037] Step 6: Introduce a sliding window edge-out strategy to retain historical observation information while maintaining optimization efficiency, thus ensuring the temporal consistency of state estimation.
[0038] Step 7: Output the optimal pose estimate of each UAV in the global coordinate system in real time.
[0039] Achieve high-precision collaborative positioning for unmanned swarms.
[0040] Furthermore, in step 1, the time calibration adopts a soft synchronization scheme, using the key frame timestamp of the visual inertial odometry as a unified reference, aligning the UWB ranging data timestamps through linear interpolation, and using the DS-TWR scheme to compensate for the clock offset between cluster nodes; the spatial calibration solves the pose transformation matrix from the camera to the IMU through the hand-eye calibration principle, and at the same time uses nonlinear least squares optimization to complete the position offset vector calibration of the UWB antenna relative to the IMU, realizing the spatial unification of the coordinate system of the multi-source sensors.
[0041] Furthermore, in step 2, the unified state space includes the position, velocity, and attitude quaternions of each UAV in the world coordinate system, the zero bias of the IMU accelerometer and gyroscope, the inverse depth value of the visual feature point in the camera coordinate system when it is first observed, and the three-dimensional spatial coordinates of the unknown UWB single anchor point in the world coordinate system. A sliding window dynamically maintains the recent key states of the UAV nodes, ensuring estimation accuracy while limiting the dimension of the global state vector to control computational complexity, thus preserving sufficient historical observation data.
[0042] Further, in step 3, the multivariate constraint system includes: IMU pre-integration residual constraining the relative motion between adjacent keyframes; visual reprojection residual constraining the projection consistency of feature points in multi-view images; UWB inter-machine ranging residual providing relative distance constraints between UAVs; UWB machine-to-anchor ranging residual providing absolute distance constraints between UAVs and anchor points; and constructing a tightly coupled optimization objective function by weighted combination of each residual term.
[0043] Furthermore, in step 4, the global state of each UAV keyframe within the sliding window and the 3D position of the unknown anchor point are used as state nodes in the factor graph. The IMU pre-integration residual, visual reprojection residual, UWB inter-UAV ranging residual, and UWB UAV-anchor ranging residual are transformed into corresponding observation factors. Each observation factor is incorporated into the global probability estimation framework in the form of a negative log-likelihood function, transforming the tightly coupled optimization problem into a factor graph optimization problem.
[0044] Furthermore, in step 5, the Levenberg-Marquardt algorithm is used to perform nonlinear iterative optimization of the fusion factor graph. The objective function is expanded in second order Taylor at the current state estimate, and an incremental equation is constructed. The damping factor is dynamically adjusted according to the changes in the iterative residuals; decreasing the residuals improves the convergence speed, while increasing the residuals ensures convergence stability.
[0045] Furthermore, in step 6, when the addition of a new keyframe causes the sliding window length to exceed a set threshold, the oldest keyframe is marginalized. By converting the state information of the old keyframe into prior constraints on the remaining state, the kinematic constraints of historical observations are preserved while removing the old keyframe to control computational complexity.
[0046] Furthermore, in step 7, the optimal pose estimation includes the three-dimensional position, attitude quaternion, velocity vector, and IMU sensor bias estimation of each UAV in the global coordinate system. The system uses the state estimation at the latest moment of the sliding window as the output to provide accurate and consistent state information for the cooperative localization and decision control of the UAV swarm.
[0047] Here are some more specific examples:
[0048] A collaborative localization method for unmanned swarms based on fusion factor graphs is applied to collaborative tasks of unmanned swarms in highly dynamic and complex environments. The method includes the following steps:
[0049] Step 1: Spatiotemporal synchronization of multiple sensor sources;
[0050] The timestamps of data collected by various sensors on a personal drone, such as Figure 1As shown, a soft synchronization scheme is used to synchronize the clocks of three types of sensors. First, the time discrepancy between the camera and IMU is accurately compensated through offline calibration or online estimation, constructing a VIO time system with highly consistent internal timing, using the VIO keyframe timestamp as a unified time reference. Based on this, linear interpolation is used to align the UWB timestamp with the VIO keyframe timestamp. When a timestamp is determined to be... After the VIO keyframe, it is possible to calculate UWB ranging value corresponding to the time:
[0051]
[0052] in, The representation is obtained through linear interpolation. UWB ranging value corresponding to the given time; and Representing the historical UWB ranging message queue and The timestamps of the two closest frames of data; and Representative at and The UWB ranging value corresponding to the given time.
[0053] Among them, targeting , , and If no corresponding value exists, it is recorded as 0;
[0054] Building upon the internal sensor time synchronization of individual UAVs, the system further considers the time synchronization between nodes within the cluster. For UWB ranging, the DS-TWR scheme is adopted to measure inter-UAV distances using the signal round-trip time difference. This method effectively compensates for clock skew between nodes without relying on strict global clock synchronization. Furthermore, to unify the clocks for inter-UWB ranging constraints and VIO local motion constraints, it is necessary to further map the VIO local state timestamps of all UAVs to a global time reference. Therefore, the system uses a node in the cluster as a time reference source, periodically broadcasting its global timestamp. Other nodes establish a mapping relationship from local time to global time by estimating the deviation between their own clock and the reference clock online.
[0055]
[0056] in, and Each represents its own clock. With reference clock By continuously calibrating the drift rate and offset between these two parameters, the key frame states output by each UAV's VIO can be uniformly converted to the global time coordinate system.
[0057] In addition to the drone node corresponding to the reference clock, each drone node calculates its own... and value.
[0058] At the spatial level, due to inherent physical installation deviations and attitude differences between the UWB antenna phase center, IMU sensing center, and camera optical center, the IMU coordinate system... Camera coordinate system and UWB coordinate system It is difficult to achieve spatial unity, and their relative spatial relationships are as follows: Figure 2 As shown. The core of spatial synchronization lies in establishing a rigid transformation relationship between the coordinate systems of each sensor and a unified reference system through extrinsic parameter calibration, thereby achieving spatial benchmark unification of multi-source observation data. This system uses the IMU coordinate system. As a unified reference frame, it is therefore necessary to calibrate the pose transformation matrix from the camera to the IMU. and the position offset vector of the UWB antenna relative to the IMU .
[0059] For the extrinsic parameter calibration of the camera-IMU, a kinematic method based on the hand-eye calibration principle is adopted. By collecting multidimensional motion data of the camera-IMU system in front of the calibration board, the hand-eye calibration equation is constructed using the relative motion relationship between adjacent time points.
[0060]
[0061] in, Representing the IMU coordinate system from Time's up The relative transformation matrix at time step can be obtained through IMU pre-integration; Represents the camera coordinate system from Time's up The relative transformation matrix at any given time can be calculated through observation using a visual calibration board. The optimal rotation matrix can be simultaneously solved by minimizing the overall constraint residuals. With translation vector .
[0062] For the extrinsic parameter calibration of UWB-IMU, since UWB itself does not provide attitude information, it is necessary to determine the three-dimensional position of the UWB antenna phase center in the IMU coordinate system. This refers to lever compensation. During calibration, it's necessary to collect 3D motion data of the UWB-IMU system within the coverage area of the UWB anchor point, simultaneously recording the IMU trajectory pose estimated by VIO and the original UWB ranging values. Next, a nonlinear least-squares optimization problem is established. By minimizing the difference between the theoretically predicted distance and the actual UWB ranging value, the optimal 3D position vector can be solved. :
[0063]
[0064] in, This represents the number of valid data pairs during the calibration process; This represents the position of the UWB anchor point in the world coordinate system; VIO estimates the trajectory and pose sequence of the IMU in the world coordinate system; This represents the original ranging sequence between the UWB module and the anchor point.
[0065] Step 2: Construct a tightly coupled optimization model based on a sliding window;
[0066] A tightly coupled strategy is employed to achieve deep fusion of multi-source heterogeneous data. Furthermore, a sliding window optimization strategy is introduced on top of this tightly coupled architecture. This strategy dynamically maintains a finite number of recent keyframes and their state information, preserving sufficient historical observation data to ensure the continuity of kinematic constraints and estimation accuracy. Simultaneously, by actively limiting the dimension of the global state vector, the computational complexity of the optimization is kept within a controllable range, thus achieving an effective balance between computational efficiency and estimation accuracy. The system state vector within the sliding window... Defined as:
[0067]
[0068] in, Represents the first in the sliding window The global state of the drone at each keyframe moment, including the drone's position in the world coordinate system. ,speed Posture Quaternions IMU accelerometer zero bias and IMU gyroscope zero bias . This represents the inverse depth value in the camera coordinate system when the i-th visual feature point within the sliding window is first observed. It represents the three-dimensional spatial state of the UWB anchor point in the world coordinate system.
[0069] The core objective of the tightly coupled architecture is to achieve the optimal estimation of the system state vector by minimizing the weighted sum of squares of the multi-source observation residuals. Each residual term represents the difference between the actual sensor observations and the predicted values based on the system state. Based on the multi-source sensor observation model, a tightly coupled optimization objective function is constructed:
[0070]
[0071] in, , , , , These represent the IMU pre-integration residual, visual reprojection residual, UWB inter-machine ranging residual, UWB machine-anchor ranging residual, and marginalized prior residual, respectively. The set of observations representing the IMU pre-integration; This represents the actual observed pixel coordinates of the l-th visual feature point in the j-th camera; Represents the marginalized prior measurement vector; , , , , These represent the types of constraints formed from observation data originating from different sensors; , , These are the covariance matrices of the corresponding residual terms mentioned above. , These are the variances of the corresponding residual terms mentioned above;
[0072] Used to characterize the uncertainty of each observation; The weighted norm is used to assign optimized weights to each residual term based on its observation uncertainty, transforming the multi-source fusion problem into a maximum a posteriori probability estimation problem, *that is, the uncertainty. This is the Huber robust kernel function, used to suppress the influence of outliers in visual observations.
[0073] Step 3: Online positioning of unknown single anchor point;
[0074] In the constructed tightly coupled optimization model based on a sliding window, the global position of the unknown single anchor point has been determined. Included in the system state vector Therefore, it is necessary to rely on the collaborative constraints of multi-source observation residuals to achieve accurate online estimation of the anchor point position. During the unmanned swarm flight, the relative positional relationship between each UAV node and the anchor point continuously changes dynamically. The ranging observation between each UAV node and the anchor point constitutes a spherical constraint in three-dimensional space with the instantaneous position of the UAV as the center and the ranging value as the radius, such as... Figure 3As shown, by combining the spherical constraints of multiple UAVs at different spatiotemporal locations, the unique position of the anchor point in three-dimensional space can be calculated.
[0075] Since the motion trajectories output by each UAV's VIO system are defined in its own local coordinate system, it is first necessary to unify the local coordinate systems of all UAVs to the same dominant coordinate system. Select the local coordinate system of any UAV within the cluster (e.g., UAV 1). As the dominant coordinate system, the other UAVs establish a rigid transformation relationship between their local coordinate system and the dominant coordinate system through UWB inter-UAV ranging observations:
[0076]
[0077] Where N represents the number of valid data pairs used to solve the coordinate transformation; and Representing drones Local coordinate system To the dominant coordinate system The rotation matrix and translation vector; and These represent the positions of UAV j and UAV 1 in their respective local coordinate systems under the k-th observation; Let represent the UWB ranging values of UAV j and UAV 1 under the k-th observation. The optimal transformation parameters are obtained through iterative solutions. and Then, all drone trajectories can be uniformly converted to the dominant coordinate system:
[0078]
[0079] After obtaining the UAV trajectory under a unified coordinate system, an initial estimation model for the anchor point position is constructed by fusing ranging observations between multiple UAV nodes and the anchor point at different spatiotemporal locations:
[0080]
[0081] Let represent the UWB ranging value between UAV j and the UWB anchor point during the k-th observation. The initial estimate of the anchor point position obtained above is then used. As prior information, it is incorporated into a tightly coupled optimization model based on a sliding window, participating in global joint optimization along with multi-source observation constraints such as visual reprojection, IMU pre-integration, and UWB ranging. During the iterative optimization process, the system continuously monitors the convergence status of the anchor point position estimation. Once the estimation result is confirmed to meet the convergence condition, the three-dimensional position of the anchor point is determined. The anchor point is precisely calibrated and removed from the state vector to be optimized. Thereafter, this anchor point serves as a constant spatial reference in the system, providing a stable and reliable global position constraint for unmanned swarm cooperative positioning.
[0082] Step 4: Multi-source observation factor modeling;
[0083] The fusion factor graph optimization model constructed in this invention is as follows: Figure 4 As shown. The core of the fusion factor graph lies in representing the constraint relationship between multi-source sensor observations and system state through probability factors. Based on the constructed observation residual models, they can be further transformed into corresponding observation factors in the factor graph model. All observation factors are incorporated into the global optimization framework in the form of negative log-likelihood functions to quantify the degree of agreement between actual observations and predictions based on system state. The following sections model various observation factors respectively:
[0084] (1) IMU pre-integration factor
[0085] The IMU pre-integration factor, as the core constraint unit connecting the motion states of adjacent keyframes, compresses the high-frequency inertial measurement data of the IMU into incremental motion observations between discrete keyframes, establishing tightly coupled kinematic constraints between consecutive keyframe states. Its probabilistic form is defined as follows:
[0086]
[0087] in, For IMU pre-integrated residuals; The covariance matrix of the IMU pre-integration observations reflects the propagation and accumulation effects of IMU noise during the pre-integration process.
[0088] (2) Visual reprojection factor
[0089] The visual reprojection factor, as the core constraint unit relating the visual features of the environment to the pose state of the UAV, transforms the observation of two-dimensional image feature points into geometric projection relationships in three-dimensional space, establishing direct visual measurement constraints between the pose states of different keyframes. Its probabilistic form is defined as follows:
[0090]
[0091] in, For visual reprojection residuals; The covariance matrix of visual observation reflects the uncertainty in feature point extraction and matching.
[0092] (3) UWB inter-machine ranging factor
[0093] The UWB inter-node ranging factor, as the core constraint unit for maintaining the relative spatial relationship within the cluster, establishes a relative spatial constraint between any two UAV nodes within the cluster. Its probabilistic form is defined as follows:
[0094]
[0095] in, For UWB inter-machine ranging residuals; Representative inter-machine distance observation value The variance.
[0096] (4) UWB machine-anchor distance factor
[0097] The UWB drone-anchor ranging factor, as the core anchoring unit connecting the local trajectory of the UAV with the global reference, transforms the ranging information between the UAV and the anchor point into absolute position constraints. Its probabilistic form is defined as follows:
[0098]
[0099] in, For UWB machine-anchor distance measurement residuals; Representative machine-anchor distance measurement observations The variance.
[0100] Step 5: Optimize the solution of the fusion factor graph;
[0101] The goal of fusion factor graph optimization is to achieve maximum a posteriori probability estimation of the system state by fusing multi-source observation information within a unified probabilistic extrapolation framework. Based on the multi-source observation factors defined above, a global objective optimization function is constructed. :
[0102]
[0103] Where K is the number of keyframes; L is the total number of visual feature points. The set of cameras that observed feature point l; This represents the set of UAV pairs that have UWB inter-UAV ranging capabilities; n is the number of UAVs.
[0104] Due to the global objective optimization function It's about the system state. Since the function is highly nonlinear, the Levenberg-Marquardt algorithm is used for iterative optimization. This algorithm dynamically adjusts the damping factor. It adaptively switches between gradient descent and Gauss-Newton methods, possessing both global convergence and fast local convergence capabilities. Next, the objective function... At the current state estimate Perform a second-order Taylor expansion at this point:
[0105]
[0106] in, This represents the system state increment. For the objective function in Jacobian matrix at the location; Let be the Hessian matrix, which characterizes the curvature properties of the objective function.
[0107] To minimize the approximate quadratic function, let it have a change with respect to the state increment. The partial derivatives are zero, and a damping factor is introduced. By balancing the iteration step size, the following incremental equation is obtained:
[0108]
[0109] in, This represents the residual vector composed of all residual terms. The state increment can be obtained by solving the above increment equation. Subsequently, a manifold update strategy is used to update the attitude quaternions to ensure their orthogonal constraint properties, thereby updating the system state. .
[0110] To meet the real-time requirements of unmanned swarm cooperative localization, a sliding window marginalization strategy is introduced during the iterative optimization of the LM algorithm. By dynamically maintaining a keyframe window of finite length, the computational complexity of the optimization problem is controlled. When the addition of a new keyframe causes the window length to exceed a set threshold, the oldest keyframe within the window is marginalized, and the state information of the old keyframe is transformed into a priori constraints on the remaining states.
[0111] As the fusion factor graph iterative optimization continues, the optimization process terminates when the state increment norm falls below a set threshold or the maximum number of iterations is reached. At this point, the system outputs the optimal state estimate for all UAV nodes within the sliding window. This optimization result not only effectively suppresses the cumulative drift of the visual-inertial odometry but also establishes a unified global coordinate system at the cluster level, providing high-precision, spatially consistent collaborative positioning capabilities for UAV clusters.
[0112] In summary, this invention proposes a cooperative localization method for unmanned swarms based on fusion factor graphs. This method ensures accurate and reliable cooperative localization capabilities for unmanned swarms even in GNSS-denied environments. By designing a multi-source sensor fusion architecture and a tightly coupled optimization model, it innovatively achieves online localization of unknown single anchor points and joint estimation of swarm states. This provides a reliable cooperative localization guarantee for the continuous and stable operation of unmanned swarms in satellite navigation denied environments, demonstrating significant engineering application value and broad development prospects.
[0113] The foregoing has provided a detailed description of the unmanned swarm cooperative localization method based on fusion factor graphs proposed in this invention. The descriptions of the embodiments above are merely illustrative of the method and its core ideas. Furthermore, those skilled in the art will recognize that modifications may be made to the specific implementation methods and application scope based on the ideas of this invention. Therefore, the content of this specification should not be construed as limiting the invention.
Claims
1. A method for unmanned cluster cooperative localization based on fusion factor graphs, characterized in that, Includes the following steps: Step 1: Construct a multi-source sensor collaborative acquisition system to synchronously acquire UWB ranging data, IMU inertial data and visual image data at each UAV node, and complete the spatiotemporal calibration between sensors to establish a unified spatiotemporal reference. Step 2: Establish a tightly coupled optimization model based on a sliding window, incorporating the pose state of each UAV, sensor bias, and unknown single anchor point position into a unified state space. Step 3: Construct a tightly coupled optimization objective function that includes IMU pre-integration residuals, visual reprojection residuals, UWB inter-machine ranging residuals, and UWB machine-anchor ranging residuals; Step 4: Estimate the initial position of the anchor point in the global coordinate system by fusing multiple UAV VIO trajectories with UWB aircraft-anchor ranging data; Step 5: A factor graphical model is used to structurally represent the tightly coupled optimization problem, with state variables as nodes and multi-source observation constraints as factors, to establish a probabilistic graphical model; specifically: The global state and unknown anchor point 3D position of each UAV key frame moment in the sliding window are used as state nodes of the factor graph. The IMU pre-integration residual, visual reprojection residual, UWB inter-UAV ranging residual, and UWB UAV-anchor ranging residual are transformed into corresponding observation factors. Each observation factor is incorporated into the global probability estimation framework in the form of a negative log-likelihood function, transforming the tightly coupled optimization problem into a factor graph optimization problem. Step 6: Iteratively optimize the fusion factor diagram, dynamically adjusting the damping factor during the iteration process; specifically: The Levenberg-Marquardt algorithm is used to perform nonlinear iterative optimization of the fusion factor graph. The objective function is expanded into a second-order Taylor series at the current state estimate and an incremental equation is constructed. The damping factor is dynamically adjusted according to the change of the iterative residual. The residual is reduced to improve the convergence speed and increased to ensure the convergence stability. Step 7: Introduce a sliding window edge-out strategy while preserving historical observation information; Step 8: Output the optimal pose estimate of each UAV in the global coordinate system in real time to complete the high-precision collaborative positioning of the UAV swarm.
2. The unmanned cluster cooperative localization method based on fusion factor graph as described in claim 1, characterized in that, In step 1, the time calibration adopts a soft synchronization scheme, using the key frame timestamp of visual inertial odometry as a unified reference, aligning the UWB ranging data timestamps through linear interpolation, and using the DS-TWR scheme to compensate for the clock offset between cluster nodes. Spatial calibration solves the pose transformation matrix from camera to IMU using the hand-eye calibration principle, and simultaneously uses nonlinear least squares optimization to complete the position vector calibration of UWB antenna relative to IMU, thus achieving spatial unification of the coordinate system of multi-source sensors.
3. The unmanned cluster cooperative localization method based on fusion factor graphs according to claim 1, characterized in that, In step 2, the unified state space includes the position, velocity, and attitude quaternions of each UAV in the world coordinate system, the zero bias of the IMU accelerometer and gyroscope, the inverse depth value of the visual feature point in the camera coordinate system when it is first observed, and the three-dimensional spatial coordinates of the unknown UWB single anchor point in the world coordinate system. The unified state space of the UAV nodes is dynamically maintained by the sliding window.
4. The unmanned cluster cooperative localization method based on fusion factor graph as described in claim 1, characterized in that, In step 3, the tightly coupled optimization objective function includes a multivariate constraint system, specifically including: IMU pre-integration residuals constraining the relative motion between adjacent keyframes; visual reprojection residuals constraining the consistency of feature point projection in multi-view images; UWB inter-machine ranging residuals providing relative distance constraints between UAVs; UWB UAV-anchor ranging residuals providing absolute distance constraints between UAVs and anchor points; and marginalization prior residuals maintaining the consistency of historical observation information by transforming the old keyframe states removed from the sliding window into probabilistic prior constraints. The tightly coupled optimization objective function is constructed by weighted combination of the residual terms.
5. The unmanned cluster cooperative localization method based on fusion factor graphs according to claim 1, characterized in that, In step 4, the local coordinate system of any UAV in the cluster is selected as the dominant coordinate system. The other UAVs establish a rigid transformation relationship between their own local coordinate system and the dominant coordinate system through UWB inter-UAV ranging observation, so as to achieve coordinate unification of the VIO trajectories of all UAVs. Based on the multi-UAV trajectories under the unified coordinate system, combined with the multi-time UWB ranging data of each UAV and the unknown single anchor point, the initial three-dimensional position estimate of the anchor point is obtained.
6. The unmanned cluster cooperative localization method based on fusion factor graph as described in claim 1, characterized in that, In step 7, when the addition of a new keyframe causes the sliding window length to exceed the set threshold, the oldest keyframe is marginalized. By converting the state information of the old keyframe into prior constraints on the remaining state, the kinematic constraints of historical observations are preserved while removing the old keyframe to control computational complexity.
7. The unmanned cluster cooperative localization method based on fusion factor graph as described in claim 1, characterized in that, In step 8, the optimal pose estimation includes the three-dimensional position, attitude quaternion, velocity vector, and IMU sensor bias estimation of each UAV in the global coordinate system. The system uses the state estimation at the latest moment of the sliding window as the output to provide accurate and consistent state information for the cooperative localization and decision control of the UAV swarm.
Citation Information
Patent Citations
Visual inertia tight coupling odometer based on UWB online anchor points and metering method
CN113124856A
Multi-unmanned aerial vehicle cooperative positioning-oriented UWB and vision fusion positioning method and system
CN117685953A
