Robot cluster distributed cooperative positioning method and system and electronic equipment

Abstract

The invention discloses a robot cluster distributed cooperative positioning method based on heterogeneous information fusion, and relates to the technical field of robot cooperative positioning. The method specifically comprises the steps of establishing a cluster joint state; establishing a kinetic model based on a local inertial sensing unit to perform forward evolution of pose self-estimation, constructing a first detection residual by using a local laser radar point cloud, and updating the local pose self-estimation through invariant extended Kalman filtering; fusing local joint state estimation by each robot in the cluster by using a consistency-based distributed information fusion rule; and constructing a second detection residual error based on relative distance information of other robots acquired by relative distance detection equipment, and updating the joint state estimation after distributed consistency fusion by using the second detection residual error. The invention aims to solve the problems that the global consistency state is difficult to obtain under a distributed architecture and the precision of a single sensor is limited.

Description

Technical Field

[0001] This invention relates to the field of robot swarm collaborative localization technology, and in particular to a distributed collaborative localization method, system and electronic device for robot swarms. Background Technology

[0002] Robot swarms, due to their intelligence far exceeding that of individual robots, are widely used in various collaborative tasks, and collaborative localization is the foundation for achieving these tasks. Current technologies typically utilize sensors such as LiDAR, inertial sensors, and various cameras for localization.

[0003] However, in a distributed architecture, relying solely on visual or radar point cloud matching is insufficient to directly obtain relative distance information between individuals, and it is prone to failure in feature-sparse regions. Furthermore, existing cooperative localization methods are mainly divided into centralized and decentralized approaches. While centralized methods offer higher accuracy, they require extremely high computational and communication resources and are difficult to scale. Decentralized methods, while robust, typically only allow each robot to perceive local information, making it difficult to obtain a globally consistent state of the cluster and thus failing to meet the global situational awareness requirements of higher-level collaborative tasks. Simultaneously, existing optimization-based methods suffer from high computational complexity and poor real-time performance, while traditional filtering-based methods are prone to introducing linearization errors when handling nonlinear errors, leading to decreased accuracy.

[0004] Therefore, how to achieve low computational cost and high accuracy in global state estimation under a distributed architecture, and effectively suppress nonlinear errors, has become a technical challenge that urgently needs to be solved. Summary of the Invention

[0005] The main objective of this invention is to provide a distributed collaborative localization method, system, and electronic device for robot swarms, which aims to achieve low computational cost and high accuracy in global state estimation under a distributed architecture, and effectively suppress nonlinear errors.

[0006] To achieve the above objectives, this invention proposes a distributed cooperative localization method for robot swarms, comprising the following steps: A cluster joint state is established, which is composed of the states of all robots in the cluster. Each robot in the cluster establishes a dynamic model based on its local inertial sensing unit to perform forward evolution of pose self-estimation, and constructs the first detection residual using the local lidar point cloud. The local pose self-estimation is updated by invariant extended Kalman filtering. Each robot in the cluster establishes a local joint state estimate. Based on the joint state estimates obtained from communication with neighboring robots, the local joint state estimates are fused using a consensus-based distributed information fusion rule. The fusion rule adopts a weighted geometric average form based on Lie group structure for the rotational states in the joint state, and a weighted arithmetic average form for the linear states in the joint state. Each robot in the cluster constructs a second detection residual based on the relative distance information obtained from other robots by the relative distance detection device, and uses the second detection residual to update the joint state estimate after distributed consensus fusion.

[0007] Preferably, the robot cluster consists of The cluster consists of [number] robots, and the joint state of the cluster is represented as follows: The robot status Specifically, it is expressed as follows:

[0008] in, For the robot's serial number, Let be a rotation matrix. For position vectors, For the speed of movement, and These are the angular velocity detection deviation and acceleration detection deviation of the inertial sensing unit, respectively. It is an identity matrix.

[0009] Preferably, the forward evolution for pose self-estimation based on the dynamic model established by the local inertial sensing unit adopts the following discrete dynamic model:

[0010]

[0011]

[0012]

[0013]

[0014] in, Represents the sampling time. The sampling interval is... and These are the detected angular velocity and acceleration, respectively. It is the acceleration due to gravity. , , , It is Gaussian noise.

[0015] Preferably, the construction of the first detection residual using the local lidar point cloud specifically includes: in the robot The At the end of the radar scan, for the first... Each radar sampling point constructs a residual in the following form:

[0016] in, The pose estimate is obtained from the forward evolution. and The rotation matrix and position vector are estimated respectively. The normal vector of the plane to which the sampling point belongs. Let be the coordinates of a point on the plane. The location of the sampling point in the radar coordinate system. This refers to the extrinsic parameters from the radar to the inertial sensing unit.

[0017] Preferably, the local pose self-estimation is updated using invariant extended Kalman filtering, specifically following the update rule:

[0018]

[0019] in, To update the obtained posterior pose self-estimation, For Kalman gain, The residual vector for all radar points. For radar observation matrix, To estimate the error covariance, The variance of the radar Gaussian detection noise.

[0020] Preferably, the establishment of the local joint state estimation is based on the acquired neighbor robot information to achieve joint state prediction, and the predicted value is determined according to the following rules. :

[0021] in, The first in the local joint state estimation One element, Update the local pose result. It's a robot. The collection of all neighboring robots, For dynamic model functions, The most recent communication time Control input.

[0022] Preferably, the step of fusing the local joint state estimates using a consensus-based distributed information fusion rule specifically includes: when At that time, for the rotational state part in the joint state The following weighted geometric mean was used for fusion:

[0023] For the linear state part of the joint state The fusion is performed using the following weighted arithmetic mean:

[0024] in, Including zero bias in position, velocity, angular velocity, and acceleration. Represents robots and Communication weights between them.

[0025] Preferably, the relative distance detection device is an ultra-wideband (UWB) sensor, and its detection model is as follows:

[0026] in, For robots Detected with robots The relative distance between them Gaussian noise detection.

[0027] Preferably, the construction of the second detection residual is based on the vector formed by the relative distances detected by robot i with other robots besides itself. The second detection residual is constructed from the difference between the predicted distance calculated by the joint state estimation and the actual distance calculated by the joint state estimation. Represented as:

[0028] in, Let be the observation function, and its component form be:

[0029] Preferably, the step of updating the joint state estimate after distributed consensus fusion using the second detection residual specifically follows the following update rule: in, For the final updated joint state estimate, Kalman gain based on UWB detection:

[0030] in, The fused estimation error covariance, This is the UWB detection matrix. The variance of noise detected by UWB.

[0031] This application also discloses a distributed cooperative localization system for robot swarms, comprising multiple robots, the system being configured to perform the method described in any of the preceding claims.

[0032] This application also discloses an electronic device including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor, when executing the program, performs the steps of the method described in any of the preceding claims.

[0033] The above technical solution has the following advantages: This invention establishes a joint state encompassing all robot states and utilizes a two-stage update using an invariant extended Kalman filter based on a Lie group structure. This not only achieves global state perception in a distributed architecture but also effectively overcomes the limitations of a single sensor through heterogeneous information fusion. Specifically, it leverages the high-frequency and high-precision characteristics of LiDAR and IMU for local pose estimation, achieves global information exchange through a distributed consensus algorithm that distinguishes between geometric mean in rotational states and arithmetic mean in linear states, and finally introduces relative distance rigidities provided by UWB to eliminate accumulated drift. This architecture significantly improves positioning accuracy and robustness while ensuring real-time performance and effectively solves the problem of linearization error introduction. Attached Figure Description

[0034] The present invention will now be described in detail with reference to specific embodiments and accompanying drawings, wherein: Figure 1 This is a flowchart illustrating the distributed collaborative localization method for robot clusters based on heterogeneous information fusion provided in Embodiment 1 of the present invention.

