Collaborative mapping method for multiple unmanned vehicles based on semantic segmentation

Abstract

The present invention discloses a multi-unmanned vehicle collaborative mapping method based on semantic segmentation. The method is based on simultaneous positioning and mapping SLAM combined with a neural network model for semantic segmentation to achieve multi-unmanned vehicle collaborative map construction in a dynamic environment. SLAM is used to extract feature points while a neural network model is used to perform semantic segmentation to distinguish feature points into dynamic and static, thereby filtering dynamic feature points. A centralized architecture is adopted, i.e., multiple unmanned vehicle front ends and a server back end. While multiple unmanned vehicles are running, mapping data is sent to the server back end. The server back end realizes map fusion between multiple unmanned vehicles through loop detection, and then uses a progressive non-convex GNC algorithm to optimize the global pose graph of the fused map, thereby effectively reducing the estimation error caused by outliers. The present invention can more accurately estimate pose in a dynamic environment and improve the mapping quality and efficiency of unmanned vehicles by adopting a multi-vehicle collaborative approach.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned vehicle mapping, and in particular to a multi-unmanned vehicle collaborative mapping method based on semantic segmentation. Background Art

[0002] Simultaneous localization and mapping (SLAM) is a foundational technology for many applications, including augmented reality (AR), robotics, and unmanned aerial vehicles (UAVs). While significant progress has been made in recent years, several challenges remain. First, most previous studies have assumed a static environment. However, in real-world environments, there are numerous dynamic objects, and these methods are often affected by these dynamic objects, making it difficult to achieve effective localization and mapping. Second, existing methods are mostly single-vehicle SLAM methods, which aim to achieve localization and mapping independently. However, in real-world environments, collaboration between multiple unmanned vehicles is essential. Collaborative localization and mapping can build a globally consistent map from the perspectives of multiple vehicles. This results in a more accurate map that is continuously updated during operation. This collaborative approach can effectively improve the efficiency of large-scale mapping. Summary of the Invention

[0003] The purpose of this invention is to overcome the shortcomings and deficiencies of the existing technology and propose a multi-unmanned vehicle collaborative mapping method based on semantic segmentation, which can more accurately estimate the pose in a dynamic environment and improve the mapping quality and efficiency of unmanned vehicles by adopting a multi-vehicle collaborative approach.

[0004] To achieve the above-mentioned purpose, the technical solution provided by the present invention is: a multi-unmanned vehicle collaborative mapping method based on semantic segmentation, which realizes the collaborative construction of maps by multiple unmanned vehicles in a dynamic environment based on simultaneous localization and mapping SLAM combined with a neural network model for semantic segmentation. While extracting feature points using SLAM, semantic segmentation is performed through a neural network model to distinguish feature points into dynamic and static, thereby filtering dynamic feature points and achieving more accurate mapping. A centralized architecture is adopted, i.e., multiple unmanned vehicle front ends and a server back end. While multiple unmanned vehicles are running, the mapping data is sent to the server back end. The server back end realizes map fusion between multiple unmanned vehicles through loop detection, and then adopts a progressive non-convex GNC algorithm to optimize the global pose graph of the fused map, thereby effectively reducing the estimation error caused by outliers.

[0005] The specific implementation of the multi-unmanned vehicle collaborative mapping method based on semantic segmentation includes the following steps:

[0006] S1: Deploy the neural network model as a TensorRT inference framework model to achieve accelerated inference of the neural network model. Use this inference framework model to build a semantic segmentation service node under the ROS operating system to prepare for calling the semantic segmentation service in subsequent steps.

[0007] S2: Extract feature points from the current frame observation image and call the semantic segmentation service. Based on the semantic segmentation results, the feature points are divided into dynamic and static, and the dynamic feature points are filtered. When the number of static feature points exceeds the preset threshold, the three-dimensional coordinates of the static feature points in the camera coordinate system, i.e., the map points, are calculated based on their depth values. The map is initialized, and the current frame is used as the reference frame. The map points near the reference frame are selected as the local map.

