Water and air cross-domain unmanned swarm collaborative localization and mapping method

Abstract

This invention relates to the field of unmanned aerial vehicle (UAV) swarm technology, and more particularly to a method for collaborative localization and mapping of UAV swarms in cross-domain water and air environments. The method is based on a multi-constraint error state Kalman filter, integrating various observational information from cameras, lidar, millimeter-wave radar, satellite navigation, inertial navigation, and swarm relative observations. It leverages the complementary advantages of different sensors to overcome the differences in perception range between UAV swarms in cross-domain water and air environments, enabling swarm collaborative localization to achieve smooth relative positioning and globally consistent absolute positioning information in such environments. Simultaneously, based on a map synchronization request and response communication mechanism and a Hilbert curve-based encoding and compression algorithm, the UAV swarm can achieve low-bandwidth, low-latency semantic occupancy grid synchronization, thereby realizing robust collaborative localization and low-latency environmental map synchronization for cross-domain UAV swarms.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) swarm technology, and in particular to a method for collaborative localization and mapping of unmanned aerial vehicle (UAV) swarms across water and air domains. Background Technology

[0002] Cross-domain swarm systems composed of unmanned aerial vehicles (UAVs) and unmanned surface vessels (USVs), compared to traditional single-domain swarm systems, possess the characteristics of information complementarity and spatial linkage. They can be applied to tasks such as autonomous inspection, search and rescue, material handling, and cleaning, and have broad application prospects. Collaborative positioning, mapping, and information sharing are important prerequisites for the autonomous cooperation of this unmanned swarm.

[0003] However, most existing research focuses on cooperative localization and mapping within a single domain. There is currently a lack of usable localization and mapping solutions to address the differences in sensing range and communication capabilities among unmanned swarms operating across water and air domains.

[0004] How to overcome the differences in perception range among unmanned swarms in water and air through multi-sensor fusion and achieve robust cooperative localization, and how to design information compression algorithms to achieve low-latency environmental map synchronization under limited bandwidth are the technical challenges for improving the intelligence of unmanned swarms in water and air. Summary of the Invention

[0005] The present invention aims to solve the technical problem of difficult collaborative positioning and mapping between existing cross-domain unmanned swarms in water and air.

[0006] To address the aforementioned technical problems, this invention provides a cross-domain unmanned swarm cooperative localization and mapping method, comprising the following steps:

[0007] Sensor data is acquired through the sensor modules of each unmanned device in the unmanned swarm. The sensor modules include cameras, lidar, millimeter-wave radar, satellite navigation sensors, and inertial navigation sensors. Data is transmitted between different unmanned devices through a wireless communication network.

[0008] The semantic point cloud information and cluster positioning information of each of the unmanned devices are obtained through the sensor data.

[0009] Based on the semantic point cloud information and the cluster positioning information, map synchronization is performed on different unmanned devices to obtain the unmanned cluster mapping data.

[0010] Furthermore, the step of acquiring semantic point cloud information and cluster positioning information of each unmanned device in the unmanned cluster through the sensor data includes the following sub-steps:

[0011] The sensor data acquired by each of the unmanned devices is preprocessed;

[0012] For each of the sensor data, a preset semantic segmentation network is used to perform semantic segmentation to obtain the semantic point cloud information;

[0013] The Kalman filter method based on the uniform motion assumption obtains relative position and velocity observations between different unmanned devices based on the semantic point cloud information.

[0014] Based on the relative position and velocity observations, as well as the sensor data, the multi-constraint error state Kalman filter method is used to process the data to obtain the cluster positioning information of each unmanned device in the unmanned swarm.

[0015] Furthermore, the step of synchronizing maps of different unmanned devices based on the semantic point cloud information and cluster positioning information to obtain unmanned cluster mapping data includes the following sub-steps:

[0016] A semantic occupancy grid map for unmanned cluster map synchronization is constructed in different unmanned devices;

[0017] The communication mechanism between different unmanned devices is based on a request and response mechanism, or the map update data based on the semantic point cloud information and the cluster positioning information is transmitted through the cluster control system of the unmanned devices.

[0018] In each of the unmanned devices, based on the map update data, Hilbert curve encoding is used to compress the data to obtain a semantic instance compressed package. Then, the semantic occupancy raster map is updated according to the semantic instance compressed package, and the updated semantic occupancy raster map is output as the mapping data of the unmanned cluster.

[0019] Furthermore, the Kalman filter method based on the assumption of uniform motion, specifically involves the following steps for obtaining relative position and velocity observations between different unmanned devices based on the semantic point cloud information:

[0020] Let unmanned device i track and identify unmanned device j. The state vector of the Kalman filter method based on the uniform motion assumption at time k satisfies the following relationship:

[0021]

