A multi-sensor multi-modal close-coupling fusion mapping and positioning method and system

Abstract

The application belongs to the field of robot positioning and mapping, and discloses a multi-sensor multi-modal tight coupling fusion mapping and positioning method and system. The method comprises the following steps: pre-processing the original data obtained by each sensor; correlating and fusing the pre-processed data of different sensors to obtain fused data; estimating and optimizing the position and attitude of the vehicle state based on the fused data; and outputting the positioning and constructing the environment map according to the estimated and optimized vehicle state and the fused data. The application realizes the tight coupling of multi-sensor multi-modal, thereby overcoming the problem of insufficient robustness in the traditional method. By combining the prediction ability of the planning control prediction odometer and the environment perception ability of other sensors, the application can more accurately predict the vehicle position and realize high-precision positioning and mapping in a complex environment.

Description

Technical Field

[0001] This invention belongs to the field of robot localization and mapping, and specifically relates to a multi-sensor, multi-modal tightly coupled fusion mapping and localization method and system. Background Technology

[0002] In the current technological context, traditional localization and mapping methods often exhibit insufficient robustness when facing complex environments and uncertainties. This is mainly reflected in the following aspects: Robustness challenges caused by environmental complexity: In complex scenarios such as urban roads and indoor environments, there are numerous dynamic obstacles, irregular road surfaces, and temporary obstacles. These factors can interfere with sensor data, making it difficult for traditional localization and mapping algorithms to handle them accurately. The impact of sensor errors and uncertainties: Sensors themselves have errors and uncertainties. For example, inertial sensors may be affected by vibration and drift, while visual sensors may be affected by changes in lighting and occlusion. These errors and uncertainties further affect the accuracy and stability of localization and mapping. Challenges caused by dynamic environmental changes: Factors such as road traffic and pedestrian behavior increase the dynamism of the environment. Traditional static mapping methods struggle to update maps in real time, leading to inconsistencies between localization and mapping results and the actual environment. Summary of the Invention

[0003] The purpose of this invention is to provide a multi-sensor, multi-modal, tightly coupled fusion mapping and localization method and system to solve the problem that traditional static mapping methods are difficult to update maps in real time, resulting in inconsistencies between localization and mapping results and the actual environment.

[0004] To achieve the above objectives, the present invention adopts the following technical solution:

[0005] In a first aspect, the present invention provides a multi-sensor, multi-modal tightly coupled fusion mapping and localization method, comprising:

[0006] The raw data acquired by each sensor is preprocessed;

[0007] The preprocessed data from different sensors are correlated and fused to obtain fused data;

[0008] Based on the fused data, the vehicle's position and attitude are estimated and optimized;

[0009] Based on the estimated and optimized vehicle status and the fused data, the localization is output and an environmental map is constructed.

[0010] Optionally, raw data acquired through various sensors may be used, including: steering data, wheel speed data, inertial measurement unit data, camera data, and lidar data acquired from the vehicle.

[0011] Optionally, the original data is pre-processed and feature extracted, including:

[0012] The steering data and wheel speed data are filtered, denoised, and pre-integrated to obtain a control prediction odometry; the inertial measurement unit (IMU) data are filtered, denoised, and integrated to obtain an IMU odometry; the camera data are calibrated to obtain images with distortion removed; the laser radar data and the inertial measurement unit (IMU) data are combined to obtain data after motion distortion; the sensors are calibrated to remove distortion and eliminate inherent errors; and the inertial measurement unit random error is filtered using Kalman filtering.

[0013] Optionally, the pre-processed data of different sensors are associated and fused, including:

[0014] The control prediction odometry and the IMU odometry are fused to obtain a fused odometry; the control prediction odometry and the IMU odometry are fused using a weight method, and the weight is determined through calibration testing;

[0015] The image-recognized traffic participants and the laser radar-recognized traffic participants are mutually accumulated and then removed separately; the image-recognized traffic participants are recognized using a deep learning method; the point cloud is removed from the ground using a traditional algorithm, and the relative speed of the traffic participants is determined by clustering; and the fusion refers to removing all the obstacles recognized by the image and the point cloud.

