A distributed collaborative positioning method and device based on relative posture measurement

Abstract

The present invention discloses a distributed collaborative positioning method and device based on relative pose measurement. A specific implementation method of the method mainly uses the current relative observation state between a reference drone and a target drone in a drone cluster, the current observation state of the reference drone, and the current observation state of the target drone to obtain the optimal observation state of the target drone in a global coordinate system. Then, the ESKF-CI framework is used to realize the collaborative robust positioning of the drone cluster under dynamic topology in a distributed manner. This not only simplifies the calculation of the observation error state covariance matrix in the drone cluster, providing a guarantee for the real-time positioning of the drone cluster, but also solves the problem of serious error accumulation in the autonomous positioning and navigation of the existing technology using only inertial measurement units.

Description

Technical Field

[0001] The present invention belongs to the field of robot positioning technology, and in particular relates to a distributed collaborative positioning method and device based on relative posture measurement. Background Art

[0002] The rapid development of unmanned intelligent systems and swarm technology has made collaborative drone swarm missions a reality. Due to their efficiency and flexibility, drone swarms can rapidly replace manual labor in complex environments, completing tasks such as disaster relief, target search, and terrain mapping. However, complex scenarios often face challenges such as GNSS signal denial or limitation, as well as unreliable communication links. These issues have made distributed autonomous navigation technology for drone swarms a current research hotspot.

[0003] In complex GNSS-denied environments, autonomous positioning based on visual SLAM technology is limited by factors such as lighting conditions, dynamic scenes, and rapid motion, resulting in insufficient positioning reliability. Furthermore, the limited computing resources on low-cost drones restrict the use of visual SLAM technology. Furthermore, when drones perform long-distance monitoring and tracking missions, the lack of loop closure constraints leads to error accumulation in visual navigation. Collaborative localization improves the positioning capabilities of low-cost drone swarms equipped with low-precision IMUs through relative measurement, offering an effective means for autonomous drone navigation. However, existing collaborative localization methods based on relative measurement technology are primarily applied to 3-DOF ground robot swarms. Stable 3-DOF posture tracking is difficult to achieve in 6-DOF drone swarms.

[0004] Autonomous drone navigation requires maintaining precise six-degree-of-freedom (DOF) state, a requirement dictated by the drone's kinematic model and collaborative mission planning. The kinematic model indicates that updating the drone's current position and attitude requires the drone's attitude information from the previous moment. Loss of attitude information can cause drift in the drone's estimated pose. Therefore, to ensure accurate autonomous drone navigation, precise real-time tracking and maintenance of the drone's attitude is essential. Large aircraft typically incorporate highly accurate IMUs to prevent excessive divergence in attitude estimation. Alternatively, they rely on satellite navigation systems for bounded error positioning without stateful error accumulation. Alternatively, feature anchors with fixed pose information are added to provide observational information for the drone's attitude information.

[0005] Most drones in a swarm are low-cost, typically equipped with relatively inexpensive inertial navigation devices. Furthermore, swarms operate in unfamiliar and complex areas without GNSS information, making it impossible to deploy fixed anchor points in advance. Consequently, they rely solely on relative measurements between drones to provide attitude observation information. Furthermore, the consistency of cluster state estimation is an unavoidable problem in swarm state estimation. Generally, in collaborative positioning problems based on filtering methods and relative measurement techniques, the cross-correlation of cluster states directly affects the a posteriori state estimate and a posteriori covariance, potentially leading to inconsistent state estimates. Therefore, there is an urgent need to provide a distributed collaborative positioning method and device based on relative pose measurement to solve the distributed state estimation problem of swarms while ensuring estimation consistency under unknown correlations. Summary of the Invention

[0006] The present invention provides a distributed collaborative positioning method and device based on relative pose measurement; the method can improve the positioning accuracy and robustness of drone clusters in dynamic topological environments, thereby solving the problem of serious error accumulation in autonomous positioning and navigation using only inertial measurement units in the prior art.

[0007] According to a first aspect of an embodiment of the present invention, a distributed collaborative positioning method based on relative pose measurement is provided, the method comprising: for any target UAV in a UAV cluster: if at a current moment there is at least one reference UAV in the UAV cluster that has a relative measurement relationship with the target UAV, then for any reference UAV: ​​based on the current observation state corresponding to the reference UAV and the current relative observation state between the reference UAV and the target UAV, correcting the current observation state of the target UAV, and outputting the current observation error state corresponding to the target UAV; fusing the current observation error state determined by each of the at least one reference UAV to obtain the fused observation error state corresponding to the target UAV; based on the fused observation error state and the fused observation error state covariance matrix, updating the predicted error state corresponding to the target UAV at the current moment, and outputting the current optimal error state corresponding to the target UAV; correcting the current observation state corresponding to the target UAV based on the current optimal error state, and outputting the optimal observation state of the target UAV.

[0008] Optionally, based on the current observation state corresponding to the reference UAV and the current relative observation state between the reference UAV and the target UAV, the current observation state of the target UAV is corrected and the current observation error state corresponding to the target UAV is output; including: based on the current relative observation state between the reference UAV and the target UAV, the current observation state corresponding to the reference UAV is converted into the estimated observation state corresponding to the target UAV; based on the current observation state corresponding to the target UAV and the estimated observation state, the current observation error state corresponding to the target UAV is determined.