[0035] Figure 2 This is a schematic diagram of the experimental results for robot swarm trajectory estimation provided in Embodiment 1 of the present invention. Detailed Implementation

[0036] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of this application, and not all of them. All other embodiments obtained by those skilled in the art based on the embodiments of this application without creative effort are within the scope of protection of this application.

[0037] Example 1 This embodiment provides a distributed cooperative localization method for robot swarms based on heterogeneous information fusion. This method is applied to a swarm system composed of multiple robots. Specifically, this swarm system may include… One robot, among which The integer is a positive integer greater than 1. Each robot is equipped with multiple heterogeneous sensors, including inertial sensing units (IMUs) for measuring its own motion state, lidar for sensing environmental information, and ultra-wideband (UWB) sensors for measuring relative distances between robots. The method in this embodiment aims to address the problems of difficulty in obtaining globally consistent states for individual robots and the limited positioning accuracy of single sensors in a distributed architecture.

[0038] See Figure 1 The distributed cooperative localization method for robot swarms based on heterogeneous information fusion specifically includes the following steps: Step S101: Establish a joint state for the cluster, which consists of the states of all robots in the cluster. To achieve global perception in a distributed architecture, this embodiment first defines a joint state covering all individuals in the cluster. Let the total number of robots in the cluster be... For the number The robot's individual state is denoted as The joint state of the entire cluster Then by all It is composed of the individual states of each robot, that is equal .

[0039] In this embodiment, in order to more accurately describe the kinematic characteristics of the robot, especially the non-Euclidean characteristics of its rotational motion, the robot... status Defined on a Lie group manifold. Specifically, the state Includes rotation matrix Position vector Speed ​​of movement Angular velocity detection deviation of the inertial sensing unit and acceleration detection deviation Among them, the rotation matrix Belongs to the three-dimensional special orthogonal group Position vector and speed of movement Belongs to three-dimensional Euclidean space To facilitate subsequent calculations based on the invariant extended Kalman filter, the state... Specifically, it can be represented in the following matrix form:

[0040] In the formula, Represents a 4th-order identity matrix. This represents a 4x3 zero matrix. Through this state definition, this embodiment unifies the robot's pose, velocity, and sensor biases within a high-dimensional manifold structure, providing a mathematical basis for subsequent processing of nonlinear errors.

[0041] Step S102: Each robot in the cluster establishes a dynamic model based on its local inertial sensing unit to perform forward evolution of pose self-estimation, and uses the local LiDAR point cloud to construct the first detection residual, and updates the local pose self-estimation through invariant extended Kalman filtering.

[0042] This step primarily utilizes local high-frequency IMU data and high-precision LiDAR data to achieve accurate estimation of the robot's own state, which is the cornerstone of cooperative localization. This process comprises two sub-processes: IMU-based time updates and LiDAR-based measurement updates.

[0043] First, there's the forward evolution based on the IMU. (Robot) Angular velocity detected by local inertial sensing unit and acceleration The state of the device is deduced using a discrete dynamics model. Considering sensor noise, angular velocity detection error and acceleration detection error are modeled as random walk processes. The specific discrete dynamics model is as follows:

[0044]

[0045]

[0046]

[0047]

[0048] in, Represents the sampling time. The sampling interval can be a specific value such as... or . This is the acceleration due to gravity. , , , All noise is Gaussian. Using this model, the robot can continuously update its prior pose estimate using high-frequency IMU data between two LiDAR scans.

[0049] Secondly, updates are based on LiDAR. When the robot... Complete one radar scan, that is, on the 1st At the end of the first radar scan, the first detection residual is constructed using the scanned point cloud data. To improve computational efficiency and accuracy, this embodiment uses the distance from a point to a plane as the residual metric. For the... For each radar sampling point, construct the residual in the following form. :

