Water-air cross-domain unmanned cluster cooperative positioning and mapping method

Abstract

The invention relates to the technical field of unmanned aerial vehicle clusters, in particular to a water-air cross-domain unmanned cluster cooperative positioning and mapping method. The method is based on a multi-constraint error state Kalman filter, integrates various observation information such as a camera, a laser radar, a millimeter wave radar, satellite navigation, inertial navigation and cluster relative observation, can overcome the difference of sensing ranges between water-air cross-domain unmanned clusters by utilizing complementary advantages between sensors, and improves the sensing accuracy of the water-air cross-domain unmanned clusters. Smooth relative positioning and absolute positioning information with global consistency can be obtained in a water-air cross-domain environment through cluster cooperative positioning; meanwhile, based on a map synchronization request and response communication mechanism and based on a Hilbert curve coding and compression algorithm, small-bandwidth and low-delay semantic occupation grid synchronization can be realized in the unmanned cluster, so that robust cooperative positioning and low-delay environmental map synchronization of the water-air cross-domain unmanned cluster are realized.

Description

Technical Field

[0001] The present invention relates to the field of unmanned aerial vehicle (UAV) cluster technology, and in particular to a water-air cross-domain unmanned aerial vehicle (UAV) cluster collaborative positioning and mapping method. Background Art

[0002] Compared with the traditional single-domain cluster system, the cross-domain cluster system composed of unmanned equipment such as drones and unmanned boats has the characteristics of information complementarity and spatial linkage. It can be applied to autonomous inspection, search and rescue, transportation, cleaning and other tasks, and has broad application prospects. Collaborative positioning, mapping and information sharing are important prerequisites for the autonomous collaboration of the unmanned cluster.

[0003] However, most existing research focuses on collaborative localization and mapping in a single domain. In view of the differences in perception range and communication capabilities of cross-domain unmanned swarms in water and air, there is currently a lack of available localization and mapping solutions.

[0004] How to overcome the differences in perception range between cross-domain water and air unmanned clusters through multi-sensor fusion and achieve robust collaborative positioning, and how to design information compression algorithms to achieve low-latency environmental map synchronization under limited bandwidth are technical challenges to improve the intelligence of cross-domain water and air unmanned clusters. Summary of the invention

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

[0006] In order to solve the above technical problems, the present invention provides a water-air cross-domain unmanned cluster collaborative positioning and mapping method, comprising the following steps:

[0007] Acquire sensor data through the sensor module of each unmanned device in the unmanned cluster, wherein the sensor module includes a camera, a laser radar, a millimeter wave radar, a satellite navigation sensor and an inertial navigation sensor, and different unmanned devices transmit data through a wireless communication network;

[0008] Acquire semantic point cloud information and cluster positioning information of each of the unmanned devices through the sensor data;

[0009] Based on the semantic point cloud information and the cluster positioning information, maps of different unmanned devices are synchronized to obtain unmanned cluster mapping data of the unmanned cluster.

[0010] Furthermore, 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:

[0011] Preprocessing the sensor data acquired by each of the unmanned devices;

[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] A Kalman filter method based on uniform motion assumption is used to obtain relative position and speed observations between different unmanned devices according to the semantic point cloud information;

[0014] Based on the relative position and speed observations, and the sensor data, a multi-constrained error state Kalman filter method is used for processing to obtain the cluster positioning information of each unmanned device in the unmanned cluster.

[0015] Furthermore, based on the semantic point cloud information and cluster positioning information, the map of different unmanned devices is synchronized to obtain the unmanned cluster mapping data of the unmanned cluster, including the following sub-steps:

[0016] Constructing a semantic occupancy grid map for unmanned cluster map synchronization in different said unmanned devices;

[0017] Transmitting map update data based on the semantic point cloud information and the cluster positioning information through a communication mechanism based on requests and responses between different unmanned devices, or through a cluster control system of the unmanned devices;

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

[0019] Furthermore, based on the Kalman filter method of uniform motion assumption, the steps of obtaining relative position and speed observations between different unmanned devices according to the semantic point cloud information are specifically as follows:

[0020] Define unmanned device i to track and identify unmanned device j. The Kalman filter method of the uniform motion assumption satisfies the following relationship at time k:

[0021]