[0022] in, It is the unmanned equipment j machine system at time k. j Compared to unmanned equipment i-machine system I i Location, It is the unmanned equipment j machine system at time k. j In the local world system of unmanned equipment i speed;

[0023] The Kalman filter method based on the uniform motion assumption consists of two parts: forward propagation and observation update. The forward propagation of the Kalman filter method based on the uniform motion assumption satisfies the following relationship:

[0024]

[0025]

[0026] in, It is the prior state at time k. It is the posterior state at time k-1, F track,k-1 It is the state transition matrix. It is the prior covariance at time k. It is the posterior covariance at time k-1, Q track It is the predicted noise matrix;

[0027] In the semantic point cloud information at time k, find the point cluster corresponding to the unmanned device j as the observation value z for the relative position and velocity observation. track,k The observation update of the Kalman filter method based on the uniform motion assumption satisfies the following relationship:

[0028]

[0029] Among them, h track The (·) function is the observation equation, H track,k It is the observation matrix. R is the posterior covariance at time k. track,k It is the noise matrix of the observation.

[0030] Furthermore, the step of processing the relative position and velocity observations and the sensor data using a multi-constraint error state Kalman filter method to obtain the cluster positioning information of each unmanned device in the unmanned swarm is as follows:

[0031] Define the state vector x of unmanned device i at time k. i,k for:

[0032]

[0033] in, Let i be the inertial state of unmanned equipment i at time k; Let i be the clone state of unmanned device i at time k, used to maintain the pose of c historical time points; Let be the cluster extrinsic state of unmanned device i at time k, used to maintain the local world system extrinsic parameters of the other n unmanned devices;

[0034] The multi-constraint error state Kalman filter method comprises three parts: forward propagation, state augmentation, and observation update. The forward propagation of the multi-constraint error state Kalman filter method satisfies the following relationship:

[0035]

[0036] in, Let f(·) be the prior state of unmanned device i at time k, and let f(·) be the equation of motion. Let a be the posterior state of unmanned device i at time k-1. m,k and w m,k These are the acceleration and angular velocity measurements of the inertial system at time k, respectively. F is the prior covariance of unmanned device i at time k. k-1 It is a state matrix. Q is the posterior covariance of unmanned device i at time k-1. k-1 It is the noise matrix of the inertial system at time k-1;

[0037] The state augmentation of the multi-constraint error state Kalman filter method satisfies the following relationship:

[0038]

[0039] Where A is the augmented Jacobian matrix and I is the identity matrix;

[0040] The observation update of the multi-constraint error state Kalman filter method satisfies the following relationship:

[0041]

[0042] Among them, z m,k The relative position and velocity observations, or the observed values ​​of the cluster positioning information corresponding to the sensor data, are given, where h(·) is the observation function and n is the number of observations. m,k To observe noise.

[0043] Furthermore, at time K, the observed value of the cluster positioning information corresponding to the camera of unmanned device i is defined as z. cam,k The observed value of the cluster positioning information corresponding to the lidar is z. lidar,k The observed value of the cluster positioning information corresponding to the millimeter-wave radar is z. radar,k The observed value of the cluster positioning information corresponding to the satellite navigation sensor is z. gnss,k The observed values ​​z of the relative position and velocity observations track,k Includes active observation z active,k and passive observation z passive,k ,in:

[0044]

[0045] h is the visual feature point observed by unmanned device i at time k. d (·) is the camera's distortion function, h p (·) is the camera's reprojection function, h t (·) is the rigid body transformation function, n cam It is the camera's observation noise;

[0046]

[0047] p L It is the laser point obtained by the lidar, laser point p L By using nearest neighbor search, the five nearest laser points are obtained, and the plane normal vector u and center point q fitted to these five laser points are calculated. and It is the extrinsic transformation matrix of the laser-radar to unmanned equipment system; n lidar It is the observation noise of the lidar;

[0048]

[0049] v R and p R These are the Doppler velocity and three-dimensional coordinates obtained from millimeter-wave radar. It is the extrinsic transformation matrix of the millimeter-wave radar to unmanned vehicle system, and the ||·| function is used to calculate the magnitude of the vector, n radar It is the observation noise of millimeter-wave radar;

[0050]

[0051] and It is the external parameter transformation between the local world system and the global world system w of the unmanned device i. It is the position observation of unmanned equipment i by the satellite navigation system at time k, n gnss This is observation noise from the satellite navigation system;

[0052]

[0053] The observed value z, representing the relative position and velocity, is obtained by tracking and identifying unmanned device j from unmanned device i at time k. track,k Relative position observation, and It is the external parameter transformation between the local world system of unmanned device i and the local world system of unmanned device j, n active It is observation noise from active observation;

[0054]

[0055] and It is the pose of the unmanned device j at time k. All are cluster positioning information sent by unmanned device j to unmanned device i, n passive It is observation noise from passive observation.

