Navigation map construction method and device

Abstract

The invention relates to a navigation map construction method and device, and belongs to the technical field of navigation map construction.The navigation map construction method comprises the steps that variance screening is conducted on at least two data sets, and the data sets meeting the variance screening condition are fused to obtain fused data; the at least two data sets are acquired by different sensors in real time, and the fusion data comprises a pose coordinate set; according to the method, the probability grid map is constructed based on the pose coordinate set, the probability grid map is updated according to the pose influence factors to obtain the real-time navigation map, the pose influence factors are used for representing the influence degree of different sensors corresponding to the fusion data on the pose coordinates, and the accuracy and reliability of the map are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of navigation map construction, and in particular, to a navigation map construction method and device. Background Art

[0002] With the continuous development of technology, intelligent robot devices are increasingly widely used in the fields of smart home, industrial automation, navigation in large public places, etc. Therefore, the demand of intelligent robot devices for precise positioning and efficient map construction technology is also increasing day by day.

[0003] Currently, the Cartographer algorithm is widely used in the autonomous navigation of intelligent robot devices in complex environments. The Cartographer algorithm realizes multi-sensor fusion by efficiently integrating data from multiple sensors such as lidar and inertial measurement units, greatly enhancing the richness of environmental information, and significantly improving the accuracy of positioning and mapping through cross-validation and error correction.

[0004] However, the Cartographer algorithm highly depends on sensor data. When the sensors are physically restricted in a complex environment and cause abnormal data, it leads to inaccuracy in performing the mapping task, and further leads to uncertainty in automatic navigation, making it difficult to accurately construct a navigation map. Summary of the Invention

[0005] In view of this, it is necessary to provide a navigation map construction method and device to solve the technical problem of low accuracy in map construction when data is abnormal in a complex environment.

[0006] To solve the above problems, in a first aspect, the present invention provides a navigation map construction method, including: Performing variance screening on at least two data sets, and fusing the data sets that meet the variance screening conditions to obtain fused data; the at least two data sets are obtained by real-time collection of different sensors, and the fused data includes a pose coordinate set; Constructing a probability grid map based on the pose coordinate set, and updating the probability grid map according to a pose influence factor to obtain a real-time navigation map, where the pose influence factor is used to characterize the influence degree of different sensors corresponding to the fused data on the pose coordinates.

[0007] In a possible implementation manner, the data sets are two groups, namely a first data set and a second data set; performing variance screening on at least two data sets, and fusing the data sets that meet the variance screening conditions to obtain fused data, includes: Obtaining the first data set and the second data set in real time; Calculating the variances of the first data set and the second data set respectively to obtain a first variance and a second variance; Fuse the data set corresponding to the smaller variance value among the first variance and the second variance to obtain fused data.

[0008] In a possible implementation manner, the step of fusing the data set corresponding to the smaller variance value among the first variance and the second variance to obtain fused data includes: Step 1: Sort the data set corresponding to the smaller variance value to obtain the sensor data with the smallest variance; Step 2: Perform Kalman filtering on the sensor data with the smallest variance by using the Kalman filtering algorithm to obtain the first optimized data; Step 3: Use the first optimized data as the first reference value, and based on the first reference value, perform Kalman filtering on the remaining sensor data with the smallest variance by using the Kalman filtering algorithm to obtain the second optimized data; Step 4: Use the second optimized data as the first correction value, and use the Kalman filtering algorithm to fuse the first correction value and the first reference value to obtain fused data; Step 5: Use the fused data as the second reference value, and repeat Steps 3 to 4 until the sensor data fusion is completed to obtain sensor fusion data.

[0009] In a possible implementation manner, the specific expression of the pose influence factor is: , where is the pose influence factor, represents the number of sensors in the fusion stage, is the th variance value received by the sensor in real time.