[0022] in, is the unmanned equipment j-machine system I at time k j Compared with unmanned equipment i machine system I i location, is the unmanned equipment j-machine system I at time k j In the unmanned device i local world system G i speed;

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

[0024]

[0025]

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

[0027] Find the point cluster corresponding to unmanned device j in the semantic point cloud information at time k as the observation value z of the relative position and speed observation track,k , the observation update of the Kalman filter method of the uniform motion assumption satisfies the following relationship:

[0028]

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

[0030] Furthermore, based on the relative position and speed observations and the sensor data, a multi-constrained error state Kalman filter method is used for processing to obtain the cluster positioning information of each unmanned device in the unmanned cluster, specifically:

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

[0032]

[0033] in, is the inertial state of unmanned equipment i at time k; is the cloned state of unmanned device i at time k, which is used to maintain the position and posture of c historical moments; is the cluster external parameter state of unmanned device i at time k, which is used to maintain the local world system external parameters of other n unmanned devices;

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

[0035]

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

[0037] The state augmentation of the multi-constrained 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-constrained error state Kalman filter method satisfies the following relationship:

[0041]

[0042] Among them, z m,k is the relative position and velocity observation, or the observed value of the cluster positioning information corresponding to the sensor data, h(·) is the observation function, n m,k is the observation noise.

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

[0044]

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

[0046]

[0047] p L is the laser point obtained by the laser radar, and the laser point p L Through the nearest neighbor search, the five adjacent laser points are obtained, and the plane normal vector u and the center point q fitted by these five laser points are calculated; and is the external parameter transformation matrix from the laser radar to the unmanned equipment i system; n lidar is the observation noise of the lidar;

[0048]

[0049] v R and p R They are the Doppler velocity and three-dimensional coordinates obtained by the millimeter-wave radar, is the external parameter transformation matrix from the millimeter wave radar to the unmanned equipment i system, the ‖·‖ function is to calculate the modulus length of the vector, n radar is the observation noise of the millimeter-wave radar;

[0050]

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

[0052]

[0053] is the relative position and speed observation value z obtained by unmanned device i tracking and identifying unmanned device j at time k track,k The relative position observation of and 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 is the observation noise of active observation;

[0054]

[0055] and is the position of unmanned equipment j at time k, are all cluster positioning information sent by unmanned device j to unmanned device i, n passive is the observation noise of passive observation.

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

[0057]

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

[0059] Furthermore, the semantic occupancy grid map includes four information: voxel index, timestamp, occupancy attribute and semantic attribute. In each of the unmanned devices, based on the map update data, Hilbert curve coding is used for compression to obtain a semantic instance compression package, including the following sub-steps:

[0060] Constructing a local voxel map of the semantic instance based on the map update data, wherein the minimum hash index and the maximum hash index of the semantic instance in the map update data are counted to obtain a voxel map composed of Boolean values, wherein the voxel attribute belonging to the semantic instance is recorded as 1, and the voxel attribute not belonging to the semantic instance is recorded as 0;

[0061] Performing Hilbert curve encoding on the local voxel map to obtain a one-dimensional Boolean array;

[0062] Serializing the one-dimensional Boolean array to obtain a one-dimensional data stream;

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

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

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

[0066] After receiving the map synchronization request message, the receiver compares the voxel index of the semantic occupancy grid map owned by itself with the voxel index, confirms the semantic instance compressed package that the requester lacks, and sends the semantic instance compressed package to the requester as a map synchronization response message;

