A method for real-time localization and map building

Abstract

This invention discloses a real-time positioning and mapping method, relating to the field of information processing technology. It involves self-calibrating all sensors in a real-time positioning and mapping system, performing joint calibration among multiple sensors based on the transformation relationships between them, and collecting data from k time nodes t1 to t2 within a time interval t. k The characteristic data values ​​of all sensors at time t are preprocessed to construct a data extrapolation model and calculate the end time node t of time interval t. k The feature data values ​​of all sensors at any given time are registered, and then the feature data values ​​of multiple sensors are fused. Mapping and navigation are built based on the data fusion results. This can solve the technical problem in the existing technology that relies mainly on sensors with slow transmission rates for data fusion, resulting in low timeliness of map building and autonomous navigation positioning in unknown environments.

Description

Technical Field

[0001] This invention relates to the field of information processing technology, and in particular to a real-time positioning and map building method. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) excels at map building and autonomous navigation in unknown environments without GPS signals. Current SLAM methods primarily utilize sensors such as LiDAR, cameras, ultrasonic sensors, UAV oblique photography, inertial measurement units (IMUs), and odometry. However, using a single sensor for SLAM in unknown environments has limitations. For example, LiDAR SLAM has poor global positioning capabilities, and visual SLAM accuracy is easily affected by lighting conditions. Multi-sensor fusion-based SLAM, on the other hand, fully leverages the advantages of various sensors in its hardware structure, overcoming the shortcomings of single-sensor SLAM and further improving the positioning and navigation accuracy and robustness of autonomous SLAM navigation. Currently, mainstream multi-sensor combination methods include visual and inertial navigation fusion SLAM, LiDAR and inertial navigation fusion SLAM, LiDAR and visual fusion SLAM, and millimeter-wave and LiDAR fusion SLAM.

[0003] Multi-sensor fusion SLAM relies on accurate descriptions of the same target by multiple sensors. Due to differences in data rates and data node inconsistencies among sensors, joint calibration and data fusion are necessary to ensure that all sensors describe the same object simultaneously. Current multi-sensor fusion methods are primarily based on loose coupling or a single sensor as the primary sensor with others as secondary sensors. These methods have low data coupling and, due to varying data rates, often prioritize the slower-speed sensor for data fusion to ensure data integrity. This results in low timeliness for map building and autonomous navigation in unknown environments. Therefore, we propose a real-time localization and map building method. Summary of the Invention

[0004] The main objective of this invention is to provide a real-time positioning and map building method that can effectively solve the problems in the background art.

[0005] To achieve the above objectives, the technical solution adopted by the present invention is as follows: A real-time localization and mapping method includes the following steps: Step 1: Perform self-calibration on all sensors of the real-time positioning and mapping system, and perform joint calibration among multiple sensors based on the transformation relationship between the sensors; Step 2: Collect k time nodes within time interval t. Feature data values ​​of all sensors at time 1 , This represents the feature data value of the j-th sensor of the i-th class at the k-th time node, where k is the time node number, i is the sensor type number, and j is the sensor number of the same type. Step 3: Process the collected sensor feature data values Preprocessing is performed; if the collected sensor data is missing, the missing data is recorded as 0. A data extrapolation model is constructed to calculate the end time node of the time interval t. Feature data values ​​of all sensors at time 10:00 ; Step four, obtain the sensor feature data values After registration, the feature data values ​​of multiple sensors are... Perform data fusion and build navigation maps based on the data fusion results; In step three, the steps for constructing the data extrapolation model are as follows: Step S1, determine the main fusion sensor, wherein the method for determining the main fusion sensor comprises the following steps: Step S11: Construct a hierarchical analysis model, use the scaling method to compare the importance of any two groups of sensors to positioning accuracy, calculate the weight value of each sensor to positioning accuracy through the judgment matrix, and perform a consistency check on the judgment matrix. Step S12: Query the feature value dataset by timestamp. The v-th time node of each sensor within the time interval t The data upload time at any given moment is used to obtain the sorting of data upload times for each sensor. Step S13: Based on the weight values ​​of each sensor's importance to positioning accuracy calculated in Step S11 and the data upload time sorting results of each sensor in Step S12, the formula is used: The sensor corresponding to the maximum value of the calculated result is the main fusion sensor, where, , These are the weighting coefficients, ; Weighting coefficient , The value is determined by the sensor type and empirical values; generally, it is taken as... ; This represents the weight value of the j-th sensor of the i-th sensor class in relation to the positioning accuracy. This represents the sorted data upload time value of the j-th sensor in the i-th sensor category; Let the main fusion sensor be a type r sensor, based on the feature data values Construct the feature data value matrix of the main fusion sensor ,in, Let n be the total number of sensors of type r, then...