[0056] Furthermore, the observation update of the multi-constraint error state Kalman filter method also satisfies the following relationship:

[0057]

[0058] in, Let be the posterior state of unmanned device i at time k. For generalized addition, Let H be the posterior covariance of unmanned device i at time k. k Let R be the observation matrix. m,k This is the observation noise matrix.

[0059] Furthermore, the semantic occupancy raster map includes four pieces of information: voxel index, timestamp, occupancy attribute, and semantic attribute. In each of the unmanned devices, the step of compressing the map update data using Hilbert curve encoding to obtain a semantic instance compressed package includes the following sub-steps:

[0060] A local voxel map of semantic instances is constructed based on the map update data. The minimum hash index and maximum hash index of the semantic instances in the map update data are counted to obtain a voxel map composed of Boolean values. Voxel attributes belonging to semantic instances are denoted as 1, and voxel attributes not belonging to semantic instances are denoted as 0.

[0061] The local voxel map is Hilbert curve encoded to obtain a one-dimensional Boolean array;

[0062] The one-dimensional Boolean array is serialized to obtain a one-dimensional data stream;

[0063] The one-dimensional data stream is compressed using Hough coding to obtain the semantic instance compressed package.

[0064] Furthermore, the communication mechanism for the request and response is specifically as follows:

[0065] The requester broadcasts the index of the semantic instance compressed package it owns as a map synchronization request message;

[0066] After receiving the map synchronization request message, the receiver compares it with the voxel index of the semantic occupied raster map it owns, confirms the semantic instance compressed package that the requester is missing, and sends the semantic instance compressed package as a map synchronization response message to the requester.

[0067] After receiving the map synchronization response message, the requester performs data duplication filtering through index comparison, and uses the filtered map synchronization response message to update its own semantic occupied grid map and unmanned cluster mapping data.

[0068] Furthermore, the step of updating the semantic occupancy grid map based on the semantic instance compressed package specifically includes:

[0069] If the unmanned device receives the map update data transmitted by the cluster control system, it obtains the voxels that need to be updated through the ray casting algorithm, updates the voxel's occupancy attribute and semantic attribute using the maximum a posteriori probability algorithm, and then uses the voxel clustering algorithm to cluster the voxels according to the semantic category to obtain multiple semantic instances. The semantic instances are used to replace the voxels that need to be updated in the semantic occupancy raster map, and the updated semantic occupancy raster map is output as the unmanned cluster mapping data.

[0070] If the unmanned device receives map update data transmitted from other unmanned devices, it decompresses the map update data to obtain the local voxel map containing semantic instances. Based on the three-dimensional spatial coordinates of the local voxel map, it uses the maximum a posteriori probability algorithm to update the occupancy attribute and semantic attribute of the voxels at the corresponding coordinates in the semantic occupancy raster map. Then, it uses a voxel clustering algorithm to cluster the voxels according to semantic categories to obtain the semantic occupancy raster map updated based on the local voxel map. The updated semantic occupancy raster map is then output as the mapping data for the unmanned cluster.

[0071] The beneficial effects achieved by this invention lie in proposing a collaborative localization and mapping method for unmanned swarms in cross-domain water and air environments based on multiple sensors and data update mechanisms. This method is based on a multi-constraint error state Kalman filter and integrates various observation information such as camera, lidar, millimeter-wave radar, satellite navigation, inertial navigation, and swarm relative observation. It can leverage the complementary advantages of different sensors to overcome the differences in perception range between unmanned swarms in cross-domain water and air environments, enabling swarm collaborative localization to obtain smooth relative positioning and globally consistent absolute positioning information in cross-domain water and air environments. At the same time, based on the communication mechanism of map synchronization request and response, and the encoding and compression algorithm based on Hilbert curves, the unmanned swarm can achieve low-bandwidth, low-latency semantic occupancy grid synchronization, thereby realizing robust collaborative localization and low-latency environmental map synchronization for unmanned swarms in cross-domain water and air environments. Attached Figure Description

[0072] Figure 1 This is a flowchart illustrating the steps of the cross-domain unmanned cluster collaborative positioning and mapping method provided in this embodiment of the invention. Detailed Implementation

[0073] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0074] Please refer to Figure 1 , Figure 1 This is a flowchart illustrating the steps of the cross-domain unmanned swarm cooperative localization and mapping method provided in this embodiment of the invention. The cross-domain unmanned swarm cooperative localization and mapping method includes the following steps:

[0075] S101. Sensor data is acquired through the sensor module of each unmanned device in the unmanned cluster. The sensor module includes a camera, lidar, millimeter-wave radar, satellite navigation sensor, and inertial navigation sensor. Data is transmitted between different unmanned devices through a wireless communication network.