[0009] Optionally, the method also includes: determining the observation error state covariance matrix corresponding to the current observation error state of the target UAV; fusing the observation error state covariance matrix determined by each of the at least one reference UAV to obtain a fused observation error state covariance matrix corresponding to the target UAV.

[0010] Optionally, the method of determining the observation error state covariance matrix corresponding to the current observation error state of the target UAV includes: obtaining the error position covariance matrix and error attitude covariance matrix corresponding to the reference UAV, the position attitude mutual covariance matrix between the error position and the error attitude, and the attitude position mutual covariance matrix between the error attitude and the error position; performing error state transformation processing on the error position covariance matrix and the error attitude covariance matrix corresponding to the reference UAV, and outputting the error position covariance matrix and the error attitude covariance matrix corresponding to the target UAV; performing error state change processing on the position attitude mutual covariance matrix and the attitude position mutual covariance matrix corresponding to the reference UAV, and outputting the position attitude mutual covariance matrix and the attitude position mutual covariance matrix corresponding to the target UAV; based on the error position covariance matrix, error attitude covariance matrix, position attitude mutual covariance matrix and attitude position mutual covariance matrix corresponding to the target UAV, determining the observation error state covariance matrix corresponding to the observation error state of the target UAV.

[0011] Optionally, the method also includes: obtaining the previous optimal error state corresponding to the target drone at the previous moment adjacent to the current moment; performing time update processing on the previous optimal error state according to the Kalman filtering method, and outputting the predicted error state corresponding to the target drone at the current moment.

[0012] Optionally, the current observation state corresponding to the target UAV includes at least: the observation position and observation attitude of the target UAV at the current moment; the current observation error state corresponding to the target UAV includes at least: the position error and attitude error of the target UAV at the current moment.

[0013] Optionally, the method further includes: at the initial moment, performing a joint calibration process on the internal and external parameters of the sensors on each drone in the drone cluster; and setting the coordinate system of the calibrated sensors to the drone body coordinate system; wherein the sensors include at least inertial measurement sensors; determining the initial state corresponding to the drone cluster based on the initial observation state corresponding to each drone at the initial moment; and initializing the observation error state corresponding to the initial state of the drone cluster.

[0014] According to the second aspect of an embodiment of the present invention, a distributed collaborative positioning device based on relative pose measurement is also provided, the device comprising: a correction module, for any target drone in a drone cluster: if at the current moment there is at least one reference drone in the drone cluster that has a relative measurement relationship with the target drone, then for any reference drone: based on the current observation state corresponding to the reference drone and the current relative observation state between the reference drone and the target drone, the current observation state of the target drone is corrected, and the current observation error state corresponding to the target drone is output; a first fusion processing module, for fusing the current observation error state determined by each of the at least one reference drone to obtain the fused observation error state corresponding to the target drone; an update processing module, for performing state update processing on the predicted error state corresponding to the target drone at the current moment based on the fused observation error state and the fused observation error state covariance matrix, and outputting the current optimal error state corresponding to the target drone; a correction processing module, for correcting the current observation state corresponding to the target drone based on the current optimal error state, and outputting the optimal observation state of the target drone.

[0015] According to a third aspect of an embodiment of the present invention, an electronic device is further provided, comprising: a processor; a memory for storing instructions executable by the processor; and the processor for reading the executable instructions from the memory and executing the instructions to implement the method described in the first aspect.

[0016] According to a fourth aspect of an embodiment of the present invention, a computer-readable medium is further provided, on which a computer program is stored. When the program is executed by a processor, the method described in the first aspect is implemented.

[0017] An embodiment of the present invention provides a distributed collaborative positioning method and device based on relative pose measurement, the method comprising: first, for any target drone in a drone cluster: if at the current moment there is at least one reference drone in the drone cluster that has a relative measurement relationship with the target drone, then for any reference drone: based on the current observation state corresponding to the reference drone and the current relative observation state between the reference drone and the target drone, the current observation state of the target drone is corrected, and the current observation error state corresponding to the target drone is output; secondly, the current observation error state determined by each of the at least one reference drone is fused to obtain the fused observation error state corresponding to the target drone; thereafter, based on the fused observation error state and the fused observation error state covariance matrix, the predicted error state corresponding to the target drone at the current moment is updated, and the current optimal error state corresponding to the target drone is output; finally, the current observation state corresponding to the target drone is corrected based on the current optimal error state, and the optimal observation state of the target drone is output. This embodiment uses relative pose measurement information and state information to obtain the pose measurement information of the UAV in the global coordinate system, and then uses the ESKF-CI framework to achieve collaborative robust positioning of UAV clusters under dynamic topology in a distributed manner, thereby solving the technical problem of serious error accumulation in the existing technology when UAVs rely solely on inertial measurement units (IMUs) for autonomous positioning and navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] Hereinafter, some specific embodiments of the present invention will be described in detail in an exemplary and non-limiting manner with reference to the accompanying drawings. The same reference numerals in the accompanying drawings indicate the same or similar components or parts. It should be understood by those skilled in the art that these drawings are not necessarily drawn to scale. In the accompanying drawings:

[0019] Figure 1 A schematic flow chart of a distributed collaborative positioning method based on relative pose measurement provided by one embodiment of the present invention;

[0020] Figure 2 A schematic structural diagram of a distributed collaborative positioning device based on relative pose measurement provided by one embodiment of the present invention. DETAILED DESCRIPTION

[0021] In order to make the purposes, features, and advantages of the present invention more obvious and easy to understand, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the embodiments described are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without making creative efforts shall fall within the scope of protection of the present invention.