[0016] Optionally, based on the fused data, the vehicle state is estimated and optimized in terms of position and attitude, including:

[0017] The obstacle-removed picture is subjected to feature point extraction to extract lane lines, pedestrian crossings, parking space information, traffic signs, and buildings; after the laser radar point cloud is removed from the obstacles, feature point extraction is performed on the point cloud to extract lane line information, parking space information, traffic signs, and line, inflection point, and surface extraction of buildings; and the image is subjected to deep learning to obtain obstacles.

[0018] Optionally, according to the estimated and optimized vehicle state and the fused data, a positioning is output, including:

[0019] The respective corresponding matching of the fused odometry and the picture and point cloud feature points is performed to obtain a visual odometry and a laser odometry, the pose estimation results obtained by the visual odometry and the laser odometry are fused through a Kalman filter, the fused pose estimation results are accumulated to obtain the position of the vehicle in a global coordinate system.

[0020] Optionally, an environment map is constructed, including:

[0021] Selecting a map type, constructing an environment map using a simultaneous localization and mapping (SLAM) algorithm based on fused sensor data, minimizing re-projection errors using graph optimization or nonlinear optimization algorithms, and correcting cumulative map errors through loop closure detection.

[0022] In a second aspect, the present application provides a multi-sensor multi-modal tightly coupled fusion mapping and positioning system, comprising:

[0023] A data acquisition module for preprocessing raw data acquired by each sensor;

[0024] A data fusion module for associating and fusing the preprocessed data of different sensors to obtain fused data;

[0025] An optimization module for estimating and optimizing the position and attitude of the vehicle state based on the fused data;

[0026] An output module for outputting positioning and constructing an environment map based on the estimated and optimized vehicle state and the fused data.

[0027] In a third aspect, the present application provides a computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of a multi-sensor multi-modal tightly coupled fusion mapping and positioning method.

[0028] In a fourth aspect, the present application provides a computer-readable storage medium storing a computer program, wherein the computer program is executed by a processor to implement the steps of a multi-sensor multi-modal tightly coupled fusion mapping and positioning method.

[0029] Compared with the prior art, the present application has the following technical effects:

[0030] The present application introduces a planning control predictive odometry technology. Planning control predictive odometry utilizes the vehicle's dynamics model and environmental perception information to achieve accurate estimation of the vehicle's position by predicting the vehicle's future trajectory. Compared with traditional single sensor positioning methods, planning control predictive odometry can better cope with dynamic environments and uncertain factors, improving the robustness and accuracy of positioning. By fusing planning control predictive odometry with other sensor data, the present application realizes multi-sensor multi-modal tight coupling, thereby overcoming the problem of insufficient robustness in traditional methods. By combining the predictive ability of planning control predictive odometry and the environmental perception ability of other sensors, the present application can more accurately predict the vehicle's position and achieve high-precision positioning and mapping in complex environments. BRIEF DESCRIPTION OF DRAWINGS

[0031] Figure 1 Flow chart for the present application.

[0032] Figure 2 Logic block diagram for the present application. DETAILED DESCRIPTION

[0033] The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are some of the embodiments of the present application, but not all the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative work fall within the protection scope of the present application.

[0034] In the description of the present application, it should be understood that the terms "comprising" and "including" indicate the presence of described features, integers, steps, operations, elements, and / or components, but do not exclude one or more other features, integers, steps, operations, elements, components, and / or sets thereof.

[0035] It should also be understood that the terms used in the specification of the present application are only for the purpose of describing specific embodiments and are not intended to limit the present application. As used in the specification and the appended claims of the present application, unless otherwise clearly indicated by the context, the singular forms "a", "an" and "the" are intended to include the plural forms.

[0036] It should be further understood that the term "and / or" used in the specification and the appended claims of the present application means any combination of one or more of the associated listed items and all possible combinations, and includes these combinations, for example, A and / or B can represent three cases of A alone, A and B together, and B alone. In addition, the character " / " in the present application generally represents an "or" relationship between the front and rear associated objects.

