Distributed cooperative positioning method and device based on relative pose measurement

Abstract

The invention discloses a distributed cooperative positioning method and device based on relative pose measurement, and a specific implementation mode of the method mainly comprises the steps: employing a current relative observation state between a reference unmanned plane and a target unmanned plane in an unmanned plane cluster, and a current observation state of the reference unmanned plane; the optimal observation state of the target unmanned aerial vehicle in a global coordinate system is obtained according to the current observation state of the target unmanned aerial vehicle, and then unmanned aerial vehicle cluster collaborative robust positioning under dynamic topology is achieved in a distributed mode by applying an ESKF-CI framework; therefore, calculation of the observation error state covariance matrix in the unmanned aerial vehicle cluster is simplified, and a guarantee is provided for real-time positioning of the unmanned aerial vehicle cluster; and the problem of serious error accumulation existing in autonomous positioning navigation only using an inertial measurement unit in the prior art is also solved.

Description

Technical Field

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

[0002] With the rapid development of unmanned intelligent systems and swarm technology, drone swarm collaborative tasks have become a reality. In complex environments, drone swarms can replace manual work to quickly complete tasks such as disaster relief, target search, and terrain mapping due to their high efficiency and flexibility. However, complex scenes often face challenges such as GNSS signal denial or limitation, and unreliable communication links. These problems have made drone swarm distributed autonomous navigation technology a current research hotspot.

[0003] In complex environments where GNSS is denied, autonomous positioning based on visual SLAM technology is limited by factors such as lighting conditions, dynamic scenes, and rapid motion, and the positioning reliability is insufficient. At the same time, the limited computing resources on low-cost drones restrict the use of visual SLAM technology. In addition, when drones perform long-distance monitoring and tracking tasks, the lack of loop constraints causes errors to accumulate in visual navigation. Collaborative positioning improves the positioning capability of low-cost drone clusters equipped with low-precision IMUs through relative measurement, and is an effective means of autonomous navigation for drones. However, the existing collaborative positioning methods based on relative measurement technology are mainly used in 3-DOF ground robot clusters, and it is difficult to achieve stable tracking of 3-DOF postures in 6-DOF drone clusters.

[0004] Autonomous navigation of drones requires the maintenance of accurate 6-DOF states, which is determined by the drone's kinematic model and collaborative mission planning requirements. According to the drone's kinematic model, the drone's position and attitude update at the current moment requires the drone's attitude information at the previous moment. When the drone's attitude information is lost, it will cause the drone's attitude estimation to drift. Therefore, in order to ensure the drone's accurate autonomous navigation, it is necessary to accurately track and maintain the drone's attitude in real time. In general, large aircraft will be equipped with an IMU with high navigation accuracy to avoid excessive divergence of the aircraft's attitude estimation; or rely on a satellite navigation system to achieve bounded error positioning of the aircraft without state error accumulation; or provide observation information for the drone's attitude information by adding feature anchors with fixed attitude information.

[0005] Most drones in a drone swarm are low-cost drones, usually equipped with relatively cheap inertial navigation devices. In addition, drone swarms perform tasks in unfamiliar and complex areas without GNSS information, and cannot arrange fixed anchor points in advance; therefore, they can only rely on relative measurements between drones to provide attitude observation information. In addition, the estimation consistency of cluster states is also an inevitable problem in drone swarm state estimation. Generally, in the collaborative positioning problem based on filtering methods and relative measurement means, the cross-correlation of cluster states directly affects the posterior state estimation and posterior covariance, which may lead to inconsistent state estimation. For this reason, it is urgently necessary to provide a distributed collaborative positioning method and device based on relative posture measurement to solve the distributed state estimation problem of drone swarms after ensuring the estimation consistency under unknown correlation. Summary of the invention

[0006] The present invention provides a distributed collaborative positioning method and device based on relative posture measurement; the method can improve the positioning accuracy and robustness of a drone cluster in a dynamic topological environment, thereby solving the problem of serious error accumulation in the autonomous positioning and navigation using only an inertial measurement unit 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 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; 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; 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; based on the current optimal error state, the current observation state corresponding to the target drone is corrected, and the optimal observation state of the target drone is output.