[0050] in, The normal vector of the plane to which the sampling point belongs. Let be the coordinates of a point on the plane. The location of the sampling point in the radar coordinate system. The residual is a fixed extrinsic parameter from the radar to the inertial sensing unit. This residual reflects the deviation between the current predicted pose and the environmental geometry. Subsequently, the pose self-estimation is updated using the Invariant Extended Kalman Filter (IEKF). Unlike the traditional EKF, the IEKF utilizes the left- or right-invariant properties of the Lie group structure, making the evolution of the error state independent of the system state trajectory, thereby greatly suppressing linearization errors. The update rule is as follows:

[0051]

[0052] In the formula, For Kalman gain, To detect noise variance in radar. (Robot) radar observation matrix It is composed of observation matrices from various radar points, specifically represented as follows:

[0053] Among them, for the first Each radar sampling point corresponds to a submatrix. Specifically defined as:

[0054] In the above formula, The specific calculation formula is as follows:

[0055] In the formula, superscript This represents the antisymmetric matrix corresponding to the vector. Through this step, the robot... High-precision local pose posterior estimation was obtained.

[0056] Step S103: Each robot in the cluster establishes a local joint state estimate. Based on the joint state estimates obtained from communication with neighboring robots, the local joint state estimates are fused using a consensus-based distributed information fusion rule.

[0057] In this step, the system expands from single-machine localization to distributed cooperative localization. Each robot... Not only does it maintain its own state, but it also maintains a local database containing all... A joint state estimation vector for each robot state.

[0058] First, perform joint state prediction. (Robot) Information about neighboring robots is obtained through a communication network. The prediction strategy differs depending on the different elements in the joint state: for each robot... For itself, it directly uses the local pose update result calculated in step S102; for neighboring robots that can communicate directly, it uses the local pose update result transmitted by the neighboring robot; for non-neighboring robots that cannot communicate directly, it uses the dynamic model function. and the most recent communication time control input Perform extrapolation and prediction. The specific rules for determining the predicted values ​​are as follows:

[0059] in, Represents robots The collection of all neighboring robots.

[0060] Next, distributed consensus fusion is performed. This is one of the core innovations of this embodiment. To ensure that the joint state estimate of each robot gradually converges to the global true value, the robot... It is necessary to fuse estimation information about the entire cluster from its neighbors. Since the rotation matrix lies on a nonlinear Lie group manifold, a simple arithmetic mean would destroy its orthogonality. Therefore, this embodiment designs a heterogeneous fusion rule based on the Lie group structure.

[0061] When targeting non-neighbor nodes When fusing the estimates, for the rotational state component in the joint state... We employ a weighted geometric mean form based on the Lie group exponent mapping:

[0062] This formula, through geometric operations involving logarithmic mapping, weighted averaging, and then exponential mapping of the rotation matrix, ensures that the merged rotation matrix remains strictly oriented within the bounded space. On the manifold.

[0063] For the linear state component in the joint state Specifically, it includes zero biases in position, velocity, angular velocity, and acceleration, expressed as a weighted arithmetic mean:

[0064] in, Represents robots and The communication weights between them. This fusion method, which distinguishes manifold structures, effectively solves the mathematical problem of state averaging in distributed systems.

[0065] Step S104: Each robot in the cluster constructs a second detection residual based on the relative distance information between itself and other robots obtained by the relative distance detection device, and uses the second detection residual to update the joint state estimate after distributed consensus fusion.

[0066] To further constrain the relative configuration of the cluster and eliminate accumulated errors, this embodiment introduces UWB as a relative distance detection device. UWB has the advantages of omnidirectional sensing and non-line-of-sight measurement, which can compensate for the shortcomings of lidar in areas with sparse features or when they are mutually occluded.

[0067] The UWB detection model is modeled as the Euclidean distance between the positions of the two robots plus Gaussian detection noise:

[0068] robot Based on the actual detected relative distance The difference between the predicted distance calculated in the joint state estimation and the actual distance is used to construct the second detection residual. :

[0069] Where the observation function It calculates the distance between the estimated positions of the two corresponding positions in the joint state.