[0006] Step S2, based on the feature data value matrix Build time nodes Feature value dataset of all sensors at time 10:00 ,in, ; Step S3: Extract the feature data value matrix The first in List and No. Constructing an initial vector point set from column elements and the set of vector points at the end ,in ; u is a positive integer, and ; Calculate the initial vector point set Up to the last vector point set vector increment point set ,in, ; Step S4, construct a set of vector increment points The calculation results are used as input to predict the incremental point set. The output neural network model is generated, and the established neural network model is trained. Based on the error between the training results and the actual results, the number of hidden layers in the model is adjusted until the accuracy is not lower than the expected value. Finally, the predicted incremental point set is output through the constructed neural network model. ,in, ; Step S5, based on the obtained incremental point set Computational data fusion node Feature data values ​​of the main fusion sensor at any given time ,in, ; Step S6: Repeat steps S1 to S5 to determine the data fusion node. Feature data values ​​of all sensors at time 1 Obtain data fusion nodes Feature value dataset at time t .

[0007] Furthermore, in step S4, the formula for calculating the expected value of the neural network model is:

[0008] in, N represents the expected value; N represents the number of input samples to the neural network model. This represents the output function of a neural network model. This represents the i-th output sample of the neural network model.

[0009] The present invention has the following beneficial effects: (1) Compared with the prior art, the technical solution of the present invention performs self-calibration of all sensors in the real-time positioning and mapping system, performs joint calibration among multiple sensors according to the transformation relationship between sensors, and collects k time nodes within the time interval t. Feature data values ​​of all sensors at time 1 The characteristic data values ​​of the collected sensors Preprocessing is performed, a data extrapolation model is constructed, and the end time node of time interval t is calculated. Feature data values ​​of all sensors at time 10:00 The acquired sensor feature data values After registration, the feature data values ​​of multiple sensors are... Data fusion is performed, and navigation is built based on the fusion results. The main fusion sensor is determined through a data extrapolation model, and the data fusion nodes are obtained. Feature data values ​​of all sensors at time 1 and data fusion nodes Feature value dataset at time t It can solve the technical problem that existing technologies rely mainly on sensors with slow transmission rates for data fusion, resulting in low timeliness of map building and autonomous navigation positioning in unknown environments. Attached Figure Description

[0010] Figure 1 This is a schematic diagram of the overall structure of the real-time positioning and map building method of the present invention. Detailed Implementation

[0011] The present invention will be further described below with reference to specific embodiments. The accompanying drawings are for illustrative purposes only and are schematic diagrams, not actual pictures. They should not be construed as limiting the present invention. In order to better illustrate the specific embodiments of the present invention, some parts in the drawings may be omitted, enlarged or reduced, and do not represent the actual product size.

[0012] Example 1 like Figure 1 The diagram shown is a flowchart of a real-time positioning and map construction method according to the technical solution of the present invention.

[0013] In this embodiment, the technical solution of the present invention is illustrated using SLAM that combines laser and vision as an example: A real-time localization and mapping method includes the following steps: Step 1: Perform self-calibration on all sensors of the real-time localization and mapping system. Then, perform joint calibration among multiple sensors based on the transformation relationships between them. The method is as follows: obtain the intrinsic parameter matrix from the camera's intrinsic parameter calibration; obtain the camera's extrinsic parameter matrix through the extrinsic parameter calibration of the camera and LiDAR; and obtain the transformation relationship from the 3D LiDAR coordinate system to the pixel coordinate system.