[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, converting the current observation state corresponding to the reference UAV into an estimated observation state corresponding to the target UAV; based on the current observation state corresponding to the target UAV and the estimated observation state, determining the current observation error state corresponding to the target UAV.

[0009] Optionally, the method also includes: determining an 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 observed error state covariance matrix corresponding to the current observed error state of the target UAV includes: obtaining 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; performing error state transformation processing on the error position covariance matrix and the error attitude covariance matrix corresponding to the reference UAV, respectively, 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, respectively, 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, the error attitude covariance matrix, the position attitude mutual covariance matrix and the attitude position mutual covariance matrix corresponding to the target UAV, determine the observed error state covariance matrix corresponding to the observed 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 also 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 posture measurement is also provided, and the device includes: a correction module, which is used for any target drone in the 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, which is used to fuse 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, which is used to perform 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 output the current optimal error state corresponding to the target drone; a correction processing module, which is used to correct the current observation state corresponding to the target drone based on the current optimal error state, and output the optimal observation state of the target drone.

[0015] According to a third aspect of an embodiment of the present invention, there is further provided an electronic device, comprising: a processor; a memory for storing instructions executable by the processor; 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, there is further provided a computer-readable medium 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] The embodiment of the present invention provides a distributed collaborative positioning method and device based on relative posture 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; then, 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 coordinated robust positioning of the UAV cluster under dynamic topology in a distributed manner, thereby solving the technical problem of serious error accumulation that exists in the prior art when UAVs only rely on the inertial measurement unit (IMU) 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 diagram of a process flow of a distributed collaborative positioning method based on relative posture measurement provided by one embodiment of the present invention;

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

[0021] In order to make the purpose, 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 described embodiments 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 creative work are within the scope of protection of the present invention.

[0022] like Figure 1 As shown, it 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 posture measurement includes at least the following steps:

[0024] S101, for any target drone in the drone cluster: if there is at least one reference drone in the drone cluster at the current moment 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;