[0070] Finally, the residual is used to perform a Kalman update on the joint state. For the robot... Joint state estimation of itself The update rules are as follows:

[0071] Kalman gain The calculation incorporates the fused estimation error covariance. and UWB detection matrix :

[0072] Among them, the detection matrix It is composed of matrices concatenated from the matrices corresponding to each UWB detection. (For applications involving robots...) and The detection, and its corresponding matrix block Specifically defined as:

[0073] Furthermore, for nodes that are not themselves... Joint state estimation Then utilize and Related UWB testing The update is performed, and the specific update rules are as follows:

[0074] Among them, gain The calculation formula is:

[0075] This update propagates the rigid distance constraints provided by UWB to the entire joint state, making the relative positional relationships of the cluster more accurate. Even when some robots cannot be located by LiDAR, high-precision global estimation can be maintained through UWB constraints and consensus algorithms. For example, in Figure 2 In the experimental scenario shown, UAV 1 was able to accurately estimate the trajectories of all UAVs using the above method, even when UAVs 3 and 4 could not communicate with it continuously. This was achieved by utilizing indirect information and UWB constraints.

[0076] Example 2 This embodiment provides a distributed cooperative localization system for robot swarms, used to execute the method in Embodiment 1 above. This system enables decentralized, high-precision full-state estimation of the swarm, and is particularly suitable for multi-robot cooperative operation scenarios in environments with satellite signal rejection or weak texture.

[0077] This robot swarm distributed cooperative localization system includes multiple robots, such as One robot, among which The integer is greater than or equal to 2. Each robot in the cluster consists of a hardware layer and an algorithm layer. At the hardware level, each robot is equipped with an inertial sensing unit, LiDAR, relative distance detection device, communication module, and onboard processor.

[0078] Specifically, the inertial sensing unit, or IMU, is used to collect the robot's angular velocity and acceleration data in real time, and the sampling frequency is typically set to... to ,For example To meet the requirements of high-frequency state extrapolation, a lidar is used to scan the surrounding environment to obtain environmental point cloud data. This lidar can be either a mechanically rotating lidar or a solid-state lidar. The relative distance detection device specifically uses an ultra-wideband (UWB) sensor module to measure the relative distance between robots and other robot nodes. A communication module is used to establish wireless communication links between robots, exchanging their joint state estimation information. The communication protocol can be WiFi, ZigBee, or a dedicated ad hoc networking protocol. An onboard processor serves as the computing core, running the aforementioned distributed localization algorithm.

[0079] At the algorithmic logic level, to achieve the above functions, each robot is equipped with multiple functional modules, which are stored in memory and executed by the onboard processor. These modules specifically include a joint state establishment module, a pose self-estimation and update module, a distributed consensus fusion module, and a relative observation update module.

[0080] The joint state establishment module is used to establish the joint state of the robot swarm. This joint state consists of the states of all robots in the swarm, and the state of each robot is defined on a Lie group manifold, including rotation matrix, position, velocity, and IMU bias.

[0081] The pose self-estimation update module processes observations from local sensors. This module first establishes a dynamic model based on the local inertial sensing unit and uses IMU data to perform high-frequency forward evolution of the robot's pose self-estimation. Subsequently, the module constructs the first detection residual, i.e., the point-to-surface distance residual, using the local LiDAR point cloud, and corrects the local pose self-estimation using an invariant extended Kalman filter (IEKF). In this process, the IEKF gain calculation utilizes the group operation relationship between the error state and the nominal state, making the filtering process independent of the state trajectory, thus ensuring stability and consistency in nonlinear systems.

[0082] The distributed consensus fusion module handles information interaction and state synchronization among robots. This module establishes a local joint state estimate and acquires the joint state estimates of neighboring robots through the communication module. During the fusion process, this module executes a consensus-based distributed information fusion rule. For the rotational state component in the joint state, this module uses a weighted geometric average based on a Lie group structure for calculation; that is, it performs a weighted average in the tangent space and then maps it back to the manifold space to strictly maintain the orthogonality of the rotation matrices. For the linear state component in the joint state, this module uses a weighted arithmetic average for calculation.