[0014] In the formula, P is the homogeneous coordinate of the target point in the lidar coordinate system; K is the intrinsic parameter matrix; T is the extrinsic parameter matrix; U and V are the coordinates of the target point in the pixel coordinate system. By transforming the lidar coordinate system to the pixel coordinate system, color information can be assigned to the 3D point cloud in the camera's field of view. Step 2: Collect K time nodes within time interval T. Feature data values ​​of all sensors at time 1 , This represents the feature data value of the j-th sensor of the i-th class at the k-th time node, where k is the time node number, i is the sensor type number, and j is the sensor number of the same type. Step 3: Process the collected sensor feature data values Preprocessing is performed, and the basic processing steps are as follows: a) Perform ground point segmentation. Determine the estimated ground point cloud by observing the number of rows occupied by the ground points in the point cloud map. In this type of point cloud, calculate the slope between two adjacent points in the column direction to determine the ground points. b. Use nearest-neighbor clustering to segment the remaining point cloud. If a point has more than 30 nearest neighbors or more than 3 rows of vertically distributed nearest neighbors, it is considered a valid cluster point; otherwise, it is judged as an outlier. c. The processed point cloud still contains easily occluded points and breakpoints that are unstable in inter-frame matching. It is necessary to use the distance between two adjacent points and the lidar to perform depth estimation to remove these two types of points. If the collected sensor data is missing, record the missing data as 0, construct a data extrapolation model, and calculate the end time node of the time interval t. Feature data values ​​of all sensors at time 10:00 The steps for constructing the data extrapolation model are as follows: Step S31, determine the main fusion sensor, wherein the method for determining the main fusion sensor comprises the following steps: Step S311: Construct a hierarchical analysis model, use the scaling method to compare the importance of any two groups of sensors to positioning accuracy, calculate the weight value of each sensor to positioning accuracy through the judgment matrix, and perform a consistency check on the judgment matrix. Step S312: Query the feature value dataset by timestamp. The v-th time node of each sensor within the time interval t The data upload time at any given moment is used to obtain the sorting of data upload times for each sensor. Step S313: Based on the weight values ​​of each sensor's importance to positioning accuracy calculated in step S311 and the data upload time sorting results of each sensor in step S312, the formula is used: The sensor corresponding to the maximum value of the calculation result is determined as the main fusion sensor. , These are the weighting coefficients, ; Generally take ; This represents the weight value of the j-th sensor of the i-th sensor class in relation to the positioning accuracy. This represents the sorted data upload time value of the j-th sensor in the i-th sensor category; Let the main fusion sensor be a type r sensor, based on the feature data values Construct the feature data value matrix of the main fusion sensor ,in, Let n be the total number of sensors of type r, then...

[0015] Step S32, based on the feature data value matrix Build time nodes Feature value dataset of all sensors at time 10:00 ,in, ; Step S33: Extract the feature data value matrix The initial vector point set is constructed from the elements of the u-th and u+1-th columns. and the set of vector points at the end ,in ; u is a positive integer, and ; Step S34, calculate the initial vector point set Up to the last vector point set vector increment point set ,in, ; Step S35, construct a set of vector increment points The calculation results are used as input to predict the incremental point set. The output neural network model is generated, and the established neural network model is trained. Based on the error between the training results and the actual results, the number of hidden layers in the model is adjusted until the accuracy is not lower than the expected value. The formula for calculating the expected value is as follows:

[0016] in, N represents the expected value; N represents the number of input samples to the neural network model. This represents the output function of a neural network model. This represents the i-th output sample of the neural network model; The constructed neural network model outputs the predicted set of incremental points. ,in, ; Step S36, based on the acquired incremental point set Computational data fusion node Feature data values ​​of the main fusion sensor at any given time ,in, ; Step S37: Repeat steps S1 to S5 to determine the data fusion node. Feature data values ​​of all sensors at time 1 Obtain data fusion nodes Feature value dataset at time t ; Step four, obtain the sensor feature data values After registration using the ICP algorithm, the Kalman filter algorithm is used to fuse the feature data values ​​of multiple sensors, and the map is built and navigation is based on the data fusion results.