[0037] It should be understood that although the terms first, second, third, etc. may be used in the embodiments of the present application to describe the preset ranges, etc., these preset ranges should not be limited to these terms. These terms are only used to distinguish the preset ranges from each other. For example, the first preset range can also be referred to as the second preset range, and similarly, the second preset range can also be referred to as the first preset range without departing from the scope of the embodiments of the present application.

[0038] Depending on the context, the word "if" as used herein can be interpreted to mean "when" or "while" or "in response to determining" or "in response to detecting." Similarly, the phrase "if it is determined" or "if [a stated condition or event] is detected" can be interpreted to mean "upon determining" or "in response to determining" or "upon detecting [the stated condition or event]" or "in response to detecting [the stated condition or event]."

[0039] Various structural diagrams according to the disclosed embodiments of the present application are shown in the drawings. These diagrams are not drawn to scale, in which certain details are exaggerated for clarity and others omitted. The shapes and relative sizes of the various regions, layers, and the relative positions of these regions / layers shown in the drawings are merely examples and may, in actuality, deviate from what is shown in the drawings due to manufacturing tolerances or technical limitations, and regions / layers with different shapes, sizes, and relative positions can be additionally designed according to actual needs by those skilled in the art.

[0040] Embodiment 1, the present application provides a multi-sensor multi-modal tight coupling fusion mapping and positioning method, comprising:

[0041] The raw data obtained by each sensor is preprocessed;

[0042] The preprocessed data of different sensors are associated and fused to obtain fused data;

[0043] Based on the fused data, the position and attitude of the vehicle state are estimated and optimized;

[0044] According to the estimated and optimized vehicle state and the fused data, the positioning is output, and the environment map is constructed.

[0045] The present application comprehensively utilizes various sensor data such as control odometer, IMU odometer, forward-looking, rear-view, left-front, right-front camera sensors, and solid-state laser radar sensors, as well as advanced data association, state estimation and optimization algorithms, to realize high-precision, real-time and robust positioning and mapping in complex environments.

[0046] Embodiment 2, the present application provides a multi-sensor multi-modal tight coupling fusion mapping and positioning method, specifically comprising:

[0047] First, the raw data obtained by each sensor is preprocessed and feature extraction is performed, then a data association algorithm is used to associate and fuse the data of different sensors to obtain comprehensive information of the environment. Then, based on the fused data, a state estimation and key frame matching algorithm is used to estimate and optimize the vehicle state, including position, attitude, etc. Subsequently, according to the estimated state and the fused data, the position is obtained, and the environment map is constructed.

[0048] (1) Real-time sensor data acquisition: Obtain steering data (EPS), wheel speed data (ODO) (including gear and brake conditions), inertial measurement unit (IMU), camera data (front, left front, right front, rear), and lidar data (Lidar) from the whole vehicle.

[0049] (2) Sensor data processing: EPS and ODO data are processed through filtering, noise reduction, and pre-integration to obtain control prediction odometry; IMU data are processed through filtering, noise reduction, and integration to obtain IMU odometry; Camera data are processed through calibration to obtain images with distortion removed; Lidar data and IMU data are combined to obtain data after motion distortion; All data processing is completed. Sensors are calibrated to remove distortion and eliminate their own errors; Kalman filter is used to filter the random errors of IMU;

[0050] (3) Odometry fusion: IMU integrated odometry and control integrated odometry are fused to obtain fused odometry (to prevent large errors in IMU random errors and control integrated odometry); IMU integration and control integration odometry are fused using the weight method, and the weight needs to be calibrated and tested to determine;

[0051] (4) Fusion of obstacle recognition: image recognition of traffic participants and laser radar recognition of traffic participants are mutually accumulated, and then are removed separately (to prevent matching errors caused by traffic participant motion); image recognition of obstacles uses deep learning method to recognize pedestrians, vehicles and other traffic participants; point cloud is removed from the ground by traditional algorithm, and the relative speed of the vehicle is used to determine the traffic participants; fusion is to remove all obstacles recognized by image and point cloud;