[0008] S3: Read the current frame observation image taken by the camera in real time to extract feature points and call the semantic segmentation service, shield the dynamic feature points, match the static feature points of the current frame with the reference frame, and the number of matching static feature points must be more than the preset number to estimate the current pose, otherwise the tracking fails and relocation is required. Relocation is to find the current position in the entire map data. If it cannot be found, then the relocation fails and it is necessary to return to step S2 to initialize a new map; if the tracking is successful, then track in the local map based on the estimated pose, that is, match with the map points in the local map to obtain more static feature point pairings, and then optimize the current pose estimation, and combine the time and feature point quality to determine whether the frame is a key frame. Non-key frames need to be discarded to reduce the amount of map data, and key frames proceed to subsequent steps;

[0009] S4: For key frames, new map points need to be generated based on the static feature points matched in step S3, and local nonlinear optimization, i.e., local BA optimization, is performed to optimize the posture of the unmanned vehicle and the three-dimensional coordinates of the generated map points;

[0010] S5: After local BA optimization, the data is forwarded to the cloud server. The cloud server realizes map fusion through loop detection and uses the progressive non-convex GNC algorithm to optimize the global pose graph to reduce outliers and then trim redundant keyframes.

[0011] Furthermore, considering the spatiotemporal consistency of frames is more reasonable than using the semantic results of only one frame. Therefore, the motion probability is used to utilize the semantic information of consecutive key frames. First, each map point is defined to have three states Status(m): static, dynamic, and unknown. Static(s) represents static s, dynamic(d) represents dynamic d, and unknown(u) represents unknown u. That is, Status(m) = {static(s), dynamic(d), unknown(u)}. Define m tIndicates the state of the map point at time t, p(m t ) is the movement probability of the map point at time t. If the movement probability of the map point is close to 1, its state is more likely to be dynamic. If the movement probability of the map point is closer to 0, the map point is more likely to be static. The initial probability of the map point movement probability is set to 0.5. The probability of the point being dynamic or static is 0.5, that is, p(m0=d)=p(m0=s)=0.5. Define z t is the observed value at time t, z 1:t is the total observed value from the initial time to time t, z 1:t-1 is the total observation value from the initial time to time t-1. The observation value refers to the observation of whether the map point is dynamic or static based on the semantic segmentation result. Considering the fact that semantic segmentation is not completely accurate, the observed movement probability is defined as:

[0012]

[0013] Where τ and λ are hyperparameters, which are related to the accuracy of semantic segmentation and are calculated based on the observed value z 1:t and the initial state m0, predict the current time movement probability bel(m t ), the movement probability update problem is formulated as a Bayesian filter problem:

[0014]

[0015] Where η is a constant, the prior probability The calculation formula is:

[0016]

[0017] Where m t-1 Represents the state of the map point at time t-1, using a predefined probability threshold θ d and θ s To determine the status of a point Status(m t ) is dynamic or static:

[0018]

[0019] Furthermore, in step S1, the DDRNet neural network is used to perform semantic segmentation on the image, and actions are used to communicate with the semantic segmentation service nodes under the ROS operating system.

[0020] Furthermore, in step S5, the loop closure detection is used to determine whether the unmanned vehicle has returned to the position it previously passed. For multiple unmanned vehicles, it is used to determine whether the multiple unmanned vehicles have passed the same position, thereby realizing the map fusion of multiple unmanned vehicles. The loop closure detection is implemented using the bag-of-words model, which is implemented by the K-Means clustering algorithm and is used to determine the similarity of two images, and then determine whether they are in the same position.