[0017] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely illustrative of the principles of the invention. Various changes and modifications can be made to the invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the present invention as claimed. The scope of protection of this invention is defined by the appended claims and their equivalents.

Claims

1. A method for real-time positioning and map construction, characterized in that, Includes the following steps, Step 1: Perform self-calibration on all sensors of the real-time positioning and mapping system, and perform joint calibration among multiple sensors based on the transformation relationship between the sensors; Step 2, data collection time interval k time nodes within Feature data values ​​of all sensors at time 1 , This represents the feature data value of the j-th sensor of the i-th class at the k-th time node, where k is the time node number, i is the sensor type number, and j is the sensor number of the same type. Step 3: Process the collected sensor feature data values Preprocessing is performed, a data extrapolation model is constructed, and the end time node of time interval t is calculated. Feature data values ​​of all sensors at time 10:00 ; Step four, obtain the sensor feature data values After registration, the feature data values ​​of multiple sensors are... Perform data fusion and build navigation maps based on the data fusion results; In step three, the steps for constructing the data extrapolation model are as follows: Step S1: Determine the main fusion sensor. Let the main fusion sensor be a type r sensor. Based on the feature data values... Construct the feature data value matrix of the main fusion sensor ,in, Let n be the total number of sensors of type r, then... ; Step S2, based on the feature data value matrix Build time nodes Feature value dataset of all sensors at time 10:00 ,in, ; Step S3: Extract the feature data value matrix The uth column and the th Column elements construct the initial vector point set and the set of vector points at the end ,in ; u is a positive integer, and ; Calculate the initial vector point set Up to the last vector point set vector increment point set ,in, ; Step S4, construct a set of vector increment points The calculation results are used as input to predict the increment point set. The output neural network model is generated, and the established neural network model is trained. Based on the error between the training results and the actual results, the number of hidden layers in the model is adjusted until the accuracy is not lower than the expected value. Finally, the predicted incremental point set is output through the constructed neural network model. ,in, ; Step S5, based on the obtained incremental point set Computational data fusion node Feature data values ​​of the main fusion sensor at any given time ,in, ; Step S6: Repeat steps S1 to S5 to determine the data fusion node. Feature data values ​​of all sensors at time 1 Obtain data fusion nodes Feature value dataset at time t .

2. The real-time positioning and map building method according to claim 1, characterized in that, In step S1, the steps for determining the main fusion sensor are as follows: Step S11: Construct a hierarchical analysis model, use the scaling method to compare the importance of any two groups of sensors to positioning accuracy, calculate the weight value of each sensor to positioning accuracy through the judgment matrix, and perform a consistency check on the judgment matrix. Step S12: Query the feature value dataset by timestamp. The v-th time node of each sensor within the time interval t The data upload time at any given moment is used to obtain the sorting of data upload times for each sensor. Step S13: Based on the weight values ​​of each sensor's importance to positioning accuracy calculated in Step S11 and the data upload time sorting results of each sensor in Step S12, the formula is used: The calculation results determine the main fusion sensor, where, , These are the weighting coefficients, ; ; This represents the weight value of the j-th sensor of the i-th sensor class in relation to the positioning accuracy. This represents the sorted data upload time value of the j-th sensor in the i-th sensor class.

3. The real-time positioning and map building method according to claim 2, characterized in that: In step S13, the main fusion sensor is determined by the formula: The sensor corresponding to the maximum value of the calculation result.

4. The real-time positioning and map building method according to claim 1, characterized in that, In step three, if the collected sensor data is missing, the missing data value is recorded as 0.

5. The instant positioning and map building method according to claim 1, characterized in that, In step S4, the formula for calculating the expected value of the neural network model is: ; in, Indicates the expected value; This indicates the number of input samples to the neural network model; This represents the output function of a neural network model. This represents the i-th output sample of the neural network model.

6. The real-time positioning and map building method according to claim 3, characterized in that: In step S13, the weighting coefficients , Determined by sensor type and empirical values, take .

Images