[0076] In this embodiment of the invention, the wireless communication network can be any wireless communication method, such as Bluetooth, Wi-Fi, 4G, 5G, etc. The sensor module perceives the external environment through five sensors: camera, lidar, millimeter-wave radar, satellite navigation system, and inertial navigation system, and sends sensor data to the swarm control system for collaborative positioning of unmanned swarms in water and air.

[0077] S102. Obtain semantic point cloud information and cluster positioning information for each of the unmanned devices through the sensor data.

[0078] S103. Based on the semantic point cloud information and the cluster positioning information, perform map synchronization on different unmanned devices to obtain the unmanned cluster mapping data of the unmanned cluster.

[0079] Furthermore, step S102, the step of obtaining semantic point cloud information and cluster positioning information of each unmanned device in the unmanned cluster through the sensor data, includes the following sub-steps:

[0080] The sensor data acquired by each of the unmanned devices is preprocessed;

[0081] For each of the sensor data, a preset semantic segmentation network is used to perform semantic segmentation to obtain the semantic point cloud information;

[0082] The Kalman filter method based on the uniform motion assumption obtains relative position and velocity observations between different unmanned devices based on the semantic point cloud information.

[0083] Based on the relative position and velocity observations, as well as the sensor data, the multi-constraint error state Kalman filter method is used to process the data to obtain the cluster positioning information of each unmanned device in the unmanned swarm.

[0084] Sensor data preprocessing may include the following steps: extracting visual feature points from camera images, compensating for motion distortion in lidar point clouds, filtering out abnormal measurements from millimeter-wave radar, and filtering inertial measurement data from inertial navigation systems.

[0085] The purpose of semantic information extraction is to obtain semantic point cloud information. Camera images are first used to perform inference using a semantic segmentation network to obtain semantically segmented images. The semantic segmentation network in this embodiment of the invention can be any real-time network, such as Nano-SAM, MobileNet, etc. The LiDAR point cloud is projected onto the semantically segmented image through extrinsic and intrinsic parameter matrices to obtain semantic point cloud information.

[0086] Furthermore, step S103, which involves synchronizing maps of different unmanned devices based on the semantic point cloud information and cluster positioning information to obtain unmanned cluster mapping data, includes the following sub-steps:

[0087] A semantic occupancy grid map for unmanned cluster map synchronization is constructed in different unmanned devices;

[0088] The communication mechanism between different unmanned devices is based on a request and response mechanism, or the map update data based on the semantic point cloud information and the cluster positioning information is transmitted through the cluster control system of the unmanned devices.

[0089] In each of the unmanned devices, based on the map update data, Hilbert curve encoding is used to compress the data to obtain a semantic instance compressed package. Then, the semantic occupancy raster map is updated according to the semantic instance compressed package, and the updated semantic occupancy raster map is output as the mapping data of the unmanned cluster.

[0090] Furthermore, the Kalman filter method based on the assumption of uniform motion, specifically involves the following steps for obtaining relative position and velocity observations between different unmanned devices based on the semantic point cloud information:

[0091] Let unmanned device i track and identify unmanned device j. The state vector of the Kalman filter method based on the uniform motion assumption at time k satisfies the following relationship:

[0092]

[0093] in, It is the unmanned equipment j machine system at time k. j Compared to unmanned equipment i-machine system I i Location, It is the unmanned equipment j machine system at time k. j In the local world system of unmanned equipment i speed;

[0094] The Kalman filter method based on the uniform motion assumption consists of two parts: forward propagation and observation update. The forward propagation of the Kalman filter method based on the uniform motion assumption satisfies the following relationship:

[0095]

[0096] in, It is the prior state at time k. It is the posterior state at time k-1, F track,k-1 It is the state transition matrix. It is the prior covariance at time k. It is the posterior covariance at time k-1, Q track It is the predicted noise matrix;

[0097] In the semantic point cloud information at time k, find the point cluster corresponding to the unmanned device j as the observation value z for the relative position and velocity observation. track,k The observation update of the Kalman filter method based on the uniform motion assumption satisfies the following relationship:

[0098]

[0099]

[0100] Among them, h track The (·) function is the observation equation, H track,k It is the observation matrix. R is the posterior covariance at time k. track,k It is the noise matrix of the observation.

[0101] Furthermore, the step of processing the relative position and velocity observations and the sensor data using a multi-constraint error state Kalman filter method to obtain the cluster positioning information of each unmanned device in the unmanned swarm is as follows:

[0102] Define the state vector x of unmanned device i at time k. i,k for:

[0103]

[0104] in, Let i be the inertial state of unmanned equipment i at time k; Let i be the clone state of unmanned device i at time k, used to maintain the pose of c historical time points; Let be the cluster extrinsic state of unmanned device i at time k, used to maintain the local world system extrinsic parameters of the other n unmanned devices;

[0105] The multi-constraint error state Kalman filter method comprises three parts: forward propagation, state augmentation, and observation update. The forward propagation of the multi-constraint error state Kalman filter method satisfies the following relationship:

[0106]

[0107] in, Let f(·) be the prior state of unmanned device i at time k, and let f(·) be the equation of motion. Let a be the posterior state of unmanned device i at time k-1. m,k and w m,k These are the acceleration and angular velocity measurements of the inertial system at time k, respectively. F is the prior covariance of unmanned device i at time k. k-1 It is a state matrix. Q is the posterior covariance of unmanned device i at time k-1. k-1 It is the noise matrix of the inertial system at time k-1;

[0108] The state augmentation of the multi-constraint error state Kalman filter method satisfies the following relationship:

[0109]

[0110] Where A is the augmented Jacobian matrix and I is the identity matrix;

[0111] The observation update of the multi-constraint error state Kalman filter method satisfies the following relationship:

[0112]

[0113] Among them, z m,k The relative position and velocity observations, or the observed values ​​of the cluster positioning information corresponding to the sensor data, are given, where h(·) is the observation function and n is the number of observations. m,k To observe noise.

[0114] Furthermore, at time K, the observed value of the cluster positioning information corresponding to the camera of unmanned device i is defined as z. cam,kThe observed value of the cluster positioning information corresponding to the lidar is z. lidar,k The observed value of the cluster positioning information corresponding to the millimeter-wave radar is z. radar,k The observed value of the cluster positioning information corresponding to the satellite navigation sensor is z. gnss,k The observed values ​​z of the relative position and velocity observations track,k Includes active observation z active,k and passive observation z passive,k ,in:

[0115]

[0116] h is the visual feature point observed by unmanned device i at time k. d (·) is the camera's distortion function, h p (·) is the camera's reprojection function, h t (·) is the rigid body transformation function, n cam It is the camera's observation noise;

[0117]

[0118] p L It is the laser point obtained by the lidar, laser point p L By using nearest neighbor search, the five nearest laser points are obtained, and the plane normal vector u and center point q fitted to these five laser points are calculated. and It is the extrinsic transformation matrix of the laser-radar to unmanned equipment system; b lidar It is the observation noise of the lidar;

[0119]

[0120] v R and p R These are the Doppler velocity and three-dimensional coordinates obtained from millimeter-wave radar. It is the extrinsic transformation matrix of the millimeter-wave radar to unmanned vehicle system, and the ||·| function is used to calculate the magnitude of the vector, n radar It is the observation noise of millimeter-wave radar;

[0121]

[0122] and It is the external parameter transformation between the local world system and the global world system w of the unmanned device i. It is the position observation of unmanned equipment i by the satellite navigation system at time k, n gnss This is observation noise from the satellite navigation system;

[0123]

[0124] The observed value z, representing the relative position and velocity, is obtained by tracking and identifying unmanned device j from unmanned device i at time k. track,k Relative position observation, and It is the external parameter transformation between the local world system of unmanned device i and the local world system of unmanned device j, n active It is observation noise from active observation;

[0125]

[0126] and It is the pose of the unmanned device j at time k. All are cluster positioning information sent by unmanned device j to unmanned device i, n passive It is observation noise from passive observation.

[0127] Furthermore, the observation update of the multi-constraint error state Kalman filter method also satisfies the following relationship:

[0128]

[0129] in, Let be the posterior state of unmanned device i at time k. For generalized addition, Let H be the posterior covariance of unmanned device i at time k. k Let R be the observation matrix. m,k This is the observation noise matrix.

[0130] In this embodiment of the invention, collaborative positioning, map building, and synchronization are performed separately. Step S102 realizes collaborative positioning between different unmanned devices, while step S103 realizes map building and synchronization between unmanned devices and cluster control systems.

[0131] In this embodiment of the invention, the purpose of using Hilbert curve encoding and compression is to compress the data of a semantic instance that has undergone state update in the semantic occupation map update, thereby optimizing the storage space of the map database and reducing the communication bandwidth requirements for map synchronization.

[0132] Furthermore, the semantic occupancy raster map includes four pieces of information: voxel index, timestamp, occupancy attribute, and semantic attribute. In each of the unmanned devices, the step of compressing the map update data using Hilbert curve encoding to obtain a semantic instance compressed package includes the following sub-steps:

[0133] A local voxel map of semantic instances is constructed based on the map update data. The minimum hash index and maximum hash index of the semantic instances in the map update data are counted to obtain a voxel map composed of Boolean values. Voxel attributes belonging to semantic instances are denoted as 1, and voxel attributes not belonging to semantic instances are denoted as 0.

[0134] The local voxel map is Hilbert curve encoded to obtain a one-dimensional Boolean array;