[0010] In a possible implementation manner, the method of constructing a probability grid map based on the pose coordinate set and updating the probability grid map according to the pose influence factor to obtain a real-time navigation map includes: Determine the radar scan frame data based on the fused data, determine the pose coordinate set based on the radar scan frame data, and construct a probability grid map based on the pose coordinate set; Mark the pose coordinates in the probability grid map based on the pose influence factor, and update the probability grid map to obtain a real-time navigation map.

[0011] In a possible implementation manner, the state of the grid position in the probability grid map includes an unknown state, a non-occupied state, and an occupied state; the step of marking the pose coordinates in the probability grid map based on the pose influence factor includes: Set the threshold for constructing the probability grid map. When the pose influence factor is less than or equal to the threshold for constructing the probability grid map, mark the grid position corresponding to the pose coordinates as unknown; When the pose influence factor is greater than the threshold for constructing the probability grid map, mark the grid position corresponding to the pose coordinates as non-occupied.

[0012] In a possible implementation, the real-time navigation map is a local sub-map, and the method further includes: Use the branch and bound method to perform loop closure detection on the fused data, and update the pose coordinates of the local sub-map based on the pose influence factor; Perform global optimization on the updated local sub-map based on the pose influence factor to generate a navigation map.

[0013] In a possible implementation, the step of using the branch and bound method to perform loop closure detection on the fused data and updating the pose coordinates of the local sub-map based on the pose influence factor includes: When using the branch and bound method to perform loop closure detection on the fused data, obtain the radar scan frame data at the current moment based on the fused data, compare the pose influence factor corresponding to the radar scan frame data with the threshold for constructing the probability grid map, and determine whether the pose coordinates are added to the local sub-map. When the pose coordinates corresponding to the radar scan frame data are located in the local sub-map, obtain the first influence factor of the pose coordinates in the local sub-map; Obtain the radar scan frame data at the next moment. When the pose coordinates corresponding to the radar scan frame data at the next moment coincide with the pose coordinates corresponding to the radar scan frame data at the current moment, obtain the second influence factor of the pose coordinates corresponding to the radar scan frame data at the next moment. When the first influence factor is less than the second influence factor, update the pose coordinates corresponding to the first influence factor to the pose coordinates of the radar scan frame data at the next moment.

[0014] In a possible implementation, the step of performing global optimization on the updated local sub-map based on the pose influence factor to generate a navigation map includes: Based on the pose influence factor, perform global optimization on each updated local sub-map through a sparse pose graph to generate a navigation map.

[0015] In a second aspect, the present invention further provides a navigation map construction device, including: A data fusion module, configured to perform variance screening on at least two data sets, and fuse the data sets that meet the variance screening conditions to obtain fused data; the at least two data sets are collected in real time by different sensors, and the fused data includes a pose coordinate set; A navigation map construction module is used to construct a probability grid map based on a set of pose coordinates and update the probability grid map according to a pose influence factor to obtain a real-time navigation map. The pose influence factor is used to characterize the influence degree of different sensors corresponding to the fusion data on the pose coordinates.

[0016] The beneficial effects of the present invention are as follows: at least two data sets are subjected to variance screening, and the data sets that meet the variance screening conditions are fused to obtain fusion data. When data is abnormal in a complex environment, unstable factors in the sensor data are filtered, ensuring the retention of high-quality data, improving the accuracy and stability of the data. A probability grid map is constructed based on a set of pose coordinates, and the probability grid map is updated according to a pose influence factor to obtain a real-time navigation map. The pose influence factor is used to characterize the influence degree of different sensors corresponding to the fusion data on the pose coordinates. By updating the probability grid map with the pose influence factor, the state of the grid map is dynamically adjusted, optimizing the local mapping process, improving the accuracy of loop detection, and ensuring that in the global optimization process, more reliable pose coordinates can be selected according to the confidence of the pose coordinates, thereby improving the accuracy and consistency of the global map; the pose influence factor is introduced to perform real-time evaluation and dynamic adjustment on the data input, greatly reducing unnecessary data processing processes, improving the map construction efficiency, saving computing resources, enhancing the map construction performance in a complex environment by evaluating and dynamically adjusting the confidence of map construction in real time through the influence factor, curbing the trend of error accumulation, improving the convergence speed and map construction consistency in a complex scenario, effectively avoiding incorrect map construction caused by low-confidence data, and improving the accuracy and reliability of the map. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following will briefly introduce the drawings required for the description of the embodiments. Obviously, the following drawings are only some embodiments of the present invention. For those skilled in the art, without creative efforts, other drawings can be obtained based on these drawings.