[0021] Furthermore, in step S5, a centralized collaborative map is constructed and the TCP protocol is used for communication. The global pose graph of the fused map is optimized using the progressive non-convex GNC algorithm. The robust cost function of the pose graph optimization problem is highly non-convex, so the local search for the optimal value method is very sensitive to the initial value. The key idea of ​​GNC is to optimize a series of proxy cost functions with small non-convexity, and these functions gradually converge to the original robust cost function during the iterative process. Each proxy problem is is of the form , where χ is the domain of the variable x, r i : is the residual function associated with the ith measurement, represents the real number domain, and the proxy function ρ is parameterized by the control parameter μ μ The following conditions hold: For some given constants μ0 and μ1:

[0022] 1) For μ→μ0, the function ρ μ is convex;

[0023] 2) For μ→μ1, ρ converges to the original robust cost function;

[0024] In practice, μ is initialized to μ0 and its value is gradually updated to approach μ1 as the optimization proceeds. For each instance of , GNC reformulates the problem using the Black-Rangarajan duality, which states that, under set conditions, it is equivalent to the following optimization problem:

[0025]

[0026] Where w i ∈[0,1] is the scalar weight associated with the i-th measurement. In the above formula, the outlier processing function For each w i A penalty term is introduced, whose expression depends on the chosen robust cost function ρ and control parameter μ:

[0027]

[0028] In the above formula (6), is a given TLS cost threshold, is the current residual; GNC for variables x and weight w i Perform alternating minimization to optimize Equation (5), but also update the control parameter μ;

[0029] 1) Variable update: For variables with fixed weight w i The x of minimizes Equation (5), which is equivalent to solving a standard weighted least squares problem;

[0030] 2) Weight update: Use fixed variable x to update w i Minimize formula (5), each w i The corresponding update of has a closed-form expression and depends on the current robust proxy function ρ μ ;

[0031] 3) Parameter update: Update μ by a constant factor to approach μ1;

[0032] After loop detection and map fusion, robust pose graph optimization is performed to obtain the best pose estimate. The pose graph optimization goal is to optimize all pose variables using all pose estimates and assumed loops.

[0033] set up represents the set of unmanned vehicles, represents the k-th pose of the unmanned vehicle α in the global coordinate system, represents the k+1th position of the unmanned vehicle α in the global coordinate system, represents the corresponding rotation amount, represents the corresponding displacement, SE(3) represents the Euclidean transformation set, n α is the total number of poses of the unmanned vehicle α, and the residual of the two adjacent poses of each unmanned vehicle α

[0034]

[0035] In the above formula, are the noise estimation terms of rotation and displacement in the odometer, w R ,w t >0 is a hyperparameter, which represents the measurement accuracy of rotation and displacement respectively. From the above, we can see that the residual error of the posture of all unmanned vehicles is

[0036] set up represents the j-th position of the unmanned vehicle β in the global coordinate system, Indicates the amount of rotation, represents the displacement, L is the loop set including loops within the unmanned vehicle and between unmanned vehicles, and there are (α k ,β j )∈L, that is, the unmanned vehicle α is in the kth position α kand βjth pose β j A loop is formed, and the residual of the loop The calculation is as follows:

[0037]

[0038] In the above formula, are the noise estimates of the rotation and displacement in the loop, respectively. It can be seen that the goal of pose graph optimization is to minimize the residual sum of all adjacent poses and the assumed loop residual sum:

[0039]

[0040] Where: is the truncated least squares TLS robust cost function, and GNC is used to solve the above pose graph optimization problem:

[0041] 1) Variable update: that is, solving the standard weighted pose graph optimization problem and optimizing the pose variables:

[0042]

[0043] Compared with Equation (9), the robust cost function ρ in the loop term is replaced by the weighted squared error, is the weight of the loop;

[0044] 2) Weight update: the weight of each residual function of TLS The update depends only on the current residual Control parameter μ and threshold of TLS cost

[0045]

[0046] After the weight update phase, its control parameter μ is also updated so that a series of proxy cost functions gradually converge to the original TLS function.