[0135] The one-dimensional Boolean array is serialized to obtain a one-dimensional data stream. Specifically, in the implementation process, the data stream consists of a low address, a high address, the number of voxels, a timestamp, semantic attributes, and a one-dimensional Boolean array. The low address and high address are the minimum hash index and the maximum hash index of the local voxel map, respectively, represented by 32-bit unsigned integers. The number of voxels is the number of voxels in the local voxel map, represented by 32-bit unsigned integers. The timestamp is represented by a 64-bit double-precision floating-point number, and the semantic attributes are represented by 8-bit unsigned integers.

[0136] The one-dimensional data stream is compressed using Hough coding to obtain the semantic instance compressed package.

[0137] Furthermore, the communication mechanism for the request and response is specifically as follows:

[0138] The requester broadcasts the index of the semantic instance compressed package it owns as a map synchronization request message;

[0139] After receiving the map synchronization request message, the receiver compares it with the voxel index of the semantic occupied raster map it owns, confirms the semantic instance compressed package that the requester is missing, and sends the semantic instance compressed package as a map synchronization response message to the requester.

[0140] After receiving the map synchronization response message, the requester performs data duplication filtering through index comparison, and uses the filtered map synchronization response message to update its own semantic occupied grid map and unmanned cluster mapping data.

[0141] Furthermore, the step of updating the semantic occupancy grid map based on the semantic instance compressed package specifically includes:

[0142] If the unmanned device receives the map update data transmitted by the cluster control system, it obtains the voxels that need to be updated through the ray casting algorithm, updates the voxel's occupancy attribute and semantic attribute using the maximum a posteriori probability algorithm, and then uses the voxel clustering algorithm to cluster the voxels according to the semantic category to obtain multiple semantic instances. The semantic instances are used to replace the voxels that need to be updated in the semantic occupancy raster map, and the updated semantic occupancy raster map is output as the unmanned cluster mapping data.

[0143] If the unmanned device receives map update data transmitted from other unmanned devices, it decompresses the map update data to obtain the local voxel map containing semantic instances. Based on the three-dimensional spatial coordinates of the local voxel map, it uses the maximum a posteriori probability algorithm to update the occupancy attribute and semantic attribute of the voxels at the corresponding coordinates in the semantic occupancy raster map. Then, it uses a voxel clustering algorithm to cluster the voxels according to semantic categories to obtain the semantic occupancy raster map updated based on the local voxel map. The updated semantic occupancy raster map is then output as the mapping data for the unmanned cluster.

[0144] The beneficial effects achieved by this invention lie in proposing a collaborative localization and mapping method for unmanned swarms in cross-domain water and air environments based on multiple sensors and data update mechanisms. This method is based on a multi-constraint error state Kalman filter and integrates various observation information such as camera, lidar, millimeter-wave radar, satellite navigation, inertial navigation, and swarm relative observation. It can leverage the complementary advantages of different sensors to overcome the differences in perception range between unmanned swarms in cross-domain water and air environments, enabling swarm collaborative localization to obtain smooth relative positioning and globally consistent absolute positioning information in cross-domain water and air environments. At the same time, based on the communication mechanism of map synchronization request and response, and the encoding and compression algorithm based on Hilbert curves, the unmanned swarm can achieve low-bandwidth, low-latency semantic occupancy grid synchronization, thereby realizing robust collaborative localization and low-latency environmental map synchronization for unmanned swarms in cross-domain water and air environments.

[0145] Those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by a computer program instructing related hardware. The program can be stored in a computer-readable storage medium, and when executed, it can include the processes of the embodiments of the above methods. The storage medium can be a magnetic disk, optical disk, read-only memory (ROM), or random access memory (RAM), etc.

[0146] It should be noted that, in this document, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Unless otherwise specified, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes that element.

[0147] Through the above description of the embodiments, those skilled in the art can clearly understand that the methods of the above embodiments can be implemented by means of software plus necessary general-purpose hardware platforms. Of course, they can also be implemented by hardware, but in many cases the former is a better implementation method. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product is stored in a storage medium (such as ROM / RAM, magnetic disk, optical disk), and includes several instructions to cause a terminal (which may be a mobile phone, computer, server, air conditioner, or network device, etc.) to execute the methods described in the various embodiments of the present invention.

[0148] The embodiments of the present invention have been described above with reference to the accompanying drawings. The disclosed embodiments are merely preferred embodiments of the present invention. However, the present invention is not limited to the specific embodiments described above. The specific embodiments described above are merely illustrative and not restrictive. Those skilled in the art can make many equivalent changes in form without departing from the spirit and scope of the claims of the present invention, and all such changes are within the protection scope of the present invention.

Claims