[0052] K-means is a commonly used distance-based clustering algorithm that divides data points into a pre-specified number of clusters. The following are the basic steps and formulas of the K-means algorithm:

[0053] Initialization: Randomly select K data points as initial cluster centers.

[0054] Cluster assignment: For each data point, calculate its distance to K cluster centers and assign it to the cluster corresponding to the nearest cluster center.

[0055] Update cluster centers: For each cluster, recalculate the mean of all its data points and take the mean as the new cluster center.

[0056] Iteration: Until the cluster centers no longer change or the maximum number of iterations is reached.

[0057] The K-means algorithm usually takes the convergence of the clustering result or the maximum number of iterations as the termination condition. It should be noted that the K-means algorithm is sensitive to the selection of the initial clustering center, so multiple random initializations are usually performed, and the optimal clustering result is selected.

[0058] The K-means algorithm is a simple and efficient clustering algorithm, but it has certain assumptions for the number of data points, the shape of the cluster, and the density distribution, so it needs to be carefully selected when applied. Parameters and appropriate preprocessing.

[0059] (5) Feature point extraction: remove the picture of the obstacle, and perform feature point extraction on the picture, mainly extract lane lines, pedestrian crossings, parking space information, traffic signs, buildings and other stable and good recognition obstacles; after removing the obstacles from the laser radar point cloud, perform feature point extraction on the point cloud, mainly extract lane line information, parking space information, traffic signs, building lines, inflection points, and surface extraction; the image uses a deep learning method to obtain the obstacle; the laser radar uses a traditional method to remove the obstacle;

[0060] (6) Feature point matching: fuse the respective corresponding matching of the odometer, picture and point cloud feature points, obtain the visual odometer and laser odometer, and obtain the current position of the vehicle; fuse the respective corresponding matching of the odometer, picture and point cloud feature points, obtain the visual odometer and laser odometer, fuse the pose estimation results obtained by the visual odometer and the laser odometer respectively through a Kalman filter, accumulate the fused pose estimation results, and obtain the position of the vehicle in the global coordinate system.

[0061] (7) Mapping: on the basis of the previous frame, increase the image information or point cloud information of the feature points identified in the current frame. Select a map type, use the SLAM algorithm of simultaneous localization and mapping based on the fused sensor data to construct an environment map, use a graph optimization or nonlinear optimization algorithm to minimize the re-projection error, and correct the cumulative map error through loop detection.

[0062] In another embodiment of the present application, a multi-sensor multi-modal tight coupling fusion mapping and positioning system is provided, which can be used to implement the multi-sensor multi-modal tight coupling fusion mapping and positioning method described above. Specifically, the system comprises:

[0063] The data acquisition module is used for preprocessing the raw data obtained by each sensor;

[0064] The data fusion module is used for associating and fusing the preprocessed data of different sensors to obtain fused data;

[0065] The optimization module is used for estimating and optimizing the position and attitude of the vehicle state based on the fused data;

[0066] An output module is configured to output the positioning and construct the environment map according to the estimated and optimized vehicle state and the fused data.

[0067] The division of the modules in the embodiments of the present application is illustrative, and is only a logical function division. In actual implementation, another division manner can be used. In addition, each function module in each embodiment of the present application can be integrated in one processor, or can be physically separated, or two or more modules can be integrated in one module. The integrated module can be realized in the form of hardware or in the form of a software function module.

[0068] In another embodiment of the present application, a computer device is provided, which includes a processor and a memory. The memory is configured to store a computer program, and the computer program includes program instructions. The processor is configured to execute the program instructions stored in the computer storage medium. The processor can be a central processing unit (CPU), and can also be other general-purpose processors, digital signal processors (DSP), application specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components, etc. The processor is the computing core and control core of the terminal, and is suitable for implementing one or more instructions, and is specifically suitable for loading and executing one or more instructions in the computer storage medium to implement a corresponding method flow or a corresponding function. The processor in the embodiments of the present application can be used for the operation of the multi-sensor multi-modal close-coupling fusion mapping and positioning method.