[0018] Figure 1 It is a flowchart of an embodiment of the navigation map construction method provided by the present invention; Figure 2 It is a flowchart of an embodiment of step S101 of the navigation map construction method provided by the present invention; Figure 3 It is a schematic structural diagram of an embodiment of the navigation map construction device provided by the present invention; DETAILED DESCRIPTION OF THE EMBODIMENTS Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative efforts belong to the scope of protection of the present invention.

[0019] In the description of the embodiments of the present invention, unless otherwise specified, "a plurality of" means two or more. "And / or" describes the association relationship of associated objects, indicating that there can be three relationships, for example: A and / or B can represent: A exists alone, A and B exist simultaneously, and B exists alone.

[0020] The descriptions such as "first" and "second" involved in the embodiments of the present invention are only for descriptive purposes, and cannot be understood as indicating or implying their relative importance or implicitly indicating the quantity of the indicated technical features. Therefore, the technical features defined with "first" and "second" may explicitly or implicitly include at least one such feature.

[0021] Referring to "embodiment" herein means that a specific feature, structure, or characteristic described in connection with the embodiment can be included in at least one embodiment of the present invention. The phrase appears in various places in the specification and does not necessarily refer to the same embodiment, nor is it an independent or alternative embodiment mutually exclusive with other embodiments. Those skilled in the art explicitly and implicitly understand that the embodiments described herein can be combined with other embodiments.

[0022] Before presenting the embodiments, the following terms are explained first.

[0023] MSF-Cartographer: The Cartographer algorithm for multi-sensor fusion. MSF (Multi-Sensor Fusion) and Cartographer is a real-time SLAM framework developed by Google, focusing on solving the problems of simultaneous localization and mapping of robots in unknown environments. The core goal is the same as that of SLAM technology, that is, to achieve high-precision localization and map generation in a dynamic environment by fusing multi-sensor data (such as lidar, IMU, odometer, etc.).

[0024] A specific embodiment of the present invention discloses a method for constructing a navigation map, as Figure 1 shown. The method for constructing a navigation map includes: S101. Perform variance screening on at least two data sets, and fuse the data sets that meet the variance screening conditions to obtain fused data; the at least two data sets are obtained by real-time acquisition of different sensors, and the fused data includes a pose coordinate set.

[0025] It should be noted that in variance screening, by introducing a mechanism for real-time calculation of the variance of the sensor data stream, the degree of data fluctuation can be quantified and dynamically monitored, providing a scientific basis for the stability evaluation of the data. Through comparison with the screening algorithm, high-quality data can be accurately screened out, and potential unstable factors can be eliminated, thus significantly improving the accuracy and reliability of multi-sensor data processing, effectively solving the problems of noise interference and inaccurate data evaluation in traditional technologies, and laying a solid foundation for subsequent multi-sensor data fusion. After dynamically selecting the optimal data source through variance analysis and preliminary optimization using Kalman filtering, the sub-optimal data sources are gradually fused and deeply processed at each stage to achieve efficient fusion, effectively removing noise, improving data accuracy, and adapting to changes in sensor data in real time, ensuring the accuracy and stability of the final fusion result, and solving the problems of insufficient fusion accuracy, lack of stage optimization, difficulty in noise removal, and insufficient ability to handle dynamic changes.

[0026] S102. Construct a probability grid map based on the pose coordinate set and update the probability grid map according to the pose influence factor to obtain a real-time navigation map. The pose influence factor is used to characterize the influence degree of different sensors corresponding to the fused data on the pose coordinates.