[0022] like Figure 1 , which is a flow chart of a distributed collaborative positioning method based on relative posture measurement provided by one embodiment of the present invention.

[0023] A distributed collaborative positioning method based on relative pose measurement includes at least the following steps:

[0024] S101, for any target UAV in the UAV cluster: if there is at least one reference UAV in the UAV cluster at the current moment that has a relative measurement relationship with the target UAV, then for any reference UAV: ​​based on the current observation state corresponding to the reference UAV and the current relative observation state between the reference UAV and the target UAV, correct the current observation state of the target UAV and output the current observation error state corresponding to the target UAV;

[0025] S102, fusing the current observation error state determined by each reference UAV in at least one reference UAV to obtain a fused observation error state corresponding to the target UAV;

[0026] S103, based on the fused observation error state and the fused observation error state covariance matrix, performing state update processing on the predicted error state corresponding to the target UAV at the current moment, and outputting the current optimal error state corresponding to the target UAV;

[0027] S104: Correct the current observation state corresponding to the target UAV based on the current optimal error state, and output the optimal observation state of the target UAV.

[0028] In S101, during the initialization phase, the internal and external parameters of the sensors on each UAV in the UAV cluster are first jointly calibrated. The coordinate system of the calibrated sensors is then set to the coordinate system of the UAV body. The sensors include at least inertial measurement sensors. Next, the initial observation state of each UAV in the UAV cluster is initialized to obtain the initial state corresponding to the UAV cluster. The observation error state corresponding to the initial state of the UAV cluster is then initialized to generate an initialization error state. The error state is generally set to zero at the initial moment.

[0029] When performing relative measurements between drones, if reference drone j lacks external measurement equipment and cannot directly obtain the global state information of target drone i, inter-drone measurement is necessary to transform and correct its position. For example, at time k+1, the target drone transmits a communication signal to the other drones in the drone cluster. Because the distances from each of the other drones to the target drone vary, the target drone can only receive feedback from some of the drones responding to the communication signal. Therefore, the drone from which the target drone can receive feedback is designated as the reference drone.

[0030] The IMU provides high-frequency state recursion estimation for drones, enabling short-term, high-precision state prediction capabilities. This serves as an important foundation for relative measurement-triggered collaborative positioning. Through the IMU state recursion algorithm, drones can obtain a relatively accurate initial pose, which is crucial for subsequent collaborative positioning.

[0031] Taking time k+1 as the current time, this embodiment determines the current observation state corresponding to the target UAV based on the IMU measurement data of the target UAV according to the IMU state recursion algorithm; wherein the current observation state corresponding to the target UAV includes at least: the observation position and observation attitude of the target UAV at the current time; the current observation error state corresponding to the target UAV includes at least: the position error and attitude error of the target UAV at the current time.

[0032] For example, during the motion of a drone, the position of the drone at time k+1 can be calculated by integrating all IMU measurement data between adjacent sensor measurement frames. speed and posture The IMU state recursion algorithm includes the following update formulas:

[0033] The position update formula is shown in formula (1):

[0034]

[0035] The speed update formula is shown in formula (2):

[0036]

[0037] The posture update formula is shown in formula (3):

[0038]

[0039] in, and is the measurement value of the accelerometer and gyroscope at time t, b at and b ωt is the zero bias of the accelerometer and gyroscope; gw is the gravitational acceleration vector. The median method can be used to obtain a discrete solution while meeting the iterative accuracy requirements. and It is the rotation matrix description of the target UAV observation state in the global coordinate system at time t; They represent the position, velocity and attitude of the UAV at time K respectively.

[0040] Therefore, the IMU state recursion algorithm can be used to determine the current observation state of the target drone. The IMU can provide accurate navigation information for the target drone, laying a solid foundation for the subsequent collaborative localization algorithm and ensuring the stability and reliability of the target drone in dynamic environments.

[0041] It should be noted that the calculation process of the current observation state of the reference drone is the same as the calculation process of the current observation state of the target drone; it will not be repeated here.

[0042] Based on the current observation state corresponding to the reference drone and the current relative observation state between the reference drone and the target drone, an algorithm or a trained model is used to correct the current observation state of the target drone, and the current observation error state corresponding to the target drone is output. Exemplarily, based on the current relative observation state between the reference drone and the target drone, the current observation state corresponding to the reference drone is converted into an estimated observation state corresponding to the target drone; and based on the current observation state corresponding to the target drone and the estimated observation state, the current observation error state corresponding to the target drone is determined.

[0043] The relative observation state includes at least: relative observation position and relative observation attitude; the estimated observation state includes at least: estimated observation position and estimated observation attitude.

[0044] The calculation of the estimated observation position and estimated observation attitude is shown in the following formula:

[0045]

[0046] Among them, p ji is the relative observation position, q ji is the relative observation attitude, and are the estimated observation position and estimated observation attitude corresponding to the target UAV i, and It is the current observation position and current observation attitude corresponding to the reference UAV j. The relative position and attitude of the UAV can be measured by the reasonable configuration of the sensor type. It is the rotation matrix description of the reference drone j in the global coordinate system at time k+1.

[0047] Due to the estimated observation position and estimated observation pose It is not the direct observation value of the state quantity in the error state frame. In order to avoid linearization error, the estimated observation position and estimated observation pose Corrected to the current observation error position and the current observation error attitude These error state quantities are approximated as first-order small quantities, and the specific calculation is shown in the following formula (5):