[0069] In still another embodiment of the present application, the present application also provides a storage medium, specifically a computer readable storage medium (Memory), which is a memory device in a computer device, used for storing programs and data. It can be understood that the computer readable storage medium herein can include the built-in storage medium in the computer device, and of course can also include the expansion storage medium supported by the computer device. The computer readable storage medium provides a storage space, which stores the operating system of the terminal. Moreover, one or more instructions suitable for being loaded and executed by the processor are also stored in the storage space, and these instructions can be one or more computer programs (including program codes). It should be noted that the computer readable storage medium herein can be a high-speed RAM memory, or a non-volatile memory such as at least one disk memory. The one or more instructions stored in the computer readable storage medium can be loaded and executed by the processor to realize the corresponding steps of the multi-sensor multi-modal tight coupling fusion mapping and positioning method in the above embodiment.

[0070] Those skilled in the art should understand that the embodiments of the present application can be provided as a method, a system, or a computer program product. Therefore, the present application can take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present application can take the form of a computer program product implemented on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROMs, optical storage, etc.) containing computer-usable program code.

[0071] The present application is described with reference to the flowcharts and / or block diagrams of the method, device (system), and computer program product according to the embodiments of the present application. It should be understood that each flow and / or block in the flowcharts and / or block diagrams, and the combination of the flows and / or blocks in the flowcharts and / or block diagrams can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device produce a device implemented in the flowcharts and / or block diagrams. Figure 1 The function specified in one or more flows and / or blocks Figure 1 The means for implementing the function specified in one or more flows and / or blocks.

[0072] These computer program instructions can also be stored in a computer readable storage medium capable of directing the computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer readable storage medium produce a manufactured product including instruction means, which implements the flowcharts and / or block diagrams. Figure 1 The function specified in one or more flows and / or blocksFigure 1 the function specified in the one or more blocks.

[0073] These computer program instructions can also be loaded into computer or other programmable data processing devices, so that a series of operation steps are performed on the computer or other programmable data processing devices to generate computer-implemented processes, thus the instructions executed on the computer or other programmable data processing devices provide processes for implementing the flows Figure 1 the flows or the flows and / or blocks Figure 1 the steps of the function specified in the one or more blocks.

[0074] Finally, it should be noted that: the above examples are only used to illustrate the technical solutions of the present application, but not to limit it, although the above embodiments of the present application have been described in detail, those skilled in the art should understand: the specific embodiments of the present application can be modified or replaced by the same, without departing from the spirit and scope of the present application, any modification or equivalent replacement, which should be covered in the protection scope of the claims of the present application.

Claims

1. A multi-sensor multi-modal tight-coupling fusion mapping and localization method, characterized in that, The application comprises the following steps: Preprocessing the raw data obtained by each sensor; Correlating and fusing the preprocessed data of different sensors to obtain fused data; Estimating and optimizing the position and attitude of the vehicle state based on the fused data; Outputting positioning and constructing an environment map according to the estimated and optimized vehicle state and the fused data; The raw data obtained by each sensor comprises steering data, wheel speed data, inertial measurement unit data, camera data and laser radar data obtained from the whole vehicle; The raw data is preprocessed and feature extracted, including: The steering data and wheel speed data are filtered, denoised and pre-integrated to obtain a control prediction odometer; the inertial measurement unit (IMU) data is filtered, denoised and integrated to obtain an IMU odometer; the camera data is calibrated to obtain images without distortion; the laser radar data and the inertial measurement unit (IMU) data are combined to obtain data with motion distortion; the sensors are calibrated to eliminate distortion and error; the inertial measurement unit random error is filtered using Kalman filtering; The preprocessed data of different sensors is correlated and fused, including: The control prediction odometer and the IMU odometer are fused to obtain a fused odometer; the control prediction odometer and the IMU odometer are fused using a weight method, and the weight is determined through calibration test; The image-recognized traffic participants and the laser radar-recognized traffic participants are mutually accumulated and then removed respectively; the image-recognized obstacles are recognized using a deep learning method; the point cloud is removed from the ground through a traditional algorithm, and the relative speed of the vehicle is used to determine the traffic participants; the fusion refers to removing all obstacles recognized by the image and the point cloud; The positioning is output according to the estimated and optimized vehicle state and the fused data, including: The fused odometer, the image and the point cloud feature points are respectively matched to obtain a visual odometer and a laser odometer; the estimated results of the visual odometer and the laser odometer are fused through a Kalman filter; and the fused estimated results are accumulated to obtain the position of the vehicle in a global coordinate system.