[0027] It should be noted that the MSF-Cartographer algorithm is used to construct the real-time navigation map. The MSF-Cartographer algorithm includes a local mapping stage, a loop detection stage, and a global mapping stage. The pose influence factor is constructed through the variance of the sensor data, and the map construction in the local mapping stage, the loop detection stage, and the global mapping stage is optimized through the pose influence factor to improve the accuracy and consistency of the navigation map construction.

[0028] In some embodiments of the present invention, in step S101, at least two data sets are subjected to variance screening. The at least two data sets are obtained by real-time acquisition of different sensors. The data sets are two groups, namely the first data set and the second data set. The first data set and the second data set are obtained in real time, and the variances of the first data set and the second data set are calculated respectively to obtain the first variance and the second variance. Multiple sensor data are obtained in real time, that is, multiple data sets are obtained by collecting data through multiple sensors. The variances of the multiple data sets are calculated, the data streams in each direction are obtained from multiple sensors in real time, and the variances of the data streams of each sensor are calculated to quantify their fluctuation degrees. The variance information is updated and stored in a specific variable in real time for subsequent analysis. Variance is an important indicator to measure the degree of data fluctuation. The smaller the variance, the smaller the data fluctuation, that is, the more stable the data. The data collected by its sensors at least includes lidar data and inertial measurement unit data. After calculating the variances of multiple data sets, the data set corresponding to the smaller variance value in the first variance and the second variance is subjected to data fusion to obtain fusion data. The fusion data includes a pose coordinate set, and the pose coordinate set is used to construct a navigation map. First, the variances of multiple data sets are screened through a comparison screening algorithm to obtain multiple data sets with small variance fluctuations. The variances of the data sets are compared through a comparison screening algorithm, and according to a preset threshold, multiple data sets corresponding to smaller variance values are screened out. After screening, multiple data sets corresponding to smaller variance values are output. By introducing real-time variance analysis and comparison screening algorithms, unstable factors in the data collected by sensors are effectively filtered, ensuring the retention of high-quality data. Secondly, data fusion is performed on the data set corresponding to the smaller variance value. The data fusion process is as follows: The Kalman filtering algorithm is used to perform phased screening on the data set corresponding to the smaller variance value, and the screened data sets are fused to obtain sensor fusion data. For specific steps, please refer to Figure 2 , such as Figure 2 shown: S1011, Step 1: Sort the data set corresponding to the smaller variance value to obtain the sensor data with the smallest variance; Sorting analysis is performed on the obtained multiple sensor data with small variance fluctuations, that is, pre-analyzing the variance information of each sensor data, performing real-time evaluation on multiple sensor data information, and identifying the sensor data with the smallest variance in the initial stage as the optimal data source.

[0029] S1012, Step 2: Perform Kalman filtering processing on the sensor data with the smallest variance using the Kalman filtering algorithm to obtain the first optimized data; After obtaining the optimal data source, the optimal data source is initially optimized through the Kalman filtering algorithm to obtain the optimized sensor data, that is, the first optimized data.

[0030] S1013. Step 3: Using the first optimized data as the first reference value, based on the first reference value, apply the Kalman filtering algorithm to perform Kalman filtering on the sensor data with the smallest remaining variance to obtain the second optimized data; Use the optimized sensor data as the reference value for subsequent fusion, that is, the first reference value. Subsequently, among the remaining sensor data, continuously search for the stable data source with the sub-optimal variance, and also apply Kalman filtering for fine processing to obtain the optimal output, that is, the first correction value.

[0031] S1014. Step 4: Using the second optimized data as the first correction value, apply the Kalman filtering algorithm to fuse the first correction value and the first reference value to obtain the fused data; Use the optimal output as the correction value for the current stage, and fuse it with the previous reference value, that is, fuse the first correction value and the first reference value using the Kalman filtering algorithm to form the dual-sensor fused data.

[0032] S1015. Step 5: Using the fused data as the second reference value, repeat steps 3 to 4 until the sensor data fusion is completed to obtain the sensor fused data.