[0047] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0048] 1. This invention deploys the semantic segmentation network using TensorRT, which effectively improves the inference speed. It applies semantic segmentation to feature matching to shield dynamic objects, thereby reducing the pose estimation error of the unmanned vehicle and improving the mapping accuracy of the unmanned vehicle.

[0049] 2. The present invention adopts a centralized approach to collect mapping data from each unmanned vehicle, and then fuses the maps. It reduces mapping errors through global optimization, and trims redundant key frames to complete the simplification of map data, thereby achieving efficient and high-quality mapping. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 It is a framework diagram of the method of the present invention. DETAILED DESCRIPTION

[0051] The present invention will be described in further detail below with reference to the embodiments and drawings, but the embodiments of the present invention are not limited thereto.

[0052] This embodiment discloses a multi-unmanned vehicle collaborative mapping method based on semantic segmentation. The method realizes multi-unmanned vehicle collaborative map construction in a dynamic environment based on simultaneous localization and mapping SLAM combined with a neural network model for semantic segmentation. While extracting feature points using SLAM, semantic segmentation is performed through a neural network model to distinguish feature points into dynamic and static, thereby filtering dynamic feature points and achieving more accurate mapping. A centralized architecture is adopted, namely multiple unmanned vehicle front ends and a server back end. While multiple unmanned vehicles are running, the mapping data is sent to the server back end. The server back end realizes map fusion between multiple unmanned vehicles through loop detection, and then adopts a progressive non-convex GNC algorithm to optimize the global pose graph of the fused map, thereby effectively reducing the estimation error caused by outliers.

[0053] like Figure 1 As shown in FIG, the specific implementation of the multi-unmanned vehicle collaborative mapping method based on semantic segmentation includes the following steps:

[0054] S1: Deploy the neural network model as a TensorRT inference framework model to achieve accelerated inference of the neural network model. Use this inference framework model to build a semantic segmentation service node under the ROS operating system to prepare for calling the semantic segmentation service in subsequent steps.

[0055] The DDRNet neural network is used for semantic segmentation of images, and actions are used for communication with semantic segmentation service nodes under the ROS operating system.

[0056] S2: Extract feature points from the current frame observation image and call the semantic segmentation service. Based on the semantic segmentation results, the feature points are divided into dynamic and static, and the dynamic feature points are filtered. When the number of static feature points exceeds the preset threshold, the three-dimensional coordinate points in the camera coordinate system, i.e., map points, are calculated based on the static feature points and their depth values. The map is initialized, and the current frame is used as the reference frame. The map points near the reference frame are selected as the local map.

[0057] S3: Read the current frame observation image taken by the camera in real time to extract feature points and call the semantic segmentation service, shield the dynamic feature points, match the static feature points of the current frame with the reference frame, and the number of matching static feature points must be more than the preset number to estimate the current pose, otherwise the tracking fails and relocation is required. Relocation is to find the current position in the entire map data. If it cannot be found, then the relocation fails and it is necessary to return to step S2 to initialize a new map; if the tracking is successful, then track in the local map based on the estimated pose, that is, match with the map points in the local map to obtain more static feature point pairings, and then optimize the current pose estimation, and combine the time and feature point quality to determine whether the frame is a key frame. Non-key frames need to be discarded to reduce the amount of map data, and key frames proceed to subsequent steps.

[0058] S4: For key frames, it is necessary to generate new map points based on the static feature points matched in step S3, and perform local nonlinear optimization, namely local BA optimization, to optimize the posture of the unmanned vehicle and the three-dimensional coordinates of the generated map points.

[0059] S5: After local BA optimization, the data is forwarded to the cloud server. The cloud server realizes map fusion through loop detection and uses the progressive non-convex GNC algorithm to optimize the global pose graph to reduce outliers and then trim redundant keyframes.