[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 occupancy grid map and the unmanned cluster mapping data.

[0068] Furthermore, the step of updating the semantic occupancy grid map according to the semantic instance compression package is specifically as follows:

[0069] If the unmanned device receives the map update data transmitted by the cluster control system, the voxels that need to be updated are obtained by a ray casting algorithm, and the occupancy attributes and semantic attributes of the voxels are updated by a maximum a posteriori probability algorithm. After that, the voxels are clustered according to the semantic categories using a voxel clustering algorithm to obtain a plurality of semantic instances, and the semantic instances are used to replace the voxels that need to be updated in the semantic occupancy grid map, and the updated semantic occupancy grid map is output as the unmanned cluster mapping data;

[0070] If the unmanned device receives the map update data transmitted by other unmanned devices, the map update data is decompressed to obtain the local voxel map containing semantic instances, and the occupancy attributes and semantic attributes of the voxels of the corresponding coordinates in the semantic occupancy grid map are updated using the maximum a posteriori probability algorithm based on the three-dimensional spatial coordinates of the local voxel map. After that, the voxels are clustered according to semantic categories using the voxel clustering algorithm to obtain the semantic occupancy grid map updated based on the local voxel map, and the updated semantic occupancy grid map is output as the unmanned cluster mapping data.

[0071] The beneficial effect achieved by the present invention is that a method for collaborative positioning and mapping of a water-air cross-domain unmanned cluster based on multiple sensors and data update mechanisms is proposed. The method is based on a multi-constrained error state Kalman filter, and integrates multiple observation information such as cameras, lidars, millimeter-wave radars, satellite navigation, inertial navigation and cluster relative observations. It can utilize the complementary advantages between sensors and overcome the differences in perception ranges between water-air cross-domain unmanned clusters, so that cluster collaborative positioning can obtain smooth relative positioning and absolute positioning information with global consistency in a water-air cross-domain environment; at the same time, based on the communication mechanism of map synchronization request and response, and the encoding and compression algorithm based on the Hilbert curve, small-bandwidth, low-latency semantic occupancy grid synchronization can be achieved within the unmanned cluster, thereby realizing robust collaborative positioning and low-latency environmental map synchronization of water-air cross-domain unmanned clusters. BRIEF DESCRIPTION OF THE DRAWINGS

[0072] Figure 1 It is a schematic diagram of the steps of the water-air cross-domain unmanned cluster collaborative positioning and mapping method provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0073] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0074] Please refer to Figure 1 , Figure 1 1 is a schematic diagram of the steps of the water-air cross-domain unmanned cluster collaborative positioning and mapping method provided by an embodiment of the present invention, and the water-air cross-domain unmanned cluster collaborative positioning and mapping method includes the following steps:

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

[0076] In the embodiment of the present invention, the wireless communication network can be any wireless communication mode, such as Bluetooth, Wi-Fi, 4G, 5G and other communication modes. The sensor module senses the external environment through five sensors, namely, camera, laser radar, millimeter wave radar, satellite navigation system and inertial navigation system, and sends sensor data to the cluster control system of water-air cross-domain unmanned cluster collaborative positioning.

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

[0078] S103: Based on the semantic point cloud information and the cluster positioning information, synchronize maps of different unmanned devices to obtain 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] Preprocessing the sensor data acquired by each of the unmanned devices;

[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] A Kalman filter method based on uniform motion assumption is used to obtain relative position and speed observations between different unmanned devices according to the semantic point cloud information;

[0083] Based on the relative position and speed observations, and the sensor data, a multi-constrained error state Kalman filter method is used for processing to obtain the cluster positioning information of each unmanned device in the unmanned cluster.

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

[0085] The purpose of semantic information extraction is to obtain semantic point cloud information. The camera image is first inferred using a semantic segmentation network to obtain a semantic segmentation image. The semantic segmentation network in the embodiment of the present invention can be any real-time network, such as Nano-SAM, MobileNet, etc. The lidar point cloud is projected into the semantic segmentation image through an external parameter matrix and an internal parameter matrix, thereby obtaining semantic point cloud information.

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

[0087] Constructing a semantic occupancy grid map for unmanned cluster map synchronization in different said unmanned devices;

[0088] Transmitting map update data based on the semantic point cloud information and the cluster positioning information through a communication mechanism based on requests and responses between different unmanned devices, or through a cluster control system of the unmanned devices;

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

[0090] Furthermore, based on the Kalman filter method of uniform motion assumption, the steps of obtaining relative position and speed observations between different unmanned devices according to the semantic point cloud information are specifically as follows:

[0091] Define unmanned device i to track and identify unmanned device j. The Kalman filter method of the uniform motion assumption satisfies the following relationship at time k:

[0092]

[0093] in, is the unmanned equipment j-machine system I at time k j Compared with unmanned equipment i machine system I i location, is the unmanned equipment j-machine system I at time k j In the unmanned device i local world system G i speed;

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

[0095]

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

[0097] Find the point cluster corresponding to unmanned device j in the semantic point cloud information at time k as the observation value z of the relative position and speed observation track,k , the observation update of the Kalman filter method of the uniform motion assumption satisfies the following relationship:

[0098]

[0099]

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