[0083] The relative observation update module is used to fuse UWB measurement information. This module constructs a second detection residual based on the relative distance information acquired by the UWB sensor and other robots. This residual reflects the difference between the measured distance and the joint state prediction distance. Subsequently, this second detection residual is used to perform a final Kalman update on the joint state estimate after distributed consensus fusion. Through this module, rigid relative geometric constraints are established within the cluster, effectively suppressing positioning drift that accumulates over time.

[0084] Through the coordinated operation of the above modules, any robot in the system, for example, numbered... The robot can not only accurately know its own position, but also accurately estimate the positions of other robots in the cluster, such as the one numbered [number missing], without direct observation. The robot's status was monitored, enabling situational awareness across the entire cluster.

[0085] Example 3 This embodiment provides an electronic device that can serve as an onboard computing platform for a single robot in the aforementioned robot cluster. This electronic device is designed to provide sufficient computing power to support the real-time operation of complex IEKF filtering and distributed consensus algorithms.

[0086] The electronic device includes a memory, a processor, and a computer program stored in the memory and executable on the processor.

[0087] The memory is used to store various types of data and programs, including but not limited to raw data collected by the inertial sensing unit, point cloud data from the lidar, UWB ranging data, status data transmitted by neighboring nodes, and operating systems and applications.

[0088] The memory can be a high-speed random access memory or a non-volatile memory, such as at least one disk storage device, flash memory device or other volatile solid-state storage device.

[0089] The processor is the control center of the electronic device, connecting all parts of the device through various interfaces and lines. By running or executing software programs and modules stored in memory, and by calling data stored in memory, the processor performs various functions and processes data, thereby enabling overall monitoring and positioning calculations for the robot. The processor can be composed of integrated circuit chips and has signal processing capabilities. For example, the processor can be one or more central processing units (CPUs), digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices.

[0090] When the computer program is executed by the processor, it implements the steps of the distributed cooperative localization method for robot swarms based on heterogeneous information fusion as described in Embodiment 1. Specifically, it includes: establishing a joint state of the swarm containing the states of all robots; performing forward dynamic evolution based on local IMU data and updating the local pose using invariant extended Kalman filtering based on lidar point cloud residuals; acquiring neighbor information through communication and fusing the joint state using a consistency fusion rule that distinguishes between the geometric mean of rotational states and the arithmetic mean of linear states; and updating the joint state based on UWB relative distance residuals.

[0091] The electronic device may also include a communication interface for communicating with other devices or communication networks. For example, it can receive sensor data from IMU, LiDAR, and UWB via the communication interface and send locally computed joint state estimates to neighboring robots via a wireless network.

[0092] Example 4 This embodiment provides a non-transitory computer-readable storage medium. The storage medium stores a computer program that, when executed by a processor, implements the steps of the robot swarm distributed cooperative localization method based on heterogeneous information fusion as described in Embodiment 1.

[0093] The storage medium can be any medium capable of storing program code, including but not limited to read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks. The computer program contains all the instruction code for executing the aforementioned distributed cooperative localization algorithm. When this code is loaded into the robot's computing unit and executed, it enables the robot to perform a series of operations, such as initializing the joint state, acquiring sensor data, performing IEKF filtering, conducting distributed communication and fusion, and correcting the state using UWB data.

[0094] By using the program in this storage medium, ordinary mobile robot clusters can be upgraded into intelligent clusters with distributed heterogeneous information fusion capabilities, enabling them to perform high-precision and robust collaborative positioning in complex environments.