[0060] Specifically, considering the spatiotemporal consistency of frames is more reasonable than using the semantic results of only one frame. Therefore, the motion probability is used to utilize the semantic information of continuous key frames. First, each map point is defined to have three states Status(m): static, dynamic, and unknown. Static(s) represents static s, dynamic(d) represents dynamic d, and unknown(u) represents unknown u, that is, Status(m) = {static(s), dynamic(d), unknown(u)}. Define m t Indicates the state of the map point at time t, p(m t ) is the movement probability of the map point at time t. If the movement probability of the map point is close to 1, its state is more likely to be dynamic. If the movement probability of the map point is closer to 0, the map point is more likely to be static. The initial probability of the map point movement probability is set to 0.5. The probability of the point being dynamic or static is 0.5, that is, p(m0=d)=p(m0=s)=0.5. Define z t is the observed value at time t, z 1:t is the total observed value from the initial time to time t, z 1:t-1is the total observation value from the initial time to time t-1. The observation value refers to the observation of whether the map point is dynamic or static based on the semantic segmentation result. Considering the fact that semantic segmentation is not completely accurate, the observed movement probability is defined as:

[0061]

[0062] Where τ and λ are hyperparameters, which are related to the accuracy of semantic segmentation and are calculated based on the observed value z 1:t and the initial state m0, predict the current time movement probability bel(m t ), the movement probability update problem is formulated as a Bayesian filter problem:

[0063]

[0064] Where η is a constant, the prior probability The calculation formula is:

[0065]

[0066] Where m t-1 Represents the state of the map point at time t-1, using a predefined probability threshold θ d and θ s To determine the status of a point Status(m t ) is dynamic or static:

[0067]

[0068] Specifically, in step S5, the loop closure detection is used to determine whether the unmanned vehicle has returned to the position it passed previously. For multiple unmanned vehicles, it is used to determine whether multiple unmanned vehicles have passed the same position, thereby realizing the map fusion of multiple unmanned vehicles. The loop closure detection is implemented using the bag-of-words model, which is implemented by the K-Means clustering algorithm and is used to determine the similarity of two images, and then determine whether they are in the same position.

[0069] Specifically, in step S5, centralized collaborative mapping is adopted, communication is carried out through the TCP protocol, and the progressive non-convex GNC algorithm is used to optimize the global pose graph of the fused map. The robust cost function of the pose graph optimization problem is highly non-convex, so the local search optimal value method is very sensitive to the initial value. The key idea of ​​GNC is to optimize a series of small non-convex proxy cost functions, and these functions gradually converge to the original robust cost function during the iterative process. Each proxy problem is is of the form , where χ is the domain of the variable x, r i : is the residual function associated with the ith measurement, represents the real number domain, and the proxy function ρ is parameterized by the control parameter μ μ The following conditions hold: For some given constants μ0 and μ1:

[0070] 1) For μ→μ0, the function ρ μ is convex;

[0071] 2) For μ→μ1, ρ μ Converges to the original robust cost function;

[0072] In practice, μ is initialized to μ0 and its value is gradually updated to approach μ1 as the optimization proceeds. For each instance of , GNC reformulates the problem using the Black-Rangarajan duality, which states that, under set conditions, it is equivalent to the following optimization problem:

[0073]

[0074] Where w i ∈[0,1] is the scalar weight associated with the i-th measurement. In the above formula, the outlier processing function For each w i A penalty term is introduced, whose expression depends on the chosen robust cost function ρ and control parameter μ:

[0075]

[0076] In the above formula (6), is a given TLS cost threshold, is the current residual; GNC for variables x and weight w i Perform alternating minimization to optimize Equation (5), but also update the control parameter μ;

[0077] 1) Variable update: For variables with fixed weight w i The x of minimizes Equation (5), which is equivalent to solving a standard weighted least squares problem;

[0078] 2) Weight update: Use fixed variable x to update w i Minimize formula (5), each w i The corresponding update of has a closed-form expression and depends on the current robust proxy function ρ μ ;