[0048]

[0049] in, is the rotation matrix of target UAV i obtained from the reference UAV observation,

[0050] It is the rotation matrix obtained recursively from the IMU measurement data.

[0051] In S102, the current observation error state determined by each reference UAV in at least one reference UAV is fused based on an algorithm or a trained large model to obtain a fused observation error state corresponding to the target UAV.

[0052] Exemplarily, by implementing the observation correction from the current relative observation state provided by all drones in the drone cluster to the current observation error state, the observation quantities of these current observation error states are not completely independent. In order to achieve consistent state estimation, the current observation error state corresponding to the target drone determined by each reference drone in at least one reference drone is fused using the covariance crossover method to output the fused observation error state corresponding to the target drone.

[0053] The calculation formula of the fusion observation error state is as follows;

[0054]

[0055] in, represents the fusion observation error state of target UAV i at time k+1; The information matrix representing the fusion observation error state of target UAV i at time k+1 is mathematically equal to the inverse of the covariance matrix, I = P -1 ; They represent the information matrix of target UAV i at time k+1, w lis the fusion weight of the information matrix. To make the fusion consistency error state measurement equal to the Kullback-Leibler average, w l The following formula (7) should be satisfied.

[0056]

[0057] In S103, the fused observation error state covariance matrix corresponding to the target UAV is determined, and the specific process is as follows: the error position covariance matrix and the error attitude covariance matrix corresponding to the reference UAV, the position attitude mutual covariance matrix between the error position and the error attitude, and the attitude position mutual covariance matrix between the error attitude and the error position are obtained; the error position covariance matrix and the error attitude covariance matrix corresponding to the reference UAV are respectively subjected to error state transformation processing, and the error position covariance matrix and the error attitude covariance matrix corresponding to the target UAV are output; the position attitude mutual covariance matrix and the attitude position mutual covariance matrix corresponding to the reference UAV are respectively subjected to error state change processing, and the position attitude mutual covariance matrix and the attitude position mutual covariance matrix corresponding to the target UAV are output; based on the error position covariance matrix, the error attitude covariance matrix, the position attitude mutual covariance matrix and the attitude position mutual covariance matrix corresponding to the target UAV, the observation error state covariance matrix corresponding to the current observation error state of the target UAV is determined. The observation error state covariance matrix determined by each reference UAV in the at least one reference UAV is fused to obtain a fused observation error state covariance matrix corresponding to the target UAV.

[0058] For example, the key to the pose measurement constraint step is to accurately model the uncertainty of pose measurement. Based on the error position covariance matrix and error attitude covariance matrix corresponding to the reference UAV, the state transformation model is used to perform error state transformation processing, and the error position covariance matrix and error attitude covariance matrix corresponding to the target UAV are output. The state transformation model is shown as follows:

[0059]

[0060] in, and They represent the current observation error state of the reference drone j at time k+1. and The corresponding error position covariance matrix and error attitude covariance matrix approximate the drone state pose and uncertainty; and are the error position covariance matrix and error attitude covariance matrix corresponding to target UAV i respectively.

[0061] The position and attitude covariance matrix corresponding to the target UAV And the attitude position covariance matrix Calculated by the following formula:

[0062]

[0063] in, is the position and attitude cross-covariance matrix between the error position and error attitude of the reference UAV; is the attitude-position cross-covariance matrix between the error attitude and error position of the reference UAV.

[0064] The observation error state covariance matrix Ω corresponding to the current observation error state of the target UAV k+1 The full formula is as follows:

[0065]

[0066] This embodiment effectively models the uncertainty in drone pose measurements, including the correlation between position and pose. Utilizing these covariance matrices, state estimation and fusion can be performed more accurately, thereby improving the positioning accuracy and consistency of drone swarms in dynamic environments. This modeling approach provides a solid theoretical foundation for subsequent state estimation and information fusion.

[0067] The prediction error state corresponding to the target UAV at the current moment is obtained by the following method: obtaining the previous optimal error state corresponding to the target UAV at the previous moment adjacent to the current moment; performing time update processing on the previous optimal error state according to the Kalman filter method, and outputting the prediction error state corresponding to the target UAV at the current moment.

[0068] For example: the value x integrated by IMU at each moment k k There is an error, in order to calibrate the deviation of the IMU and The error state update model based on the Kalman filter method updates the previous optimal error state in time; outputs the predicted error state δx corresponding to the target UAV at the current moment k+1|k The error state update model is shown as follows:

[0069] δx k+1|k =F k δx k +V k δw k

[0070]

[0071] Among them, δw k is the process error term, Contains random measurement errors n of the accelerometer and gyroscope a and n ω , and the accelerometer and gyroscope bias errors and F k is the error state transfer matrix, V k is the process error transfer matrix. k+1|k is the prediction error state covariance matrix at time k+1; δx k is the optimal error state corresponding to time k; P k is the optimal error state covariance matrix at time k; Q k is the process noise covariance matrix, reflecting the process error δw k uncertainty.

[0072] Therefore, the error state update model described above can effectively calibrate IMU bias and introduce dynamic compensation for bias in the target UAV's state estimation. This approach helps improve the positioning accuracy and stability of the target UAV in dynamic environments. Through continuous state prediction and covariance updates, real-time error correction can be achieved, thereby enhancing the UAV's navigation performance.

[0073] In the extended Kalman filter (ESKF) framework, when there is a relative pose measurement, the state is updated using the fused observation error state and the predicted error state as the actual measurement to obtain the optimal error state δx k+1 , the specific formula is as follows:

[0074]

[0075] P k+1 =(IK k+1 H k+1 )P k+1|k

[0076] δx k+1 =δx k+1|k +K k+1 (y k+1 -H k+1 δx k+1 |k) Formula (12);

[0077] Among them, P k+1∣k is the forecast error state covariance matrix; H k+1 is the observation matrix, which maps the error state to the measurement space; Ω k+1 is the covariance matrix of the measurement noise, which represents the uncertainty of the measurement; I is the identity matrix; δxk+1∣k is the prediction error state; y k+1 is the fusion observation error state at time k+1; δx k+1 Current optimal error state δx k+1 .

[0078] As a result, the ESKF can effectively fuse the predicted error state with the relative observed state to obtain the current optimal error state. This process is particularly important in the positioning and navigation of drones or other mobile platforms, significantly improving the accuracy and robustness of the system.

[0079] In S104, the ESKF framework outputs the result, i.e., the current optimal error state, to calibrate the deviation of the accelerometer and gyroscope in the IMU and eliminate the accumulated drift of the IMU inertial solution state. The state correction formula is shown in Equation (13):

[0080]

[0081] Based on the above state correction, the target UAV is made to approach 0 for the corresponding observation error state variable. The state of each target UAV in the UAV cluster is corrected so that the error state of the UAV cluster is set to 0, which means that the error state initialization of the UAV cluster is achieved.

[0082] If there is no reference drone in the drone cluster at the current moment that has a relative measurement relationship with the target drone, there is no need to perform the measurement update and state correction steps. Only the time update of the optimal error state of the target drone is performed. The time update of the optimal error state is not completed until the reference drone corresponding to the target drone is detected.

[0083] This embodiment addresses the serious error accumulation problem in autonomous positioning and navigation using only an inertial measurement unit (IMU). It utilizes a loosely coupled distributed collaborative positioning method based on combined relative pose measurements to simplify the calculation of the error state covariance matrix, effectively reducing computational complexity and providing a guarantee for real-time positioning of drone clusters. Furthermore, this application combines error state Kalman filtering with a covariance crossover method to address the difficulty in obtaining potential correlations caused by common process noise and priors, achieving robust positioning of drone clusters with dynamic topologies.

[0084] In a preferred implementation manner of this embodiment, the current observation state corresponding to the target UAV includes: the observation position, observation attitude, observation speed, accelerometer observation bias and gyroscope observation bias of the target UAV at the current moment; the current observation error state corresponding to the target UAV includes: the position error, attitude error, velocity error, accelerometer bias error, and gyroscope bias error of the target UAV at the current moment.

[0085] For example: the initial state of the drone cluster Where n, N∈N + ,N represents the size of the drone cluster, represents the initial observation state of the nth UAV;

[0086] in, and Represent the position, velocity and attitude of the UAV in the global coordinate system respectively, and are the zero bias of the accelerometer and gyroscope respectively.

[0087] Initialize error state in, represents the observation error state of the nth UAV, in, and Represent the position error, velocity error and attitude angle error of the UAV respectively, and are the bias errors of the accelerometer and gyroscope respectively.

[0088] Generally, the error state is set to zero at the initial moment, that is,

[0089] It should be noted that the observation speed of the target UAV or reference UAV in the observation state is determined based on the observation position and observation time. The accelerometer observation bias and gyroscope observation bias change dynamically with the change of the observation posture and can be effectively read based on the accelerometer and gyroscope.

[0090] The following describes in detail a distributed collaborative positioning method based on relative pose measurement provided by this embodiment in conjunction with a specific application scenario.

[0091] A distributed collaborative positioning method based on relative pose measurement includes at least the following steps:

[0092] S1. At the initial moment, the internal and external parameters of the sensors on each drone in the drone cluster are jointly calibrated; and the coordinate system of the calibrated sensors is set to the drone body coordinate system; wherein the sensors include at least inertial measurement sensors; based on the initial observation state corresponding to each drone at the initial moment, the initial state corresponding to the drone cluster is determined; and the observation error state corresponding to the initial state of the drone cluster is initialized.

[0093] S2, for any target UAV in the UAV cluster: determine whether there is at least one reference UAV in the UAV cluster at the current moment that has a relative measurement relationship with the target UAV; if so, execute step S3; if not, execute step S9.

[0094] S3. For any reference drone: based on the current relative observation state between the reference drone and the target drone, convert the current observation state corresponding to the reference drone into an estimated observation state corresponding to the target drone; and based on the current observation state corresponding to the target drone and the estimated observation state, determine the current observation error state corresponding to the target drone. The current observation state corresponding to the target drone includes: the target drone's current observation position, observation attitude, observation velocity, accelerometer observation bias, and gyroscope observation bias; and the current observation error state corresponding to the target drone includes: the target drone's current position error, attitude error, velocity error, accelerometer bias error, and gyroscope bias error.

[0095] S4, obtain the error position covariance matrix and error attitude covariance matrix corresponding to the reference UAV, the position attitude mutual covariance matrix between the error position and the error attitude, and the attitude position mutual covariance matrix between the error attitude and the error position; perform error state transformation processing on the error position covariance matrix and error attitude covariance matrix corresponding to the reference UAV, and output the error position covariance matrix and error attitude covariance matrix corresponding to the target UAV; perform error state change processing on the position attitude mutual covariance matrix and attitude position mutual covariance matrix corresponding to the reference UAV, and output the position attitude mutual covariance matrix and attitude position mutual covariance matrix corresponding to the target UAV; based on the error position covariance matrix, error attitude covariance matrix, position attitude mutual covariance matrix and attitude position mutual covariance matrix corresponding to the target UAV, determine the observation error state covariance matrix corresponding to the current observation error state of the target UAV.