[0101] Furthermore, based on the relative position and speed observations and the sensor data, a multi-constrained error state Kalman filter method is used for processing to obtain the cluster positioning information of each unmanned device in the unmanned cluster, specifically:

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

[0103]

[0104] in, is the inertial state of unmanned equipment i at time k; is the cloned state of unmanned device i at time k, which is used to maintain the position and posture of c historical moments; is the cluster external parameter state of unmanned device i at time k, which is used to maintain the local world system external parameters of other n unmanned devices;

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

[0106]

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

[0108] The state augmentation of the multi-constrained 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-constrained error state Kalman filter method satisfies the following relationship:

[0112]

[0113] Among them, z m,k is the relative position and velocity observation, or the observed value of the cluster positioning information corresponding to the sensor data, h(·) is the observation function, n m,k is the observation noise.

[0114] Furthermore, it is defined that at time K, the observation value of the cluster positioning information corresponding to the camera of unmanned device i is z cam,k, the observation value of the cluster positioning information corresponding to the laser radar is z lidar,k The observation value of the cluster positioning information corresponding to the millimeter wave radar is z radar,k The observation value of the cluster positioning information corresponding to the satellite navigation sensor is z gnss,k , the observed value z of the relative position and velocity observation track,k Including active observation z active,k and passive observation z passive,k ,in:

[0115]

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

[0117]

[0118] p L is the laser point obtained by the laser radar, and the laser point p L Through the nearest neighbor search, the five adjacent laser points are obtained, and the plane normal vector u and the center point q fitted by these five laser points are calculated; and is the external parameter transformation matrix from the laser radar to the unmanned equipment i system; b lidar is the observation noise of the lidar;

[0119]

[0120] v R and p R They are the Doppler velocity and three-dimensional coordinates obtained by the millimeter-wave radar, is the external parameter transformation matrix from the millimeter wave radar to the unmanned equipment i system, the ‖·‖ function is to calculate the modulus length of the vector, n radar is the observation noise of the millimeter-wave radar;

[0121]

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

[0123]

[0124] is the relative position and speed observation value z obtained by unmanned device i tracking and identifying unmanned device j at time k track,k The relative position observation of and 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 is the observation noise of active observation;

[0125]

[0126] and is the position of unmanned equipment j at time k, are all cluster positioning information sent by unmanned device j to unmanned device i, n passive is the observation noise of passive observation.

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

[0128]

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

[0130] In the embodiment of the present invention, collaborative positioning and map construction and synchronization are performed separately. Step S102 implements collaborative positioning between different unmanned devices, while step S103 implements map construction and synchronization between the unmanned devices and the cluster control system.

[0131] In the embodiment of the present invention, the purpose of using Hilbert curve encoding and compression is to compress data of a certain semantic instance in which a state update occurs during a semantic occupation map update, thereby optimizing the storage space of the map database and reducing the communication bandwidth requirement for map synchronization.

[0132] Furthermore, the semantic occupancy grid map includes four information: voxel index, timestamp, occupancy attribute and semantic attribute. In each of the unmanned devices, based on the map update data, Hilbert curve coding is used for compression to obtain a semantic instance compression package, including the following sub-steps:

[0133] Constructing a local voxel map of the semantic instance based on the map update data, wherein the minimum hash index and the maximum hash index of the semantic instance in the map update data are counted to obtain a voxel map composed of Boolean values, wherein the voxel attribute belonging to the semantic instance is recorded as 1, and the voxel attribute not belonging to the semantic instance is recorded as 0;

[0134] Performing Hilbert curve encoding on the local voxel map to obtain a one-dimensional Boolean array;

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

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

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

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

[0139] After receiving the map synchronization request message, the receiver compares the voxel index of the semantic occupancy grid map owned by itself with the voxel index, confirms the semantic instance compressed package that the requester lacks, and sends the semantic instance compressed package to the requester as a map synchronization response message;