[0033] Use the dual-fused data as the reference for the next round of fusion, that is, obtain the second reference value. This process is iterated. In each round, select the data with the smallest variance from the remaining sensors, optimize it through Kalman filtering as the new correction value, and deeply fuse it with the previous round of fusion result through the Kalman filtering algorithm, gradually expanding to the third, fourth, until the full fusion of all sensor data.

[0034] Through the way of cyclic iteration, not only makes full use of the unique advantages of each sensor, but also effectively eliminates noise through the powerful data processing ability of Kalman filtering, improves the accuracy and reliability of the fused data, and provides a solid data foundation for subsequent applications.

[0035] In some embodiments, in step S102, construct a probability grid map based on the pose coordinate set, and update the probability grid map according to the pose influence factor to obtain a real-time navigation map. The pose influence factor is used to characterize the influence degree of different sensors corresponding to the fused data on the pose coordinates; after obtaining the sensor fused data, use the MSF-Cartographer algorithm to construct the navigation map, and optimize the local mapping stage, loop detection stage and global mapping stage of the MSF-Cartographer algorithm by introducing the pose influence factor; in order to enhance the ability of the MSF-Cartographer algorithm to handle complex environments, the pose influence factor is introduced The construction strategy takes into account the variance information stored by multiple sensors during the data fusion stage. By integrating this variance data, a pose influence factor that can reflect the reliability of sensor data is constructed. That is, it characterizes the influence degree of different sensors corresponding to the fused data on the pose coordinates. By effectively measuring the data information received by the sensors through the pose influence factor, the mapping quality of the MSF-Cartographer algorithm is further analyzed. The specific expression of its pose influence factor is: , where, is the pose influence factor, is the number of sensors in the fusion stage, is the th variance value received by the sensor in real time; The calculated pose influence factor assists the MSF-Cartographer algorithm in precisely comparing each input radar scan frame (scan) with the local submap data when updating the map, and performing corresponding mapping optimization operations to obtain more refined map information; After obtaining the sensor fusion data, the MSF-Cartographer algorithm determines the radar scan frame data based on the sensor fusion data, determines the pose coordinate set based on the radar scan frame data, constructs a probability grid map based on the pose coordinate set, marks the pose coordinates in the probability grid map based on the pose influence factor, and updates the probability grid map to obtain a real-time navigation map.

[0036] After obtaining the sensor fusion data, the MSF-Cartographer algorithm enters the local mapping stage. In the local mapping stage of the MSF-Cartographer algorithm: The radar scan frame data can be determined through real-time sensor fusion data, and the pose coordinate set can be determined through real-time scanning of the radar scan frame. And the influence factor corresponding to the pose coordinates is determined according to the construction strategy of the pose influence factor. , its pose influence factor is used as the confidence of the pose coordinates, and the influence factor (confidence) at the pose coordinates is used to optimize the probability grid map; the submaps in MSF-Cartographer use the Probability Grids. Each grid in the probability grid map has three states, namely unknown, miss, and hit, that is, the states of the grid positions in the probability grid map include the unknown state, the non-occupied state, and the occupied state; after obtaining the pose coordinate set through the radar scan frame, a probability grid map is constructed based on the pose coordinate set, and the pose coordinates in the probability grid map are marked based on the pose influence factor, and the probability grid map is updated to generate a local submap. The update process is as follows: after determining the pose influence factor, set the threshold for constructing the probability grid map , when the pose influence factor is less than or equal to the threshold for constructing the probability grid map , it is considered that the radar scan frame (scan) at this place is not credible, and the grid position corresponding to the pose coordinates is marked as the unknown state, that is, the grids covered by this area are set to the unknown state, and the radar scan information here is discarded; when the pose influence factor is greater than the threshold for constructing the probability grid map, it is considered credible, and the grid position corresponding to the pose coordinates is marked as the non-occupied state, that is, the grid state covered by this area is the grid state obtained by actual scanning; by introducing the pose influence factor, it is possible to effectively avoid incorrect mapping when the confidence of the pose coordinates is low, thereby improving the reliability of the MSF-Cartographer algorithm.