[0096] S5: Fusing the current observation error state determined by each of the at least one reference drone to obtain a fused observation error state corresponding to the target drone. Fusing the observation error state covariance matrix determined by each of the at least one reference drone to obtain a fused observation error state covariance matrix corresponding to the target drone.

[0097] S6, obtaining the previous optimal error state corresponding to the target UAV at the previous moment adjacent to the current moment; performing time update processing on the previous optimal error state according to the Kalman filter method, and outputting the predicted error state corresponding to the target UAV at the current moment.

[0098] S7, based on the fused observation error state and the fused observation error state covariance matrix, perform state update processing on the predicted error state corresponding to the target UAV at the current moment, and output the current optimal error state corresponding to the target UAV.

[0099] S8, performing correction processing on the current observation state corresponding to the target UAV based on the current optimal error state, and outputting the optimal observation state of the target UAV.

[0100] S9, executing step S6, and ending the time update of the optimal error state until a reference UAV corresponding to the target UAV is detected.

[0101] This embodiment proposes a distributed collaborative localization technique based on a combined relative measurement and covariance intersection method. The optimal observation state of the target UAV in the global coordinate system is determined by using the current relative observation state between the reference UAV and the target UAV, the current observation state of the reference UAV, and the current observation state of the target UAV. The ESKF-CI framework is then used to achieve distributed robust collaborative localization of UAV clusters in dynamic topologies, addressing the serious error accumulation problem of autonomous localization and navigation using only inertial measurement units (IMUs).

[0102] like Figure 2 , which is a structural diagram of a distributed collaborative positioning device based on relative posture measurement provided by one embodiment of the present invention.

[0103] A distributed collaborative positioning device based on relative pose measurement, the device 200 includes: a correction module 201, for any target drone in a drone cluster: if there is at least one reference drone in the drone cluster that has a relative measurement relationship with the target drone at the current moment, then for any reference drone: based on the current observation state corresponding to the reference drone and the current relative observation state between the reference drone and the target drone, the current observation state of the target drone is corrected and the current observation error state corresponding to the target drone is output; a first fusion processing module 202, for fusing the current observation error state determined by each of the at least one reference drone to obtain the fused observation error state corresponding to the target drone; an update processing module 203, for performing state update processing on the predicted error state corresponding to the target drone at the current moment based on the fused observation error state and the fused observation error state covariance matrix, and outputting the current optimal error state corresponding to the target drone; a correction processing module 204, for correcting the current observation state corresponding to the target drone based on the current optimal error state, and outputting the optimal observation state of the target drone.

[0104] In a preferred implementation manner of this embodiment, the correction module includes: a conversion unit for converting the current observation state corresponding to the reference drone into an estimated observation state corresponding to the target drone based on the current relative observation state between the reference drone and the target drone; a determination unit for determining the current observation error state corresponding to the target drone based on the current observation state corresponding to the target drone and the estimated observation state.

[0105] In a preferred implementation of this embodiment, the device also includes: a first determination module, used to determine the observation error state covariance matrix corresponding to the current observation error state of the target UAV; a second fusion processing module, used to fuse the observation error state covariance matrix determined by each of the at least one reference UAV to obtain the fused observation error state covariance matrix corresponding to the target UAV.

[0106] In a preferred implementation manner of this embodiment, the first determination module includes: an acquisition unit, used to acquire the error position covariance matrix and error attitude covariance matrix corresponding to the reference drone, the position attitude mutual covariance matrix between the error position and the error attitude, and the attitude position mutual covariance matrix between the error attitude and the error position; a first error state transformation unit, used to perform error state transformation processing on the error position covariance matrix and the error attitude covariance matrix corresponding to the reference drone, respectively, and output the error position covariance matrix and the error attitude covariance matrix corresponding to the target drone; a second error state transformation processing unit, used to perform error state change processing on the position attitude mutual covariance matrix and the attitude position mutual covariance matrix corresponding to the reference drone, respectively, and output the position attitude mutual covariance matrix and the attitude position mutual covariance matrix corresponding to the target drone; a determination unit, used to determine the observation error state covariance matrix corresponding to the current observation error state of the target drone based on the error position covariance matrix, error attitude covariance matrix, position attitude mutual covariance matrix and attitude position mutual covariance matrix corresponding to the target drone.

[0107] In a preferred implementation of this embodiment, the device also includes: an acquisition module for acquiring the previous optimal error state corresponding to the target drone at the previous moment adjacent to the current moment; an update processing module for performing time update processing on the previous optimal error state according to the Kalman filtering method, and outputting the predicted error state corresponding to the target drone at the current moment.

[0108] In a preferred implementation of this embodiment, the current observation state corresponding to the target UAV includes at least: the observation position and observation attitude of the target UAV at the current moment; the current observation error state corresponding to the target UAV includes at least: the position error and attitude error of the target UAV at the current moment.