[0079] 3) Parameter update: Update μ by a constant factor to approach μ1;

[0080] After loop detection and map fusion, robust pose graph optimization is performed to obtain the best pose estimate. The pose graph optimization goal is to optimize all pose variables using all pose estimates and assumed loops.

[0081] set up represents the set of unmanned vehicles, represents the k-th pose of the unmanned vehicle α in the global coordinate system, represents the k+1th position of the unmanned vehicle α in the global coordinate system, represents the corresponding rotation amount, represents the corresponding displacement, SE(3) represents the Euclidean transformation set, n α is the total number of poses of the unmanned vehicle α, and the residual of the two adjacent poses of each unmanned vehicle α

[0082]

[0083] In the above formula, are the noise estimation terms of rotation and displacement in the odometer, w R ,w t >0 is a hyperparameter, which represents the measurement accuracy of rotation and displacement respectively. From the above, we can see that the residual error of the posture of all unmanned vehicles is

[0084] set up represents the j-th position of the unmanned vehicle β in the global coordinate system, Indicates the amount of rotation, represents the displacement, L is the loop set including loops within the unmanned vehicle and between unmanned vehicles, and there are (α k ,β j )∈L, that is, the unmanned vehicle α is in the kth position α k and βjth pose β j A loop is formed, and the residual of the loop The calculation is as follows:

[0085]

[0086] In the above formula, are the noise estimates of the rotation and displacement in the loop, respectively. It can be seen that the goal of pose graph optimization is to minimize the residual sum of all adjacent poses and the assumed loop residual sum:

[0087]

[0088] Where: is the truncated least squares TLS robust cost function, and GNC is used to solve the above pose graph optimization problem:

[0089] 1) Variable update: that is, solving the standard weighted pose graph optimization problem and optimizing the pose variables:

[0090]

[0091] Compared with Equation (9), the robust cost function ρ in the loop term is replaced by the weighted squared error, is the weight of the loop;

[0092] 2) Weight update: the weight of each residual function of TLS The update depends only on the current residual Control parameter μ and threshold of TLS cost

[0093]

[0094] After the weight update phase, its control parameter μ is also updated so that a series of proxy cost functions gradually converge to the original TLS function.

[0095] The above embodiments are preferred implementation modes of the present invention, but the implementation modes of the present invention are not limited to the above embodiments. Any other changes, modifications, substitutions, combinations, and simplifications that do not deviate from the spirit and principles of the present invention should be considered as equivalent replacement methods and are included in the scope of protection of the present invention.

Claims