[0037] In the local mapping stage, by introducing the analysis of the pose coordinate confidence (pose influence factor), the state of the grid map is dynamically adjusted to optimize the local mapping process. When the pose coordinate confidence is lower than the threshold, the grids in this area are set to the unknown state to avoid the interference of unreliable data; when the confidence is higher, the actual scan results are retained. Through this strategy, the accuracy and reliability of map construction are effectively improved, the problems of insufficient confidence processing, lack of dynamic optimization, and error accumulation are solved, and the local mapping accuracy and overall reliability of the MSF-Cartographer algorithm are significantly improved.

[0038] When the real-time navigation map is a local submap, the MSF-Cartographer algorithm will enter the loop closure detection stage and the global mapping stage; in the loop closure detection stage, the branch and bound method is used to perform loop closure detection on the fused data, and the pose coordinates of the local submap are updated based on the pose influence factor; in the global mapping stage, the updated local submap is globally optimized based on the pose influence factor to generate a navigation map; In the closed-loop detection stage of the MSF-Cartographer algorithm: The branch and bound method is used to perform closed-loop detection on the radar scan frame data. The pose coordinates of the local submap are updated based on the pose influence factor. After each successful closed-loop detection, the updated local submap is transferred to the global mapping; in the MSF-Cartographer algorithm, when performing local optimization on the pose the pose influence factor is introduced through the branch and bound method Closed-loop detection is performed on each frame of radar data input in the sensor fusion data. The MSF-Cartographer algorithm uses branch and bound to perform the closed-loop detection task, that is, the branch and bound method is used to perform closed-loop detection on the radar scan frame data in the real-time fusion data. The branch and bound to perform the closed-loop detection task includes: candidate submap search, multi-resolution map construction, branch traversal and boundary calculation, pruning strategy application, optimal solution verification and selection, non-linear optimization verification; when using the branch and bound method to perform closed-loop detection on the fusion data, the radar scan frame data at the current moment is obtained based on the fusion data, and the pose influence factor corresponding to the radar scan frame data is compared with the threshold value of the probability grid map construction to determine whether the pose coordinates of the radar scan frame data are added to the local submap. When the pose coordinates corresponding to the radar scan frame data are located in the local submap, the first influence factor of the pose coordinates in the local submap is obtained ; obtain the radar scan frame data at the next moment. When the pose coordinates of the radar scan frame data at the next moment coincide with the pose coordinates of the radar scan frame data at the current moment, obtain the second influence factor of the pose coordinates of the radar scan frame data at the next moment , when the first influence factor is less than the second influence factor, that is when, update the pose coordinates corresponding to the first influence factor to the pose coordinates of the radar scan frame data at the next moment. When when, discard the pose coordinates corresponding to the second influence factor detected at this time corresponding, and still use corresponding pose coordinates as the pose information of the local submap. In order to reduce the computing resources of the hardware and maintain the stability of mapping at a high confidence level, and reduce the mapping drift problem caused by data variance at a high confidence level, a confidence threshold is set. When when, it is considered that the mapping information is reliable, and the comparison and coverage tasks of the pose of the newly detected radar frame with the original pose will be stopped, that is, it means that this closed-loop detection is over, and the corresponding local submap is transferred to the global mapping, and the local submap is globally optimized through the global mapping; the setting of the confidence threshold can be adjusted according to actual needs.

[0039] In the closed-loop detection stage, the pose influence factor is introduced to optimize the closed-loop detection. By comparing the influence factors of the old and new poses, it is dynamically determined whether to update the pose information in the submap, thereby improving the accuracy of the closed-loop detection. By setting a confidence threshold, the consumption of computing resources is reduced and the mapping drift problem under high confidence is prevented, improving the efficiency.