[0109] In a preferred implementation of this embodiment, the device also includes: a joint calibration module, which is used to perform joint calibration processing on the internal and external parameters of the sensors on each drone in the drone cluster at the initial moment; and set the coordinate system of the calibrated sensor to the drone body coordinate system; wherein the sensor includes at least an inertial measurement sensor; a second determination module, which is used to determine the initial state corresponding to the drone cluster based on the initial observation state corresponding to each drone at the initial moment; and an initialization module, which is used to initialize the observation error state corresponding to the initial state of the drone cluster.

[0110] The above-described device can implement a distributed collaborative positioning method based on relative pose measurement provided by an embodiment of the present invention, and possesses the corresponding functional modules and beneficial effects of executing a distributed collaborative positioning method based on relative pose measurement. For technical details not fully described in this embodiment, please refer to the Distributed Collaborative Positioning Method Based on Relative Pose Measurement Provided by an Embodiment of the Present Invention.

[0111] The present invention also provides an electronic device, comprising: a processor; a memory for storing executable instructions of the processor; the processor is used to read the executable instructions from the memory and execute the instructions to implement a distributed collaborative positioning method based on relative posture measurement described in the present invention.

[0112] In addition to the above-mentioned methods and devices, an embodiment of the present application may also be a computer program product, which includes computer program instructions, which, when executed by a processor, enable the processor to execute the steps of the method according to various embodiments of the present application described in the above-mentioned "Exemplary Method" section of this specification.

[0113] The computer program product may be written in any combination of one or more programming languages ​​to implement the program code for performing the operations of the embodiments of the present application, including object-oriented programming languages ​​such as Java, C++, and conventional procedural programming languages ​​such as "C" or similar programming languages. The program code may be executed entirely on the user's computing device, partially on the user's computing device, as a standalone software package, partially on the user's computing device and partially on a remote computing device, or entirely on a remote computing device or server.

[0114] In addition, an embodiment of the present application may also be a computer-readable storage medium having computer program instructions stored thereon, which, when executed by a processor, enable the processor to execute the steps of the method according to the following embodiments of the present application described in the above "Exemplary Method" section of this specification.

[0115] The computer-readable storage medium can adopt any combination of one or more readable media. The readable medium can be a readable signal medium or a readable storage medium. The readable storage medium can, for example, include but is not limited to a system, device or component of electricity, magnetism, light, electromagnetic, infrared, or semiconductor, or any combination thereof. More specific examples (non-exhaustive list) of readable storage media include: an electrical connection with one or more wires, a portable disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination thereof.

[0116] The basic principles of the present application have been described above in conjunction with specific embodiments. However, it should be noted that the advantages, strengths, and effects mentioned in this application are merely illustrative and not restrictive, and it should not be assumed that these advantages, strengths, and effects are required of each embodiment of this application. In addition, the specific details disclosed above are merely illustrative and facilitating understanding, and are not restrictive. The above details do not limit this application to necessarily being implemented using the above specific details.

[0117] The block diagrams of the devices, devices, equipment, and systems involved in this application are merely illustrative examples and are not intended to require or imply that they must be connected, arranged, or configured in the manner shown in the block diagrams. As will be appreciated by those skilled in the art, these devices, devices, equipment, and systems can be connected, arranged, or configured in any manner. Words such as "include," "comprise," "have," and the like are open-ended words, meaning "including but not limited to," and can be used interchangeably therewith. The words "or" and "and" used herein refer to the words "and / or" and can be used interchangeably therewith, unless the context clearly indicates otherwise. The word "such as" used herein refers to the phrase "such as but not limited to," and can be used interchangeably therewith.

[0118] It should also be noted that in the apparatus, device, and method of the present application, each component or each step can be decomposed and / or recombined, and such decomposition and / or recombination should be regarded as equivalent solutions of the present application.

[0119] The above description of the disclosed aspects is provided to enable any person skilled in the art to make or use the present application. Various modifications to these aspects will be readily apparent to those skilled in the art, and the general principles defined herein may be applied to other aspects without departing from the scope of the present application. Therefore, the present application is not intended to be limited to the aspects shown herein, but rather to be accorded the widest scope consistent with the principles and novel features disclosed herein.

[0120] The above description has been provided for the purpose of illustration and description. Furthermore, this description is not intended to limit the embodiments of the present application to the forms disclosed herein. Although a number of example aspects and embodiments have been discussed above, those skilled in the art will recognize certain variations, modifications, alterations, additions, and sub-combinations thereof.

[0121] In the description of this specification, reference to the terms "one embodiment," "some embodiments," "examples," "specific examples," or "some examples" means that the specific features, structures, materials, or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. Moreover, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples. In addition, those skilled in the art may combine and integrate different embodiments or examples described in this specification, as well as features of different embodiments or examples, unless otherwise inconsistent.

[0122] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of the technical features being referred to. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of such features. In the description of the present invention, "plurality" means two or more, unless otherwise specifically defined.

[0123] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any modifications or substitutions that can be easily conceived by a person skilled in the art within the technical scope disclosed in the present invention should be included in the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be based on the scope of protection of the claims.

Claims