[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 occupancy grid map and the unmanned cluster mapping data.

[0141] Furthermore, the step of updating the semantic occupancy grid map according to the semantic instance compression package is specifically as follows:

[0142] If the unmanned device receives the map update data transmitted by the cluster control system, the voxels that need to be updated are obtained by a ray casting algorithm, and the occupancy attributes and semantic attributes of the voxels are updated by a maximum a posteriori probability algorithm. After that, the voxels are clustered according to the semantic categories using a voxel clustering algorithm to obtain a plurality of semantic instances, and the semantic instances are used to replace the voxels that need to be updated in the semantic occupancy grid map, and the updated semantic occupancy grid map is output as the unmanned cluster mapping data;

[0143] If the unmanned device receives the map update data transmitted by other unmanned devices, the map update data is decompressed to obtain the local voxel map containing semantic instances, and the occupancy attributes and semantic attributes of the voxels of the corresponding coordinates in the semantic occupancy grid map are updated using the maximum a posteriori probability algorithm based on the three-dimensional spatial coordinates of the local voxel map. After that, the voxels are clustered according to semantic categories using the voxel clustering algorithm to obtain the semantic occupancy grid map updated based on the local voxel map, and the updated semantic occupancy grid map is output as the unmanned cluster mapping data.

[0144] The beneficial effect achieved by the present invention is that a method for collaborative positioning and mapping of a water-air cross-domain unmanned cluster based on multiple sensors and data update mechanisms is proposed. The method is based on a multi-constrained error state Kalman filter, and integrates multiple observation information such as cameras, lidars, millimeter-wave radars, satellite navigation, inertial navigation and cluster relative observations. It can utilize the complementary advantages between sensors and overcome the differences in perception ranges between water-air cross-domain unmanned clusters, so that cluster collaborative positioning can obtain smooth relative positioning and absolute positioning information with global consistency in a water-air cross-domain environment; at the same time, based on the communication mechanism of map synchronization request and response, and the encoding and compression algorithm based on the Hilbert curve, small-bandwidth, low-latency semantic occupancy grid synchronization can be achieved within the unmanned cluster, thereby realizing robust collaborative positioning and low-latency environmental map synchronization of water-air cross-domain unmanned clusters.

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

[0146] It should be noted that, in this article, the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, an element defined by the sentence "comprises a ..." does not exclude the existence of other identical elements in the process, method, article or device including the element.

[0147] Through the description of the above implementation methods, those skilled in the art can clearly understand that the above-mentioned embodiment methods can be implemented by means of software plus a necessary general hardware platform, and of course by hardware, but in many cases the former is a better implementation method. Based on such an understanding, the technical solution of the present invention, or the part that contributes to the prior art, can be embodied in the form of a software product, which is stored in a storage medium (such as ROM / RAM, a magnetic disk, or an optical disk), and includes a number of instructions for enabling a terminal (which can be a mobile phone, a computer, a server, an air conditioner, or a network device, etc.) to execute the methods described in each embodiment of the present invention.

[0148] The embodiments of the present invention are described above in conjunction with the accompanying drawings. What is disclosed is only the preferred embodiment of the present invention. However, the present invention is not limited to the above-mentioned specific implementation manner. The above-mentioned specific implementation manner is only illustrative rather than restrictive. Under the enlightenment of the present invention, ordinary technicians in this field can also make many forms and equivalent changes without departing from the scope of protection of the purpose of the present invention and the claims, all of which are within the protection of the present invention.

Claims

1. A method for collaborative positioning and mapping of unmanned clusters across water and air domains, characterized in that: The following steps are involved: Acquire sensor data through the sensor module of each unmanned device in the unmanned cluster, wherein the sensor module includes a camera, a laser radar, a millimeter wave radar, a satellite navigation sensor and an inertial navigation sensor, and different unmanned devices transmit data through a wireless communication network; Acquire semantic point cloud information and cluster positioning information of each of the unmanned devices through the sensor data; Based on the semantic point cloud information and the cluster positioning information, maps of different unmanned devices are synchronized to obtain unmanned cluster mapping data of the unmanned cluster.

2. The method for collaborative positioning and mapping of cross-domain unmanned swarms in water and air according to claim 1 is characterized in that: 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: Preprocessing the sensor data acquired by each of the unmanned devices; For each of the sensor data, a preset semantic segmentation network is used to perform semantic segmentation to obtain the semantic point cloud information; A Kalman filter method based on uniform motion assumption is used to obtain relative position and speed observations between different unmanned devices according to the semantic point cloud information; Based on the relative position and speed observations, and the sensor data, a multi-constrained error state Kalman filter method is used for processing to obtain the cluster positioning information of each unmanned device in the unmanned cluster.

3. The method for collaborative positioning and mapping of cross-domain unmanned swarms in water and air according to claim 1 is characterized in that: Based on the semantic point cloud information and cluster positioning information, the step of synchronizing maps of different unmanned devices to obtain unmanned cluster mapping data of the unmanned cluster includes the following sub-steps: Constructing a semantic occupancy grid map for unmanned cluster map synchronization in different said unmanned devices; Transmitting map update data based on the semantic point cloud information and the cluster positioning information through a communication mechanism based on requests and responses between different unmanned devices, or through a cluster control system of the unmanned devices; In each of the unmanned devices, based on the map update data, Hilbert curve coding is used for compression to obtain a semantic instance compression package. Then, the semantic occupancy grid map is updated according to the semantic instance compression package, and the updated semantic occupancy grid map is output as the unmanned cluster mapping data.

4. The method for collaborative positioning and mapping of unmanned clusters across water and air domains according to claim 2 is characterized in that: The Kalman filter method based on the uniform motion assumption, the steps of obtaining the relative position and speed observation between different unmanned devices according to the semantic point cloud information, are specifically: Define unmanned device i to track and identify unmanned device j. The Kalman filter method of the uniform motion assumption satisfies the following relationship at time k: in, is the unmanned equipment j-machine system I at time k j Compared with unmanned equipment i machine system I i location, is the unmanned equipment j-machine system I at time k j In the unmanned device i local world system G i speed; The Kalman filter method of uniform motion assumption includes two parts: forward propagation and observation update. The forward propagation of the Kalman filter method of uniform motion assumption satisfies the following relationship: in, is the prior state at time k, is the posterior state at time k-1, F track,k-1 is the state transition matrix, is the prior covariance at time k, is the posterior covariance at time k-1, Q track is the predicted noise matrix; Find the point cluster corresponding to unmanned device j in the semantic point cloud information at time k as the observation value z of the relative position and speed observation track,k , the observation update of the Kalman filter method of the uniform motion assumption satisfies the following relationship: Among them, h track (·) function is the observation equation, H track,k is the observation matrix, is the posterior covariance at time k, R track,k is the observed noise matrix.

5. The method for collaborative positioning and mapping of cross-domain unmanned swarms in water and air according to claim 4 is characterized in that: Based on the relative position and speed observations and the sensor data, a multi-constrained error state Kalman filter method is used for processing to obtain the cluster positioning information of each unmanned device in the unmanned cluster, specifically comprising: Define the state vector x of unmanned device i at time k i,k for: in, is the inertial state of unmanned equipment i at time k; is the cloned state of unmanned device i at time k, which is used to maintain the position and posture of c historical moments; is the cluster external parameter state of unmanned device i at time k, which is used to maintain the local world system external parameters of other n unmanned devices; The multi-constrained error state Kalman filter method includes three parts: forward propagation, state augmentation and observation update. The forward propagation of the multi-constrained error state Kalman filter method satisfies the following relationship: in, is the prior state of unmanned equipment i at time k, f(·) is the motion equation, is the posterior state of unmanned device i at time k-1, a m,k and w m,k They are respectively the acceleration measurement and angular velocity measurement of the inertial system at time k, is the prior covariance of unmanned device i at time k, F k-1 is the state matrix, is the posterior covariance of unmanned device i at time k-1, Q k-1 is the noise matrix of the inertial system at time k-1; The state augmentation of the multi-constrained error state Kalman filter method satisfies the following relationship: Where A is the augmented Jacobian matrix and I is the identity matrix; The observation update of the multi-constrained error state Kalman filter method satisfies the following relationship: Among them, z m,k is the relative position and velocity observation, or the observed value of the cluster positioning information corresponding to the sensor data, h(·) is the observation function, n m,k is the observation noise.

6. The method for collaborative positioning and mapping of unmanned clusters across water and air domains according to claim 5 is characterized in that: Define that at time K, the observation value of the cluster positioning information corresponding to the camera of unmanned device i is z cam,k , the observation value of the cluster positioning information corresponding to the laser radar is z lidar,k The observation value of the cluster positioning information corresponding to the millimeter wave radar is z radar,k The observation value of the cluster positioning information corresponding to the satellite navigation sensor is z gnss,k , the observed value z of the relative position and velocity observation track,k Including active observation z active,k and passive observation z passive,k ,in: is the visual feature point observed by unmanned device i at time k, h d (·) is the distortion function of the camera, h p (·) is the camera reprojection function, h t (·) is the rigid body transformation function, n cam is the observation noise of the camera; p L is the laser point obtained by the laser radar, and the laser point p L Through the nearest neighbor search, the five adjacent laser points are obtained, and the plane normal vector u and the center point q fitted by these five laser points are calculated; and is the external parameter transformation matrix from the laser radar to the unmanned equipment i system; n lidar is the observation noise of the lidar; v R and p R They are the Doppler velocity and three-dimensional coordinates obtained by the millimeter-wave radar, is the external parameter transformation matrix of the millimeter wave radar to the unmanned equipment i system, the ‖·‖ function is to calculate the modulus length of the vector, n radar is the observation noise of the millimeter-wave radar; and is the external parameter transformation between the local world system of unmanned device i and the global world system w, is the position observation of unmanned equipment i by the satellite navigation system at time k, n gnss is the observation noise of the satellite navigation system; is the relative position and speed observation value z obtained by unmanned device i tracking and identifying unmanned device j at time k track,k The relative position observation of and 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 is the observation noise of active observation; and is the position of unmanned equipment j at time k, are all cluster positioning information sent by unmanned device j to unmanned device i, n passive is the observation noise of passive observation.

7. The method for collaborative positioning and mapping of unmanned clusters across water and air domains according to claim 5 is characterized in that: The observation update of the multi-constrained error state Kalman filter method also satisfies the following relationship: in, is the posterior state of unmanned device i at time k, is generalized addition, is the posterior covariance of unmanned equipment i at time k, H k is the observation matrix, R m,k is the observation noise matrix.

8. The method for collaborative positioning and mapping of unmanned clusters across water and air domains according to claim 3 is characterized in that: The semantic occupancy grid map includes four information: voxel index, timestamp, occupancy attribute and semantic attribute. In each of the unmanned devices, based on the map update data, Hilbert curve coding is used for compression to obtain a semantic instance compression package, including the following sub-steps: Constructing a local voxel map of the semantic instance based on the map update data, wherein the minimum hash index and the maximum hash index of the semantic instance in the map update data are counted to obtain a voxel map composed of Boolean values, wherein the voxel attribute belonging to the semantic instance is recorded as 1, and the voxel attribute not belonging to the semantic instance is recorded as 0; Performing Hilbert curve encoding on the local voxel map to obtain a one-dimensional Boolean array; Serializing the one-dimensional Boolean array to obtain a one-dimensional data stream; The one-dimensional data stream is compressed by Hough coding to obtain the semantic instance compression package.

9. The method for collaborative positioning and mapping of cross-domain unmanned aerial and water swarms according to claim 3 is characterized in that: The communication mechanism of the request and response is specifically as follows: The requester broadcasts the index of the compressed package of the semantic instance owned by itself as a map synchronization request message; After receiving the map synchronization request message, the receiver compares the voxel index of the semantic occupancy grid map owned by itself with the voxel index, confirms the semantic instance compressed package that the requester lacks, and sends the semantic instance compressed package to the requester as a map synchronization response message; 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 occupancy grid map and the unmanned cluster mapping data.

10. The method for collaborative positioning and mapping of unmanned clusters across water and air domains according to claim 8 is characterized in that: The step of updating the semantic occupancy grid map according to the semantic instance compression package is specifically: If the unmanned device receives the map update data transmitted by the cluster control system, the voxels that need to be updated are obtained by a ray casting algorithm, and the occupancy attributes and semantic attributes of the voxels are updated by a maximum a posteriori probability algorithm. After that, the voxels are clustered according to the semantic categories using a voxel clustering algorithm to obtain a plurality of semantic instances, and the semantic instances are used to replace the voxels that need to be updated in the semantic occupancy grid map, and the updated semantic occupancy grid map is output as the unmanned cluster mapping data; If the unmanned device receives the map update data transmitted by other unmanned devices, the map update data is decompressed to obtain the local voxel map containing semantic instances, and the occupancy attributes and semantic attributes of the voxels of the corresponding coordinates in the semantic occupancy grid map are updated using the maximum a posteriori probability algorithm based on the three-dimensional spatial coordinates of the local voxel map. After that, the voxels are clustered according to semantic categories using the voxel clustering algorithm to obtain the semantic occupancy grid map updated based on the local voxel map, and the updated semantic occupancy grid map is output as the unmanned cluster mapping data.

Images