[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, based on the current optimal error state, correct the current observation state corresponding to the target UAV and output the optimal observation state of the target UAV.

[0028] In S101, in the initialization stage, firstly, 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 sensor is set to the drone body coordinate system; wherein the sensor includes at least an inertial measurement sensor; secondly, the initial observation state of each drone in the drone cluster needs to be initialized to obtain the initial state corresponding to the drone cluster; then, the observation error state corresponding to the initial state of the drone cluster is initialized to generate the initialization error state. Generally, the error state is set to zero at the initial moment.

[0029] When performing relative measurements between drones, if the reference drone j is not assisted by external measurement equipment and cannot directly obtain the global state information of the target drone i, then it is necessary to perform position conversion and correction through inter-drone measurement. For example: at time k+1, the target drone sends a communication signal to other drones in the drone cluster. Since the distance from each of the other drones to the target drone is different, the target drone can only receive feedback signals from some drones for the communication signal; therefore, the drone whose feedback signal the target drone can receive is determined as the reference drone.

[0030] IMU provides high-frequency state recursion estimation for drones, and has short-term and high-precision state prediction capabilities, becoming an important basis for relative measurement to trigger collaborative positioning. Through the IMU state recursion algorithm, drones can obtain a relatively accurate initial position, 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 recursive 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 solved 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 iteration accuracy requirements. and It is the rotation matrix description form 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 current observation state of the target UAV can be determined through the above IMU state recursion algorithm. IMU can provide accurate navigation information for the target UAV, lay a solid foundation for the subsequent collaborative positioning algorithm, and ensure the stability and reliability of the target UAV in a dynamic environment.

[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; they 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, the current observation state of the target drone is corrected using an algorithm or a trained model, 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 the estimated observation state corresponding to the target drone; 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: the relative observation position and the relative observation attitude; the estimated observation state includes at least: the estimated observation position and the 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 UAV can measure the relative position and attitude of the UAV through the reasonable configuration of sensor types. It is the rotation matrix description form of reference UAV 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 by observing the reference UAV,

[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 observations 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 of the at least one reference drone is fused to obtain a fused observation error state covariance matrix corresponding to the target drone.

[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 drone, 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 drone are output. The state transformation model is shown as follows:

[0059]

[0060] in, and They represent the current observation error state of reference UAV j at time k+1 respectively. and The corresponding error position covariance matrix and error attitude covariance matrix approximate the drone state posture and The uncertainty of and They 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 cross-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] Therefore, this embodiment effectively models the uncertainty of the drone when performing position and attitude measurement, including the correlation between position and attitude. Using these covariance matrices, state estimation and fusion can be performed more accurately, thereby improving the positioning accuracy and consistency of the drone group in a dynamic environment. This modeling method can provide 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; according to the Kalman filtering method, the previous optimal error state is time-updated and the prediction error state corresponding to the target UAV at the current moment is output.

[0068] For example: The value x integrated by the IMU at each moment k k There is an error, in order to calibrate the IMU deviation and The error state update model based on the Kalman filter method performs time update processing on the previous optimal error state; 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 of the accelerometer and gyroscope n 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, through the above error state update model, the IMU deviation can be effectively calibrated, and dynamic compensation for the deviation can be introduced in the state estimation of the target UAV. This method helps to improve the positioning accuracy and stability of the target UAV in a dynamic environment. Through continuous state prediction and covariance update, real-time error correction can be achieved, thereby enhancing the navigation performance of the UAV.

[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 ) 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, representing 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] Therefore, ESKF can effectively fuse the predicted error state and the relative observation state to obtain the current optimal error state. This process is particularly important in the positioning and navigation of drones or other mobile platforms, and can significantly improve the accuracy and robustness of the system.

[0079] In S104, the ESKF framework output result, i.e., the current optimal error state, is used 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 formula (13):

[0080]

[0081] Based on the above state correction, the target UAV's corresponding observation error state variable tends to 0. 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, that is, the error state initialization of the UAV cluster is achieved.

[0082] If there is no reference UAV in the UAV cluster at the current moment that has a relative measurement relationship with the target UAV, 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 UAV is performed until the reference UAV corresponding to the target UAV is detected.

[0083] This embodiment addresses the serious error accumulation problem of autonomous positioning navigation using only an inertial measurement unit (IMU). It uses a loosely coupled distributed collaborative positioning method based on combined relative pose measurement to simplify the calculation of the error state covariance matrix, effectively reduce the computational complexity, and provide a guarantee for the real-time positioning of the drone cluster. Furthermore, this application combines the error state Kalman filter with the covariance crossover method to solve the problem that the potential correlation caused by common process noise and priors is difficult to obtain, and realizes the robust positioning of the drone cluster with a dynamic topology structure.

[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 zero bias and gyroscope observation zero 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, speed error, accelerometer zero bias error, and gyroscope zero 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 They represent the position, velocity and attitude of the UAV in the global coordinate system respectively. and Represent the zero bias of the accelerometer and gyroscope respectively.

[0087] Initialize error status in, represents the observation error state of the nth UAV, in, and They represent the position error, velocity error and attitude angle error of the UAV respectively. and They represent 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 in the observation state of the target UAV or the reference UAV is determined based on the observation position and observation time, and the accelerometer observation zero bias and the gyroscope observation zero bias change dynamically with the change of the observation posture and can be effectively read based on the accelerometer and gyroscope.

[0090] A distributed collaborative positioning method based on relative posture measurement provided by this embodiment will be described in detail below in conjunction with a specific application scenario.

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

[0092] S1, at the initial moment, the internal and external parameters of the sensors on each UAV in the UAV cluster are jointly calibrated; and the coordinate system of the calibrated sensors is set to the coordinate system of the UAV body; wherein the sensors include at least inertial measurement sensors; based on the initial observation state corresponding to each UAV at the initial moment, the initial state corresponding to the UAV cluster is determined; and the observation error state corresponding to the initial state of the UAV 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 the estimated observation state corresponding to the target drone; 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 observation position, observation attitude, observation speed, accelerometer observation bias and gyroscope observation bias of the target drone at the current moment; the current observation error state corresponding to the target drone includes: the position error, attitude error, speed error, accelerometer bias error, and gyroscope bias error of the target drone at the current moment.

[0095] S4, obtaining the error position covariance matrix and error attitude covariance matrix, 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 corresponding to the reference UAV; performing error state transformation processing on the error position covariance matrix and the error attitude covariance matrix corresponding to the reference UAV, respectively, 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, respectively, 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, the error attitude covariance matrix, the position attitude mutual covariance matrix and the attitude position mutual covariance matrix corresponding to the target UAV, determining the observed error state covariance matrix corresponding to the current observed 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 filtering 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, based on the current optimal error state, correct the current observation state corresponding to the target UAV and output the optimal observation state of the target UAV.

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

[0101] This embodiment proposes a distributed collaborative positioning technology based on combined relative measurement and covariance crossover method. 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 are used to obtain the optimal observation state of the target UAV in the global coordinate system, and then the ESKF-CI framework is used to achieve the coordinated robust positioning of the UAV cluster under dynamic topology in a distributed manner, solving the problem of serious error accumulation in autonomous positioning and navigation using only inertial measurement units (IMUs).

[0102] like Figure 2 , which is a schematic diagram of the structure 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 posture measurement, the device 200 includes: a correction module 201, 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 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 performing correction processing on 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, which is used to convert 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; and a determination unit, which is used to determine 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 manner 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, 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, 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, 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, respectively, 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 observed error state covariance matrix corresponding to the current observed 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.

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

[0108] In a preferred implementation manner 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 manner 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; an initialization module, which is used to initialize the observation error state corresponding to the initial state of the drone cluster.

[0110] The above-mentioned device can execute a distributed collaborative positioning method based on relative posture measurement provided by an embodiment of the present invention, and has the corresponding functional modules and beneficial effects of executing a distributed collaborative positioning method based on relative posture measurement. For technical details not described in detail in this embodiment, please refer to a distributed collaborative positioning method based on relative posture 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 write program codes for performing the operations of the embodiments of the present application, including object-oriented programming languages, such as Java, C++, etc., and conventional procedural programming languages, such as "C" language or similar programming languages. The program code may be executed entirely on the user computing device, partially on the user device, as an independent software package, partially on the user 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 on which computer program instructions are stored. When the computer program instructions are executed by a processor, the processor executes 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 include, for example, but is not limited to, a system, device or device of electricity, magnetism, light, electromagnetic, infrared, or semiconductor, or any combination of the above. 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 of the above.

[0116] The basic principles of the present application are described above in conjunction with specific embodiments. However, it should be noted that the advantages, strengths, effects, etc. mentioned in the present application are only examples and not limitations, and it cannot be considered that these advantages, strengths, effects, etc. are required by each embodiment of the present application. In addition, the specific details disclosed above are only for the purpose of illustration and ease of understanding, not for limitation, and the above details do not limit the present application to being implemented by adopting the above specific details.

[0117] The block diagrams of the devices, apparatuses, equipment, and systems involved in this application are only illustrative examples and are not intended to require or imply that they must be connected, arranged, and configured in the manner shown in the block diagram. As will be appreciated by those skilled in the art, these devices, apparatuses, equipment, and systems can be connected, arranged, and configured in any manner. Words such as "including", "comprising", "having", etc. are open words, referring to "including but not limited to", and can be used interchangeably with them. The words "or" and "and" used here refer to the words "and / or" and can be used interchangeably with them, unless the context clearly indicates otherwise. The words "such as" used here refer to the phrase "such as but not limited to", and can be used interchangeably with them.

[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. 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 the widest scope consistent with the principles and novel features disclosed herein.

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

[0121] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", 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 one or more embodiments or examples in a suitable manner. In addition, those skilled in the art may combine and combine different embodiments or examples described in this specification and the features of different embodiments or examples, unless they are contradictory.

[0122] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include at least one of the features. In the description of the present invention, the meaning of "plurality" is two or more, unless otherwise clearly and specifically defined.

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

Claims

1. A distributed collaborative positioning method based on relative posture 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; The current observation error state determined by each of the at least one reference UAV is fused to obtain a fused observation error state 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 processed, 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 2, characterized in that Also includes: Determining an observation error state covariance matrix corresponding to a current observation error state of the target UAV; The observation error state covariance matrix determined by each of the at least one reference drone is fused to obtain a fused observation error state covariance matrix corresponding to the target drone.

4. The method according to claim 3, characterized in that: The step of determining the observation error state covariance matrix corresponding to the current observation error state of the target UAV comprises: Obtaining an error position covariance matrix and an error attitude covariance matrix corresponding to the reference drone, a position attitude cross-covariance matrix between the error position and the error attitude, and an attitude position cross-covariance matrix between the error attitude and the error position; Perform error state transformation processing on the error position covariance matrix and the error attitude covariance matrix corresponding to the reference UAV respectively, and output 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; Based on the error position covariance matrix, error attitude covariance matrix, position attitude cross-covariance matrix and attitude position cross-covariance matrix corresponding to the target UAV, an observation error state covariance matrix corresponding to the current observation error state of the target UAV is determined.

5. The method according to claim 1, characterized in that Also includes: Obtaining the last optimal error state corresponding to the target UAV at the last moment adjacent to the current moment; According to the Kalman filtering 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.

6. The method according to claim 1, characterized in that 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.

7. The method according to claim 1, characterized in that 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 drone body; wherein the sensor at least includes 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; The observation error state corresponding to the initial state of the UAV cluster is initialized.

8. A distributed collaborative positioning device based on relative posture measurement, characterized in that: include: A correction module is used for: 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, correct 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 used 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; An update processing module, 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.

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

10. A computer readable medium having a computer program stored thereon, wherein the program, when executed by a processor, implements the method according to any one of claims 1 to 7.

Images