1. A distributed collaborative positioning method based on relative pose measurement, characterized in that: include: For any target drone in the drone cluster: if at the current moment there is at least one reference drone in the drone cluster that has a relative measurement relationship with the target drone, then for any reference drone: based on the current observation state corresponding to the reference drone and the current relative observation state between the reference drone and the target drone, correct the current observation state of the target drone and output the current observation error state corresponding to the target drone; fusing the current observation error state determined by each of the at least one reference UAV to obtain a fused observation error state corresponding to the target UAV; Obtaining the error position covariance matrix and error attitude covariance matrix corresponding to the reference UAV, the position attitude mutual covariance matrix between the error position and the error attitude, and the attitude position mutual covariance matrix between the error attitude and the error position; performing error state transformation processing on the error position covariance matrix and the error attitude covariance matrix corresponding to the reference UAV, and outputting the error position covariance matrix and the error attitude covariance matrix corresponding to the target UAV; Perform error state change processing on the position attitude mutual covariance matrix and the attitude position mutual covariance matrix corresponding to the reference UAV respectively, and output the position attitude mutual covariance matrix and the attitude position mutual covariance matrix corresponding to the target UAV; Determine an observation error state covariance matrix corresponding to a current observation error state of the target UAV based on the error position covariance matrix, the error attitude covariance matrix, the position attitude cross-covariance matrix, and the attitude position cross-covariance matrix corresponding to the target UAV; Fusing the observation error state covariance matrix determined for each of the at least one reference UAV to obtain a fused observation error state covariance matrix corresponding to the target UAV; Based on the fused observation error state and the fused observation error state covariance matrix, a state update process is performed on the predicted error state corresponding to the target UAV at the current moment, and a current optimal error state corresponding to the target UAV is output; Based on the current optimal error state, a current observation state corresponding to the target UAV is corrected and an optimal observation state of the target UAV is output.

2. The method according to claim 1, characterized in that The method comprises: performing correction processing on the current observation state of the target UAV based on the current observation state corresponding to the reference UAV and the current relative observation state between the reference UAV and the target UAV, and outputting the current observation error state corresponding to the target UAV; comprising: Based on the current relative observation state between the reference UAV and the target UAV, converting the current observation state corresponding to the reference UAV into the estimated observation state corresponding to the target UAV; Based on the current observation state corresponding to the target UAV and the estimated observation state, a current observation error state corresponding to the target UAV is determined.

3. The method according to claim 1, characterized in that Also includes: Obtaining the previous optimal error state corresponding to the target UAV at the previous moment adjacent to the current moment; According to the Kalman filter method, the previous optimal error state is time-updated and the predicted error state corresponding to the target UAV at the current moment is output.

4. The method according to claim 1, wherein The current observation state corresponding to the target UAV includes at least: the observation position and observation posture of the target UAV at the current moment; The current observation error state corresponding to the target UAV includes at least: a position error and an attitude error of the target UAV at the current moment.

5. The method according to claim 1, wherein Also includes: At the initial moment, the internal and external parameters of the sensors on each UAV in the UAV cluster are jointly calibrated; and setting the coordinate system of the calibrated sensor to the coordinate system of the UAV body; wherein the sensor includes at least an inertial measurement sensor; Determine the initial state corresponding to the drone cluster based on the initial observation state corresponding to each drone at the initial moment; Initialize the observation error state corresponding to the initial state of the UAV cluster.

6. A distributed collaborative positioning device based on relative posture measurement, characterized in that: include: The correction module is configured to, for any target UAV in the UAV cluster: if at the current moment there is at least one reference UAV in the UAV cluster that has a relative measurement relationship with the target UAV, then for any reference UAV: ​​based on the current observation state corresponding to the reference UAV and the current relative observation state between the reference UAV and the target UAV, perform correction processing on the current observation state of the target UAV, and output the current observation error state corresponding to the target UAV; A first fusion processing module is configured to fuse the current observation error state determined by each of the at least one reference UAV to obtain a fused observation error state corresponding to the target UAV; A first determination module is used to determine an observation error state covariance matrix corresponding to the current observation error state of the target UAV; The first determination module includes: an acquisition unit, which is used to acquire the error position covariance matrix and the error attitude covariance matrix corresponding to the reference drone, the position attitude mutual covariance matrix between the error position and the error attitude, and the attitude position mutual covariance matrix between the error attitude and the error position; a first error state transformation unit, which is used to perform error state transformation processing on the error position covariance matrix and the error attitude covariance matrix corresponding to the reference drone, and output the error position covariance matrix and the error attitude covariance matrix corresponding to the target drone; a second error state transformation processing unit, which is used to perform error state change processing on the position attitude mutual covariance matrix and the attitude position mutual covariance matrix corresponding to the reference drone, and output the position attitude mutual covariance matrix and the attitude position mutual covariance matrix corresponding to the target drone; a determination unit, which is used to determine the observation error state covariance matrix corresponding to the current observation error state of the target drone based on the error position covariance matrix, the error attitude covariance matrix, the position attitude mutual covariance matrix and the attitude position mutual covariance matrix corresponding to the target drone; A second fusion processing module is used to fuse the observation error state covariance matrix determined by each reference UAV in the at least one reference UAV to obtain a fused observation error state covariance matrix corresponding to the target UAV; An update processing module is used to perform state update processing on the predicted error state corresponding to the target UAV at the current moment based on the fused observation error state and the fused observation error state covariance matrix, and output the current optimal error state corresponding to the target UAV; The correction processing module is used to perform correction processing on the current observation state corresponding to the target UAV based on the current optimal error state, and output the optimal observation state of the target UAV.

7. An electronic device, comprising: processor; a memory for storing instructions executable by the processor; The processor is configured to read the executable instructions from the memory and execute the instructions to implement the method according to any one of claims 1 to 5.

8. A computer-readable medium having a computer program stored thereon, wherein when the program is executed by a processor, the method according to any one of claims 1 to 5 is implemented.

Images