2. The multi-sensor multi-modal tightly coupled fusion mapping and localization method according to claim 1, characterized in that, The position and attitude of the vehicle state are estimated and optimized based on the fused data, including: The image of the obstacle is removed, and the feature points of the image are extracted to extract lane lines, pedestrian crossings, parking space information, traffic signs and buildings; after the laser radar point cloud is removed from the obstacle, the feature points of the point cloud are extracted to extract lane line information, parking space information, traffic signs, building lines, inflection points and surface extraction; the image is used to obtain the obstacle using a deep learning method.

3. The multi-sensor multi-modal tightly coupled fusion mapping and localization method according to claim 1, characterized in that, The environment map is constructed, including: Selecting a map type, constructing an environment map using a simultaneous localization and mapping (SLAM) algorithm based on the fused sensor data, minimizing the re-projection error using graph optimization or nonlinear optimization algorithm, and correcting the accumulated map error through loop detection.

4. A multi-sensor multi-modal tight-coupled fusion mapping and localization system, characterized in that, The application comprises the following steps: A data acquisition module is configured to preprocess the raw data obtained by each sensor. A data fusion module is configured to associate and fuse the preprocessed data of different sensors to obtain fused data. An optimization module is configured to estimate and optimize the position and attitude of the vehicle based on the fused data. An output module is configured to output positioning and construct an environment map based on the estimated and optimized vehicle state and the fused data. Raw data obtained by the sensors include steering data, wheel speed data, inertial measurement unit data, camera data, and laser radar data obtained from the whole vehicle. The raw data is preprocessed and feature-extracted, including: The steering data and wheel speed data are filtered, denoised, and pre-integrated to obtain a control prediction odometry; the inertial measurement unit (IMU) data is filtered, denoised, and integrated to obtain an IMU odometry; the camera data is calibrated to obtain images with distortion removed; the laser radar data and the inertial measurement unit (IMU) data are combined to obtain data with motion distortion; the sensors are calibrated to remove distortion and eliminate inherent errors; and the inertial measurement unit random error is filtered using a Kalman filter. The preprocessed data of different sensors is associated and fused, including: The control prediction odometry and the IMU odometry are fused to obtain a fused odometry; the control prediction odometry and the IMU odometry are fused using a weight method, and the weight is determined through calibration testing; The image-recognized traffic participants and the laser radar-recognized traffic participants are mutually accumulated and then removed separately; the image-recognized obstacles are recognized using a deep learning method; the point cloud is removed from the ground using a traditional algorithm, and the relative speed of the vehicle is used to determine the traffic participants; and the fusion refers to removing all obstacles recognized by the image and the point cloud. The estimated and optimized vehicle state and the fused data are used to output positioning, including: The fused odometry and the image and point cloud feature points are matched separately to obtain a visual odometry and a laser odometry, the estimated results of the poses obtained by the visual odometry and the laser odometry are fused through a Kalman filter, the fused estimated results of the poses are accumulated to obtain the position of the vehicle in a global coordinate system.

5. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, The processor executes the computer program to implement the steps of the multi-sensor multi-modal tight coupling fusion mapping positioning method according to any one of claims 1 to 3.

6. A computer-readable storage medium storing a computer program, the computer program comprising instructions that, when executed by a computer, cause the computer to perform the method of any one of claims 1 to 5. The computer program is executed by the processor to implement the steps of the multi-sensor multi-modal tight coupling fusion mapping positioning method according to any one of claims 1 to 3.

Images