1. A multi-unmanned vehicle collaborative mapping method based on semantic segmentation, characterized by: This method is based on simultaneous localization and mapping (SLAM) combined with a neural network model for semantic segmentation to achieve collaborative map construction by multiple unmanned vehicles in a dynamic environment. SLAM is used to extract feature points while a neural network model is used for semantic segmentation to distinguish feature points into dynamic and static. This allows for filtering of dynamic feature points and more accurate mapping. A centralized architecture is used, i.e., multiple unmanned vehicle front ends and a server back end. While multiple unmanned vehicles are running, they send mapping data to the server back end. The server back end uses loop closure detection to achieve map fusion between multiple unmanned vehicles, and then uses a progressively non-convex GNC algorithm to optimize the global pose graph of the fused map, effectively reducing the estimation error caused by outliers. The specific implementation of the multi-unmanned vehicle collaborative mapping method based on semantic segmentation includes the following steps: S1: Deploy the neural network model as a TensorRT inference framework model to achieve accelerated inference of the neural network model. Use this inference framework model to build a semantic segmentation service node under the ROS operating system to prepare for calling the semantic segmentation service in subsequent steps. S2: Extract feature points from the current frame observation image and call the semantic segmentation service. Based on the semantic segmentation results, the feature points are divided into dynamic and static, and the dynamic feature points are filtered. When the number of static feature points exceeds the preset threshold, the three-dimensional coordinates of the static feature points in the camera coordinate system, i.e., the map points, are calculated based on their depth values. The map is initialized, and the current frame is used as the reference frame. The map points near the reference frame are selected as the local map. S3: Read the current frame observation image taken by the camera in real time to extract feature points and call the semantic segmentation service, shield the dynamic feature points, match the static feature points of the current frame with the reference frame, and the number of matching static feature points must be more than the preset number to estimate the current pose, otherwise the tracking fails and relocation is required. Relocation is to find the current position in the entire map data. If it cannot be found, then the relocation fails and it is necessary to return to step S2 to initialize a new map; if the tracking is successful, then track in the local map based on the estimated pose, that is, match with the map points in the local map to obtain more static feature point pairings, and then optimize the current pose estimation, and combine the time and feature point quality to determine whether the frame is a key frame. Non-key frames need to be discarded to reduce the amount of map data, and key frames proceed to subsequent steps; S4: For key frames, new map points need to be generated based on the static feature points matched in step S3, and local nonlinear optimization, i.e., local BA optimization, is performed to optimize the posture of the unmanned vehicle and the three-dimensional coordinates of the generated map points; S5: After local BA optimization, the data is forwarded to the cloud server. The cloud server realizes map fusion through loop detection and uses the progressive non-convex GNC algorithm to optimize the global pose graph to reduce outliers and then trim redundant keyframes.

2. The multi-unmanned vehicle collaborative mapping method based on semantic segmentation according to claim 1 is characterized in that: Considering the spatiotemporal consistency of frames is more reasonable than using the semantic results of only one frame. Therefore, the motion probability is used to utilize the semantic information of consecutive key frames. First, each map point is defined to have three states: static, dynamic, and unknown. Static(s) represents static s, dynamic(d) represents dynamic d, and unknown(u) represents unknown u. That is, Status(m) = {static(s), dynamic(d), unknown(u)}. Define m t Indicates the state of the map point at time t, p(m t ) is the movement probability of the map point at time t. If the movement probability of the map point is close to 1, its state is more likely to be dynamic. If the movement probability of the map point is closer to 0, the map point is more likely to be static. The initial probability of the map point movement probability is set to 0.

5. The probability of the point being dynamic or static is 0.5, that is, p(m0=d)=p(m0=s)=0.

5. Define z t is the observed value at time t, z 1:t is the total observed value from the initial time to time t, z 1:t-1 is the total observation value from the initial time to time t-1. The observation value refers to the observation of whether the map point is dynamic or static based on the semantic segmentation result. Considering the fact that semantic segmentation is not completely accurate, the observed movement probability is defined as: Where τ and λ are hyperparameters, which are related to the accuracy of semantic segmentation and are calculated based on the observed value z 1:t and the initial state m0, predict the current time movement probability bel(m t ), the movement probability update problem is formulated as a Bayesian filter problem: Where η is a constant, the prior probability The calculation formula is: Where m t-1 Represents the state of the map point at time t-1, using a predefined probability threshold θ d and θ s To determine the status of a point Status(m t ) is dynamic or static:

3. The multi-unmanned vehicle collaborative mapping method based on semantic segmentation according to claim 1 is characterized in that: In step S1, the DDRNet neural network is used to perform semantic segmentation on the image, and the semantic segmentation service node under the ROS operating system uses action for communication.

4. The multi-unmanned vehicle collaborative mapping method based on semantic segmentation according to claim 1 is characterized in that: In step S5, the loop closure detection is used to determine whether the unmanned vehicle has returned to the position it previously passed. For multiple unmanned vehicles, it is used to determine whether the multiple unmanned vehicles have passed the same position, thereby realizing the map fusion of multiple unmanned vehicles. The loop closure detection is implemented using the bag-of-words model, which is implemented by the K-Means clustering algorithm. It is used to determine the similarity of two images and then determine whether they are at the same location.