1. A method for cross-domain unmanned swarm cooperative localization and mapping, characterized in that, Includes the following steps: Sensor data is acquired through the sensor modules of each unmanned device in the unmanned swarm. The sensor modules include cameras, lidar, millimeter-wave radar, satellite navigation sensors, and inertial navigation sensors. Data is transmitted between different unmanned devices through a wireless communication network. The semantic point cloud information and cluster positioning information of each of the unmanned devices are obtained through the sensor data. Based on the semantic point cloud information and the cluster positioning information, map synchronization is performed on different unmanned devices to obtain unmanned cluster mapping data of the unmanned cluster. The step of acquiring semantic point cloud information and cluster positioning information of each unmanned device in the unmanned cluster through the sensor data includes the following sub-steps: The sensor data acquired by each of the unmanned devices is preprocessed; For each of the sensor data, a preset semantic segmentation network is used to perform semantic segmentation to obtain the semantic point cloud information; The Kalman filter method based on the uniform motion assumption obtains relative position and velocity observations between different unmanned devices based on the semantic point cloud information. Based on the relative position and velocity observations, as well as the sensor data, the multi-constraint error state Kalman filter method is used to process the data to obtain the cluster positioning information of each unmanned device in the unmanned swarm.

2. The water-air cross-domain unmanned swarm cooperative localization and mapping method according to claim 1, characterized in that, The step of synchronizing maps of different unmanned devices based on the semantic point cloud information and cluster positioning information to obtain unmanned cluster mapping data includes the following sub-steps: A semantic occupancy grid map for unmanned cluster map synchronization is constructed in different unmanned devices; The communication mechanism between different unmanned devices is based on a request and response mechanism, or the map update data based on the semantic point cloud information and the cluster positioning information is transmitted through the cluster control system of the unmanned devices. In each of the unmanned devices, based on the map update data, Hilbert curve encoding is used to compress the data to obtain a semantic instance compressed package. Then, the semantic occupancy raster map is updated according to the semantic instance compressed package, and the updated semantic occupancy raster map is output as the mapping data of the unmanned cluster.

3. The water-air cross-domain unmanned swarm cooperative localization and mapping method according to claim 1, characterized in that, The Kalman filter method based on the assumption of uniform motion, specifically involves the following steps for obtaining relative position and velocity observations between different unmanned devices based on the semantic point cloud information: Let unmanned device i track and identify unmanned device j. The state vector of the Kalman filter method based on the uniform motion assumption at time k satisfies the following relationship: ； in, It is the unmanned equipment j-machine system at time k. Compared to unmanned equipment i-machine system Location, It is the unmanned equipment j-machine system at time k. In the local world system of unmanned equipment speed; The Kalman filter method based on the uniform motion assumption consists of two parts: forward propagation and observation update. The forward propagation of the Kalman filter method based on the uniform motion assumption satisfies the following relationship: ； ； in, It is the prior state at time k. It is the posterior state at time k-1. It is the state transition matrix. It is the prior covariance at time k. It is the posterior covariance at time k-1. It is the predicted noise matrix; In the semantic point cloud information at time k, find the point cluster corresponding to the unmanned device j as the observation value for the relative position and velocity observation. The observation update of the Kalman filter method based on the uniform motion assumption satisfies the following relationship: ； ； ； in, The function is the observation equation. It is the observation matrix. It is the posterior covariance at time k. It is the noise matrix of the observation.

4. The water-air cross-domain unmanned swarm cooperative localization and mapping method according to claim 3, characterized in that, The step of obtaining the cluster positioning information of each unmanned device in the unmanned swarm based on the relative position and velocity observations and the sensor data using the multi-constraint error state Kalman filter method is as follows: Define the state vector of unmanned device i at time k. for: ； in, Let i be the inertial state of unmanned equipment i at time k; Let i be the clone state of unmanned device i at time k, used to maintain the pose of c historical time points; Let be the cluster extrinsic state of unmanned device i at time k, used to maintain the local world system extrinsic parameters of the other n unmanned devices; The multi-constraint error state Kalman filter method comprises three parts: forward propagation, state augmentation, and observation update. The forward propagation of the multi-constraint error state Kalman filter method satisfies the following relationship: ； ； in, It is the prior state of unmanned device i at time k. It is the equation of motion. It is the posterior state of unmanned device i at time k-1. and These are the acceleration and angular velocity measurements of the inertial system at time k, respectively. It is the prior covariance of unmanned device i at time k. It is a state matrix. It is the posterior covariance of unmanned device i at time k-1. It is the noise matrix of the inertial system at time k-1; The state augmentation of the multi-constraint error state Kalman filter method satisfies the following relationship: ； in, To augment the Jacobian matrix, It is the identity matrix; The observation update of the multi-constraint error state Kalman filter method satisfies the following relationship: ； in, The relative position and velocity observations, or the observed values ​​of the cluster positioning information corresponding to the sensor data. For the observation function, To observe noise.