[0040] In the global mapping stage of the MSF-Cartographer algorithm: Based on the pose influence factor, the sparse pose graph is used to globally optimize each updated local submap to generate a navigation map; in the closed-loop detection stage, after determining that the updated local submap needs to be globally optimized, based on the pose influence factor, the sparse pose graph is used to globally optimize each local submap, and the pose coordinate influence factor of the local submap is determined based on the pose influence factor And the pose coordinate influence factors corresponding to all radar scan frame data, and the pose coordinate influence factors corresponding to all radar scan frame data are the influence factors of the global pose coordinates , when When, use The corresponding global pose coordinates replace The corresponding pose coordinates. When When, use The pose coordinates of the corresponding local submap replace The corresponding global pose coordinates to complete the optimization of the local submap. After completing the optimization of each local submap, project the optimized local submap into a unified coordinate system and fuse the probability grid data to generate a globally consistent map, that is, generate the navigation map of the robot device.

[0041] In the global mapping stage, by introducing the influence factor between the global pose and the local pose, the replacement relationship between the two is dynamically determined. This strategy ensures that in the global optimization process, a more reliable pose can be selected according to the confidence of the pose, thereby improving the accuracy and consistency of the global map. In addition, by dynamically adjusting the optimization strategy, different quality data can be flexibly handled, avoiding the deficiencies in conflict handling and the defects of the static optimization strategy, and significantly improving the optimization effect of the global mapping.

[0042] In summary, the navigation map construction method provided by the present invention performs variance screening on at least two data sets, and fuses the data sets that meet the variance screening conditions to obtain fused data; at least two data sets are obtained by real-time collection of different sensors, and the fused data includes a pose coordinate set; a probability grid map is constructed based on the pose coordinate set, and the probability grid map is updated according to the pose influence factor to obtain a real-time navigation map. The pose influence factor is used to characterize the influence degree of different sensors corresponding to the fused data on the pose coordinates, improving the accuracy and reliability of the navigation map.

[0043] To better implement the navigation map construction method in the embodiments of the present invention, correspondingly, based on the navigation map construction method, as Figure 3 shown, the embodiments of the present invention further provide a navigation map construction device. The navigation map construction device 300 includes a data fusion module 301 and a navigation map construction module 302; The data fusion module 301 is configured to perform variance screening on at least two data sets, and fuse the data sets that meet the variance screening conditions to obtain fused data; the at least two data sets are obtained by real-time acquisition of different sensors, and the fused data includes a pose coordinate set; The navigation map construction module 302 is configured to construct a probability grid map based on the pose coordinate set, and update the probability grid map according to the pose influence factor to obtain a real-time navigation map. The pose influence factor is used to characterize the influence degree of different sensors corresponding to the fused data on the pose coordinates.

[0044] The above is only a preferred specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily thought of by those skilled in the art within the technical scope disclosed by the present invention should be covered by the protection scope of the present invention.

Claims

1. A method for constructing a navigation map, characterized in that, Including: Performing variance screening on at least two data sets, and fusing the data sets that meet the variance screening conditions to obtain fused data; The at least two data sets are obtained by real-time acquisition of different sensors, and the fused data includes a pose coordinate set; Constructing a probability grid map based on the pose coordinate set, and updating the probability grid map according to the pose influence factor to obtain a real-time navigation map, where the pose influence factor is used to characterize the influence degree of different sensors corresponding to the fused data on the pose coordinates.

2. The navigation map construction method according to claim 1, wherein There are two data sets, namely the first data set and the second data set; Performing variance screening on at least two data sets, and fusing the data sets that meet the variance screening conditions to obtain fused data, including: Obtaining the first data set and the second data set in real time; Calculating the variances of the first data set and the second data set respectively to obtain the first variance and the second variance; Fusing the data sets corresponding to the smaller variance value among the first variance and the second variance to obtain fused data.