[0095] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A method for distributed cooperative localization of a swarm of robots, characterized in that, The method comprises the following steps: establishing a joint state of the cluster, the joint state being composed of states of all robots in the cluster; each robot in the cluster establishes a forward evolution of pose self-estimation based on a local inertial sensing unit to establish a dynamic model, and constructs a first detection residual using a local laser radar point cloud, and updates the local pose self-estimation by using an invariant extended Kalman filter; each robot in the cluster establishes a local joint state estimation, and fuses the local joint state estimation based on a joint state estimation obtained by communication with a neighbor robot by using a distributed information fusion rule based on consistency; wherein the fusion rule adopts a weighted geometric mean form based on a Lie group structure for a rotation state in the joint state, and adopts a weighted arithmetic mean form for a linear state in the joint state; each robot in the cluster constructs a second detection residual based on relative distance information of other robots obtained by a relative distance detection device, and updates the joint state estimation after distributed consistency fusion by using the second detection residual.

2. The method of claim 1, wherein, The robot cluster is composed of robots, and a cluster joint state is represented as ; wherein the state of the robot is specifically represented as: wherein, is a number of the robot, is a rotation matrix, is a position vector, is a motion velocity, and are an angular velocity detection bias and an acceleration detection bias of the inertial sensor unit, respectively, is an identity matrix.

3. The method of claim 2, wherein, The forward evolution of pose self-estimation based on the local inertial sensing unit to establish a dynamic model adopts the following discrete dynamic model: wherein represents a sampling time instant, is a sampling interval, and are the detected angular velocity and acceleration, respectively, is the gravitational acceleration, , , , is a Gaussian noise.

4. The method of claim 3, wherein, The construction of the first detection residual using local LiDAR point cloud specifically includes: in the robot The At the end of the radar scan, for the first... Each radar sampling point constructs a residual in the following form: wherein, is the pose estimate obtained by forward evolution, and are the rotation matrix and position vector estimate in it, respectively, is the normal vector of the plane the sample point belongs to, is the coordinate of a point on the plane, is the position of the sample point in the radar coordinate system, is the extrinsic parameter from radar to inertial sensor unit.

5. The method of claim 4, wherein, The updating of the local pose self-estimation by using the invariant extended Kalman filter specifically follows the following updating rule: wherein, is the updated posterior pose self-estimation, is the Kalman gain, is the residual vector of all radar points, is the radar observation matrix, is the estimation error covariance, is the variance of the radar Gaussian detection noise.

6. The method of claim 2, wherein, The local joint state estimation is established, the joint state prediction is realized based on the obtained neighbor robot information, and the predicted value is determined according to the following rules : in, The first in the local joint state estimation One element, Update the local pose result. It's a robot. The collection of all neighboring robots, For dynamic model functions, The most recent communication time Control input.

7. The method of claim 6, wherein, The local joint state estimation is fused by using the consistent-based distributed information fusion rule, and specifically includes the following steps. When the rotation state part in the joint state is fused by using the following weighted geometric mean form: Linear state part in the combined state The fusion is performed in the form of a weighted arithmetic mean as follows: wherein, comprises position, velocity, angular velocity bias and acceleration bias, represents a communication weight between the robot and and the robot.

8. The method of claim 7, wherein, The relative distance detection device is an ultra-wideband sensing device UWB, and a detection model thereof is: wherein, is a robot a detected relative distance between the robot and the object, is a Gaussian detection noise.

9. The method of claim 8, wherein, The second detection residual is constructed based on a vector of relative distances between the robot i and the robots other than itself detected by the robot i The second detection residual is constructed based on a vector of relative distances between the robot i and the robots other than itself detected by the robot i is represented as: wherein is an observation function, whose component form is 。 10. The method of claim 9, wherein, The second detection residual is used to update the joint state estimation after distributed consistency fusion, specifically following the following update rule: wherein, is the final updated joint state estimation, is the Kalman gain based on UWB detection: wherein, is the estimated error covariance after fusion, is the UWB detection matrix, is the variance of the UWB detection noise.

11. A robotic swarm distributed cooperative localization system, comprising: The system comprises a plurality of robots, and is configured to perform the method according to any one of claims 1 to 10.

12. An electronic device comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, The processor implements the steps of the method according to any one of claims 1 to 10 when executing the program.

Images