5. The water-air cross-domain unmanned swarm cooperative localization and mapping method according to claim 4, characterized in that, Defined at time K, the observed value of the cluster positioning information corresponding to the camera of unmanned device i is... The observed value of the cluster positioning information corresponding to the lidar is The observed value of the cluster positioning information corresponding to the millimeter-wave radar is The observed value of the cluster positioning information corresponding to the satellite navigation sensor is The observed values ​​of the relative position and velocity. Includes active observation and passive observation ,in: ； These are the visual feature points observed by the unmanned device i at time k. It is the camera's distortion function. It is the camera's reprojection function. It is a rigid body transformation function. It is the camera's observation noise; ； These are laser points obtained by lidar. By using nearest neighbor search, the five nearest laser points are obtained, and the plane normal vector fitted to these five laser points is calculated. and center point ; and It is the external parameter transformation matrix of the laser-radar to unmanned equipment i-machine system; It is the observation noise of the lidar; ； ； and These are the Doppler velocity and three-dimensional coordinates obtained from millimeter-wave radar. It is the extrinsic transformation matrix of the millimeter-wave radar to unmanned equipment system. The function calculates the magnitude of a vector. It is the observation noise of millimeter-wave radar; ； and It is the external parameter transformation between the local world system and the global world system w of the unmanned device i. It is the position observation of unmanned device i by the satellite navigation system at time k. This is observation noise from the satellite navigation system; ； The relative position and velocity observation values ​​obtained by unmanned device i tracking and identifying unmanned device j at time k are... Relative position observation, and It is the external parameter transformation between the local world system of unmanned device i and the local world system of unmanned device j. It is observation noise from active observation; ； and It is the pose of the unmanned device j at time k. All of these are cluster positioning information sent from unmanned device j to unmanned device i. It is observation noise from passive observation.

6. The water-air cross-domain unmanned swarm cooperative localization and mapping method according to claim 4, characterized in that, The observation update of the multi-constraint error state Kalman filter method also satisfies the following relationship: ； ； ； in, Let be the posterior state of unmanned device i at time k. For generalized addition, Let be the posterior covariance of unmanned device i at time k. For the observation matrix, This is the observation noise matrix.

7. The water-air cross-domain unmanned swarm cooperative localization and mapping method according to claim 2, characterized in that, The semantic occupancy raster map includes four pieces of information: voxel index, timestamp, occupancy attribute, and semantic attribute. In each of the unmanned devices, the step of compressing the map update data using Hilbert curve encoding to obtain a semantic instance compressed package includes the following sub-steps: A local voxel map of semantic instances is constructed based on the map update data. The minimum hash index and maximum hash index of the semantic instances in the map update data are counted to obtain a voxel map composed of Boolean values. Voxel attributes belonging to semantic instances are denoted as 1, and voxel attributes not belonging to semantic instances are denoted as 0. The local voxel map is Hilbert curve encoded to obtain a one-dimensional Boolean array; The one-dimensional Boolean array is serialized to obtain a one-dimensional data stream; The one-dimensional data stream is compressed using Hough coding to obtain the semantic instance compressed package.

8. The water-air cross-domain unmanned swarm cooperative localization and mapping method according to claim 2, characterized in that, The communication mechanism for the request and response is as follows: The requester broadcasts the index of the semantic instance compressed package it owns as a map synchronization request message; After receiving the map synchronization request message, the receiver compares it with the voxel index of the semantic occupied raster map it owns, confirms the semantic instance compressed package that the requester is missing, and sends the semantic instance compressed package as a map synchronization response message to the requester. After receiving the map synchronization response message, the requester performs data duplication filtering through index comparison, and uses the filtered map synchronization response message to update its own semantic occupied grid map and unmanned cluster mapping data.

9. The water-air cross-domain unmanned swarm cooperative localization and mapping method according to claim 7, characterized in that, The steps for updating the semantically occupied grid map based on the semantic instance compressed package are as follows: If the unmanned device receives the map update data transmitted by the cluster control system, it obtains the voxels that need to be updated through the ray casting algorithm, updates the voxel's occupancy attribute and semantic attribute using the maximum a posteriori probability algorithm, and then uses the voxel clustering algorithm to cluster the voxels according to the semantic category to obtain multiple semantic instances. The semantic instances are used to replace the voxels that need to be updated in the semantic occupancy raster map, and the updated semantic occupancy raster map is output as the unmanned cluster mapping data. If the unmanned device receives map update data transmitted from other unmanned devices, it decompresses the map update data to obtain the local voxel map containing semantic instances. Based on the three-dimensional spatial coordinates of the local voxel map, it uses the maximum a posteriori probability algorithm to update the occupancy attribute and semantic attribute of the voxels at the corresponding coordinates in the semantic occupancy raster map. Then, it uses a voxel clustering algorithm to cluster the voxels according to semantic categories to obtain the semantic occupancy raster map updated based on the local voxel map. The updated semantic occupancy raster map is then output as the mapping data for the unmanned cluster.

Images