3. The navigation map construction method according to claim 2, wherein The fusing the data sets corresponding to the smaller variance value among the first variance and the second variance to obtain fused data includes: Step 1: Sorting the data set corresponding to the smaller variance value to obtain the sensor data with the smallest variance; Step 2: Performing Kalman filtering on the sensor data with the smallest variance by using the Kalman filtering algorithm to obtain the first optimized data; Step 3: Taking the first optimized data as the first reference value, and based on the first reference value, performing Kalman filtering on the remaining sensor data with the smallest variance by using the Kalman filtering algorithm to obtain the second optimized data; Step 4: Taking the second optimized data as the first correction value, and fusing the first correction value and the first reference value by using the Kalman filtering algorithm to obtain fused data; Step 5: Taking the fused data as the second reference value, and repeating Step 3 to Step 4 until the sensor data fusion is completed to obtain the sensor fusion data.

4. The navigation map construction method according to claim 1, characterized in that, The specific expression of the pose influence factor is: ， Among them, is the pose influence factor, n represents the number of sensors in the fusion stage, represents the i variance value received in real time by the th sensor.

5. The navigation map construction method according to claim 1, characterized in that, The constructing a probability grid map based on the pose coordinate set, and updating the probability grid map according to the pose influence factor to obtain a real-time navigation map includes: Determining radar scan frame data based on the fused data, determining a pose coordinate set based on the radar scan frame data, and constructing a probability grid map based on the pose coordinate set; Marking the pose coordinates in the probability grid map based on the pose influence factor, and updating the probability grid map to obtain a real-time navigation map.

6. The navigation map construction method according to claim 5, characterized in that, The state of the grid position in the probability grid map includes an unknown state, a non-occupied state, and an occupied state; the marking the pose coordinates in the probability grid map based on the pose influence factor includes: Setting a threshold for constructing the probability grid map, and when the pose influence factor is less than or equal to the threshold for constructing the probability grid map, marking the grid position corresponding to the pose coordinate as the unknown state; When the pose influence factor is greater than the threshold for constructing the probability grid map, marking the grid position corresponding to the pose coordinate as the non-occupied state.

7. The navigation map construction method according to claim 1, characterized in that The real-time navigation map is a local sub-map, and the method further includes: The closed-loop detection is performed on the fused data by using the branch and bound method, and the pose coordinates of the local submap are updated based on the pose influence factor; The globally optimized updated local submap is generated based on the pose influence factor to generate a navigation map.

8. The navigation map construction method according to claim 7, characterized in that, The closed-loop detection is performed on the fused data by using the branch and bound method, and the pose coordinates of the local submap are updated based on the pose influence factor, including: When the closed-loop detection is performed on the fused data by using the branch and bound method, the radar scan frame data at the current moment is obtained based on the fused data, and the pose influence factor corresponding to the radar scan frame data is compared with the threshold value constructed by the probability grid map to determine whether the pose coordinates are added to the local submap. When the pose coordinates corresponding to the radar scan frame data are located in the local submap, the first influence factor of the pose coordinates in the local submap is obtained; The radar scan frame data at the next moment is obtained. When the pose coordinates corresponding to the radar scan frame data at the next moment coincide with the pose coordinates corresponding to the radar scan frame data at the current moment, the second influence factor of the pose coordinates corresponding to the radar scan frame data at the next moment is obtained. When the first influence factor is less than the second influence factor, the pose coordinates corresponding to the first influence factor are updated to the pose coordinates corresponding to the radar scan frame data at the next moment.

9. The navigation map construction method according to claim 7, wherein The globally optimized updated local submap is generated based on the pose influence factor to generate a navigation map, including: Based on the pose influence factor, each updated local submap is globally optimized through a sparse pose graph to generate a navigation map.

10. A navigation map construction device, characterized in that, Including: A data fusion module for performing variance screening on at least two data sets and fusing the data sets that meet the variance screening conditions to obtain fused data; The at least two data sets are obtained by real-time collection of different sensors, and the fused data includes a pose coordinate set; A navigation map construction module for constructing a probability grid map based on the pose coordinate set and updating the probability grid map according to the pose influence factor to obtain a real-time navigation map, where the pose influence factor is used to characterize the influence degree of different sensors corresponding to the fused data on the pose coordinates.

Images