5. The multi-unmanned vehicle collaborative mapping method based on semantic segmentation according to claim 1 is characterized in that: In step S5, centralized collaborative mapping is adopted, communication is carried out through the TCP protocol, and the progressive non-convex GNC algorithm is used to optimize the global pose graph of the fused map. The robust cost function of the pose graph optimization problem is highly non-convex, so the local search optimal value method is very sensitive to the initial value. The key idea of ​​GNC is to optimize a series of small non-convex proxy cost functions, and these functions gradually converge to the original robust cost function during the iterative process. Each proxy problem is is of the form , where χ is the domain of the variable x, is the residual function associated with the ith measurement, represents the real number domain, and the proxy function ρ is parameterized by the control parameter μ μ The following conditions hold: For some given constants μ0 and μ1: 1) For μ→μ0, the function ρ μ is convex; 2) For μ→μ1, ρ μ Converges to the original robust cost function; In practice, μ is initialized to μ0 and its value is gradually updated to approach μ1 as the optimization proceeds. For each instance of , GNC reformulates the problem using the Black-Rangarajan duality, which states that, under set conditions, it is equivalent to the following optimization problem: Where w i ∈[0,1] is the scalar weight associated with the i-th measurement. In the above formula, the outlier processing function For each w i A penalty term is introduced, whose expression depends on the chosen robust cost function ρ and control parameter μ: In the above formula (6), is a given TLS cost threshold, is the current residual; GNC for variables x and weight w i Perform alternating minimization to optimize Equation (5), but also update the control parameter μ; 1) Variable update: For variables with fixed weight w i The x of minimizes Equation (5), which is equivalent to solving a standard weighted least squares problem; 2) Weight update: Use fixed variable x to update w i Minimize formula (5), each w i The corresponding update of has a closed-form expression and depends on the current robust proxy function ρ μ ; 3) Parameter update: Update μ by a constant factor to approach μ1; After loop detection and map fusion, robust pose graph optimization is performed to obtain the best pose estimate. The pose graph optimization goal is to optimize all pose variables using all pose estimates and assumed loops. set up represents the set of unmanned vehicles, represents the k-th pose of the unmanned vehicle α in the global coordinate system, represents the k+1th position of the unmanned vehicle α in the global coordinate system, represents the corresponding rotation amount, represents the corresponding displacement, SE(3) represents the Euclidean transformation set, n α is the total number of poses of the unmanned vehicle α, and the residual of the two adjacent poses of each unmanned vehicle α In the above formula, are the noise estimation terms of rotation and displacement in the odometer, w R ,w t >0 is a hyperparameter, which represents the measurement accuracy of rotation and displacement respectively. From the above, we can see that the residual error of the posture of all unmanned vehicles is set up represents the j-th position of the unmanned vehicle β in the global coordinate system, Indicates the amount of rotation, represents the displacement, L is the loop set including loops within the unmanned vehicle and between unmanned vehicles, and there are (α k ,β j )∈L, that is, the unmanned vehicle α is in the kth position α k and βjth pose β j A loop is formed, and the residual of the loop The calculation is as follows: In the above formula, are the noise estimates of the rotation and displacement in the loop, respectively. It can be seen that the goal of pose graph optimization is to minimize the residual sum of all adjacent poses and the assumed loop residual sum: Where, is the truncated least squares TLS robust cost function, and GNC is used to solve the above pose graph optimization problem: 1) Variable update: that is, solving the standard weighted pose graph optimization problem and optimizing the pose variables: Compared with Equation (9), the robust cost function ρ in the loop term is replaced by the weighted squared error, is the weight of the loop; 2) Weight update: the weight of each residual function of TLS The update depends only on the current residual Control parameter μ and threshold of TLS cost After the weight update phase, its control parameter μ is also updated so that a series of proxy cost functions gradually converge to the original TLS function.

Images