A ground robot repositioning method and device based on multi-sensor fusion
Through the multi-sensor fusion method, combined with visual camera, lidar and IMU data, the BoW word bag model and ESKF algorithm are used to generate local maps, solving the problems of low positioning accuracy and large calculation volume of lidar, achieving high-precision and real-time Robot positioning.
Patent Information
- Application Number
- CN202411289966.1
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2024-09-14
- Publication Date
- 2024-12-03
- Estimated Expiration
- 2044-09-14
AI Technical Summary
The existing high-precision map positioning method based on lidar is difficult to complete autonomous global positioning without obtaining prior position attitude data, poor robustness, susceptible to sensor errors, and large calculations lead to excessive load on the system and inability to live. run.
Using a multi-sensor fusion method, combining visual camera data, lidar data and IMU data, the position and attitude estimates of the computer robot are generated through visual relocation, laser relocation and ESKF algorithms, and the BoW word bag model and GICP registration. Local maps to improve positioning accuracy and robustness.
It effectively improves positioning accuracy and robustness, reduces the amount of map matching calculation, realizes real-time output positioning results, alleviates the problems of inability to reposition or low repositioning accuracy in traditional methods.
Smart Images

Figure CN118864602B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of map positioning technology, and in particular to a ground robot repositioning method and device based on multi-sensor fusion. Background Art
[0002] With the development of SLAM (Simultaneous Localization and Mapping) technology, mapping and positioning by integrating multiple sensors such as LiDAR, IMU, and GPS is an important basis for the automatic navigation of intelligent unmanned vehicles. Figure 1 Generally, it is a road-level map, which contains less map information and has an accuracy of meters. However, positioning based on high-precision maps can be accurate to 10-20cm, which can provide a more comprehensive data foundation for autonomous driving.
[0003] The existing high-precision map positioning method based on LiDAR is difficult to complete autonomous global positioning without obtaining prior position and posture data due to the sparsity of laser point clouds. It has poor robustness and is easily affected by sensor errors. It is easy to fail to reposition or the repositioning results vary greatly. Usually, the high-precision map positioning method based on LiDAR uses a device equipped with a 3D LiDAR to collect data, and then combines the collected data with the high-precision map. The ICP algorithm (Iterative Closest Point) or NDT (Normal Distributions Transform) algorithm is usually used for calculation to obtain the robot's position and posture. Since the positioning result is obtained by map matching calculation, the high-precision map positioning method based on LiDAR is not suitable for robot positioning. Figure 1 Generally, the area is large, and it is inevitable that there will be a large amount of calculation required for retrieval and many similar scenes, which will lead to the inability to locate, positioning errors, or excessive system load causing calculation jams and inability to run in real time. In order to avoid the situation where the system is overloaded, the map matching rate is usually reduced, and the odometer is combined to output high-frequency positioning results. For example, the EKF algorithm (Extended Kalman Filter) is used to fuse lidar data and IMU sensor data to output a high-frequency positioning odometer. Although this method has a fast calculation speed, the EKF algorithm itself may have the problem that the optimized state quantity is too far away from the working point, resulting in insufficient linear approximation. Summary of the invention
[0004] The purpose of the present invention is to overcome the deficiencies in the prior art and provide a ground robot repositioning method and device based on multi-sensor fusion to solve various technical problems existing in the background technology of repositioning operation using a single sensor.
[0005] To achieve the above object, the present invention is implemented by adopting the following technical solutions:
[0006] In a first aspect, the present invention provides a ground robot relocalization method based on multi-sensor fusion, comprising:
[0007] Obtain visual camera data, lidar data, and IMU data;
[0008] Visual relocalization and laser relocalization are performed based on visual camera data and lidar data respectively;
[0009] If the visual relocation and / or laser relocation is successful, a first position and posture estimation value and / or a second position and posture estimation value of the ground robot is calculated based on the visual camera data and / or the laser radar data;
[0010] Based on the lidar data and IMU data, the ESKF algorithm is used to calculate the third position attitude estimate of the ground robot;
[0011] Obtain the estimated position and posture value of the ground robot at the previous moment, obtain the laser point cloud data of M adjacent moments from the pre-generated high-precision map data, and stitch them together to generate a local map;
[0012] Calculate a fourth position pose estimate of the ground robot based on the laser radar data and the local map;
[0013] Based on the first position and attitude estimation value, the second position and attitude estimation value, the third position and attitude estimation value, and the fourth position and attitude estimation value, a final position and attitude value is obtained by fusion.
[0014] Optionally, the acquiring of visual camera data, lidar data and IMU data further includes time aligning the visual camera data, lidar data and IMU data according to their timestamps.
[0015] Optionally, if the visual relocalization is successful and the laser relocalization is unsuccessful, calculating a first position pose estimate of the ground robot based on the visual camera data;
[0016] If visual relocalization is unsuccessful and laser relocalization is successful, calculating a second position pose estimate of the ground robot based on the lidar data;
[0017] If the visual relocalization is successful and the laser relocalization is successful, a first position and posture estimation value and a second position and posture estimation value of the ground robot are calculated according to the visual camera data and the laser radar data respectively;
[0018] If visual relocalization is unsuccessful and laser relocalization is unsuccessful, repeat the following steps until visual relocalization and / or laser relocalization is successful:
[0019] Laser relocalization and visual relocalization are performed based on the visual camera data and lidar data at the next moment respectively.
[0020] Optionally, the visual relocation includes:
[0021] Extract target feature points from visual camera data at the target moment, and cluster the target feature points into different categories of words;
[0022] Combine the word type and the number of occurrences into a Bow descriptor;
[0023] Based on the Bow descriptor, traverse the pre-built BoW descriptor library, find all visual image data with the same word type, and calculate the BoW similarity score;
[0024] Perform GICP registration on the laser point cloud data corresponding to the visual image data with the highest BoW similarity score and the laser radar data at the target time;
[0025] If the registration score meets the preset visual registration score threshold, the visual relocalization is successful;
[0026] Calculate a first position and attitude estimation value based on the laser point cloud data and laser radar data corresponding to successful visual relocalization;
[0027] The laser repositioning comprises:
[0028] Calculate the SC descriptor based on the laser radar data at the target time;
[0029] Based on the SC descriptor traversal and construction, the SC descriptor library is constructed to find the most similar N laser point cloud data and calculate the SC similarity scores respectively;
[0030] Perform GICP registration on the laser point cloud data with the highest SC similarity score and the laser radar data at the target time;
[0031] If the registration score meets the preset laser registration score threshold, the laser relocalization is successful;
[0032] Calculate a second position attitude estimate value based on the laser point cloud data and laser radar data corresponding to the successful laser repositioning;
[0033] The construction process of the SC descriptor library and the BoW descriptor library includes:
[0034] Read the pre-generated high-precision map data, calculate the BoW descriptor for the visual image data at each moment and save the descriptor to the BoW descriptor library; calculate the SC descriptor for the laser point cloud data at each moment and save the descriptor to the SC descriptor library.
[0035] Optionally, the calculation process of the third position and posture estimation value includes:
[0036] Construct the estimated state of the ground robot at time k :
[0037] ;
[0038] In the formula, are the position, velocity, attitude, accelerometer bias, gyroscope bias and gravity acceleration of the ground robot at time k;
[0039] Estimated state quantity Derivative with respect to time, obtain the discrete error recursion formula of IMU data, and form the Jacobian matrix in the prediction process of the ESKF algorithm based on the discrete error recursion formula and , and construct the prediction equation:
[0040] ;
[0041] ;
[0042] In the formula, is the predicted state error at time k relative to time k-1, is the predicted state error The corresponding prediction covariance matrix, is the prediction covariance matrix Correction value of
[0043] The IMU data between time k-1 and time k are brought into the prediction equation for recursion to obtain the predicted state error. and the prediction covariance matrix ;
[0044] Construct the observation equation for the prediction equation:
[0045] ;
[0046] In the formula, is the observed data, is the observation noise, The observation noise The covariance matrix of is an abstract function;
[0047] Further calculation of Kalman gain :
[0048] ;
[0049] ;
[0050] In the formula, is an intermediate variable;
[0051] Based on Kalman gain Calculate the predicted state error and the prediction covariance matrix Correction value :
[0052] ;
[0053] ;
[0054] Based on the corrected value Correction of estimated state , and from the corrected estimated state quantity Extract location from and posture as a third estimate.
[0055] Optionally, the step of generating a local map by stitching further comprises:
[0056] The local map is downsampled and filtered by SOR to obtain a smaller local map.
[0057] Optionally, the calculating a fourth position and posture estimation value of the ground robot based on the laser radar data and the local map includes:
[0058] Perform GICP registration of the LiDAR data with the laser point cloud data at each moment in the local map;
[0059] When the obtained registration score satisfies a preset map registration score threshold, a fourth position attitude estimation value is calculated according to the number of laser point clouds and the laser radar data at the corresponding moment.
[0060] Optionally, the fusion to obtain the final value of position and posture includes:
[0061] Get the system relocation flag;
[0062] If the system relocation flag is FALSE, retrieve the first position and attitude estimate and the second position and attitude estimate. If either estimate is not empty, use the non-empty estimate as the final position and attitude value, and set the system relocation flag to True. If both estimates are empty, the system relocation flag remains FALSE.
[0063] If the system repositioning flag is True, the difference between the third position and attitude estimation value and the fourth position and attitude estimation value is calculated. If the difference value exceeds the preset difference threshold, the system repositioning flag is set to FALSE; if the difference value does not exceed the preset difference threshold, the third position and attitude estimation value and the fourth position and attitude estimation value are fused through the G2O library to perform joint factor graph optimization to obtain the final position and attitude value.
[0064] In a second aspect, the present invention provides a ground robot repositioning device based on multi-sensor fusion, comprising:
[0065] Data acquisition module, used to acquire visual camera data, lidar data and IMU data;
[0066] A relocalization module for performing visual relocalization and laser relocalization based on visual camera data and lidar data, respectively;
[0067] A first second estimation value calculation module, configured to calculate a first position and posture estimation value and / or a second position and posture estimation value of the ground robot according to the visual camera data and / or the laser radar data if the visual relocation and / or the laser relocation is successful;
[0068] A third estimated value calculation module, used to calculate a third position and posture estimated value of the ground robot based on the laser radar data and the IMU data through an ESKF algorithm;
[0069] A fourth estimated value calculation module is used to obtain the position and attitude estimation value of the ground robot at the previous moment, obtain the laser point cloud data of M adjacent moments from the pre-generated high-precision map data, and stitch them to generate a local map; calculate the fourth position and attitude estimation value of the ground robot based on the laser radar data and the local map;
[0070] The estimated value fusion module is used to fuse and obtain a final position and attitude value based on the first position and attitude estimated value, the second position and attitude estimated value, the third position and attitude estimated value, and the fourth position and attitude estimated value.
[0071] Optionally, the fusion to obtain the final value of position and posture includes:
[0072] Get the system relocation flag;
[0073] If the system relocation flag is FALSE, retrieve the first position and attitude estimate and the second position and attitude estimate. If either estimate is not empty, use the non-empty estimate as the final position and attitude value, and set the system relocation flag to True. If both estimates are empty, the system relocation flag remains FALSE.
[0074] If the system repositioning flag is True, the difference between the third position and attitude estimation value and the fourth position and attitude estimation value is calculated. If the difference value exceeds the preset difference threshold, the system repositioning flag is set to FALSE; if the difference value does not exceed the preset difference threshold, the third position and attitude estimation value and the fourth position and attitude estimation value are fused through the G2O library to perform joint factor graph optimization to obtain the final position and attitude value.
[0075] Compared with the prior art, the present invention has the following beneficial effects:
[0076] The present invention provides a ground robot repositioning method and device based on multi-sensor fusion. The present invention combines visual camera data and the BoW bag-of-words model algorithm, and uses the rich visual information to effectively solve the problem of low repositioning accuracy or inability to retrieve repositioning with a single laser radar. The ESKF algorithm is used to output real-time odometer results, and the optimized state error is always near zero, so there will be no approximation problem, and the amount of map matching calculations is reduced, which greatly improves the frequency of positioning data output, facilitating the subsequent planning and control functions. In summary, the present invention can effectively alleviate the problem that traditional laser radar-based positioning methods cannot be repositioned or have low repositioning accuracy under large maps, and can output positioning results in real time with extremely high robustness. BRIEF DESCRIPTION OF THE DRAWINGS
[0077] Figure 1 is a method flow chart of a ground robot repositioning method based on multi-sensor fusion provided by an embodiment of the present invention;
[0078] Figure 2 It is a structural schematic diagram of a ground robot repositioning device based on multi-sensor fusion provided in an embodiment of the present invention. DETAILED DESCRIPTION
[0079] The present invention will be further described below in conjunction with the accompanying drawings. The following embodiments are only used to more clearly illustrate the technical solution of the present invention, and cannot be used to limit the protection scope of the present invention.
[0080] Embodiment 1:
[0081] like Figure 1 As shown, an embodiment of the present invention provides a ground robot repositioning method based on multi-sensor fusion, comprising the following steps:
[0082] Step S1, obtaining visual camera data, lidar data and IMU data.
[0083] IMU data is data generated by the Inertial Measurement Unit; obtaining visual camera data, LiDAR data, and IMU data also includes time alignment of visual camera data, LiDAR data, and IMU data according to their timestamps. Time alignment facilitates the subsequent unified processing of visual camera data, LiDAR data, and IMU data.
[0084] Step S2: perform visual repositioning and laser repositioning based on visual camera data and lidar data respectively.
[0085] In the actual implementation process, visual repositioning and laser repositioning can be carried out simultaneously or one after the other as needed.
[0086] Step S2.1, visual relocalization includes:
[0087] Step S2.1.1, extracting target feature points from the visual camera data at the target moment, and clustering the target feature points into different types of words;
[0088] Step S2.1.2, combining the type and the number of occurrences of the word into a Bow descriptor;
[0089] Step S2.1.3, based on the Bow descriptor, traverse the pre-built BoW descriptor library, find all visual image data with the same word type, and calculate the BoW similarity score;
[0090] Step S2.1.4, perform GICP registration on the laser point cloud data corresponding to the visual image data with the highest BoW similarity score and the laser radar data at the target time;
[0091] Step S2.1.5: If the registration score meets the preset visual registration score threshold, the visual relocalization is successful;
[0092] Step S2.1.6, calculate the first position attitude estimation value based on the laser point cloud data and laser radar data corresponding to the successful visual relocalization.
[0093] Step S2.2, laser repositioning includes:
[0094] Step S2.2.1, calculating the SC descriptor according to the laser radar data at the target time;
[0095] Step S2.2.2, based on the SC descriptor traversal constructed by the SC descriptor library, find the most similar N laser point cloud data and calculate the SC similarity score respectively;
[0096] Step S2.2.3, perform GICP registration on the laser point cloud data with the highest SC similarity score and the laser radar data at the target time;
[0097] Step S2.2.4: If the registration score meets the preset laser registration score threshold, the laser repositioning is successful;
[0098] Step S2.2.5, calculating a second position attitude estimation value according to the laser point cloud data and laser radar data corresponding to the successful laser repositioning;
[0099] The construction process of the SC descriptor library and the BoW descriptor library includes:
[0100] Read the pre-generated high-precision map data, calculate the BoW descriptor for the visual image data at each moment and save the descriptor to the BoW descriptor library; calculate the SC descriptor for the laser point cloud data at each moment and save the descriptor to the SC descriptor library.
[0101] Step S3: If the visual relocation and / or laser relocation is successful, a first position and posture estimation value and / or a second position and posture estimation value of the ground robot is calculated according to the visual camera data and / or the laser radar data;
[0102] If the visual relocalization is successful and the laser relocalization is unsuccessful, a first position pose estimate of the ground robot is calculated based on the visual camera data;
[0103] If visual relocalization is unsuccessful and laser relocalization is successful, calculating a second position pose estimate of the ground robot based on the lidar data;
[0104] If the visual relocalization is successful and the laser relocalization is successful, a first position and posture estimation value and a second position and posture estimation value of the ground robot are calculated according to the visual camera data and the laser radar data respectively;
[0105] If visual relocalization is unsuccessful and laser relocalization is unsuccessful, repeat the following steps until visual relocalization and / or laser relocalization is successful:
[0106] Laser relocalization and visual relocalization are performed based on the visual camera data and lidar data at the next moment respectively.
[0107] Step S4: Calculate a third position and posture estimation value of the ground robot based on the laser radar data and the IMU data through the ESKF algorithm.
[0108] The calculation process of the third position and attitude estimation value includes:
[0109] Step S4.1: Construct the estimated state of the ground robot at time k :
[0110] ;
[0111] In the formula, are the position, velocity, attitude, accelerometer bias, gyroscope bias and gravity acceleration of the ground robot at time k;
[0112] Step S4.2: Estimated state quantity Derivative with respect to time, obtain the discrete error recursion formula of IMU data, and form the Jacobian matrix in the prediction process of the ESKF algorithm based on the discrete error recursion formula and , and construct the prediction equation:
[0113] ;
[0114] ;
[0115] In the formula, is the predicted state error at time k relative to time k-1, is the predicted state error The corresponding prediction covariance matrix, is the prediction covariance matrix Correction value of
[0116] Step S4.3: Substitute the IMU data between time k-1 and time k into the prediction equation for recursion to obtain the predicted state error. and the prediction covariance matrix ;
[0117] Step S4.4, construct the observation equation of the prediction equation:
[0118] ;
[0119] In the formula, is the observed data, is the observation noise, The observation noise The covariance matrix of is an abstract function;
[0120] Step S4.5: Further calculate the Kalman gain :
[0121] ;
[0122] ;
[0123] In the formula, is an intermediate variable;
[0124] Step S4.6: Based on Kalman gain Calculate the predicted state error and the prediction covariance matrix Correction value :
[0125] ;
[0126] ;
[0127] Step S4.7: Based on the correction value Correction of estimated state , and from the corrected estimated state quantity Extract location from and posture as a third estimate.
[0128] Step S5: Obtain the estimated position and posture value of the ground robot at the previous moment, obtain the laser point cloud data of M adjacent moments from the pre-generated high-precision map data, and stitch them to generate a local map.
[0129] The stitching to generate a local map also includes:
[0130] The local map is downsampled and filtered by SOR to obtain a smaller local map.
[0131] Step S6: Calculate a fourth position and posture estimate of the ground robot based on the laser radar data and the local map.
[0132] Perform GICP registration of the LiDAR data with the laser point cloud data at each moment in the local map;
[0133] When the obtained registration score satisfies a preset map registration score threshold, a fourth position attitude estimation value is calculated according to the number of laser point clouds and the laser radar data at the corresponding moment.
[0134] Step S7: Based on the first position and attitude estimation value, the second position and attitude estimation value, the third position and attitude estimation value, and the fourth position and attitude estimation value, a final position and attitude value is obtained by fusing.
[0135] The final value of the position and attitude obtained by fusion includes:
[0136] Get the system relocation flag;
[0137] If the system relocation flag is FALSE, retrieve the first position and attitude estimate and the second position and attitude estimate. If either estimate is not empty, use the non-empty estimate as the final position and attitude value, and set the system relocation flag to True. If both estimates are empty, the system relocation flag remains FALSE.
[0138] If the system repositioning flag is True, the difference between the third position and attitude estimation value and the fourth position and attitude estimation value is calculated. If the difference value exceeds the preset difference threshold, the system repositioning flag is set to FALSE; if the difference value does not exceed the preset difference threshold, the third position and attitude estimation value and the fourth position and attitude estimation value are fused through the G2O library to perform joint factor graph optimization to obtain the final position and attitude value.
[0139] Embodiment 2:
[0140] like Figure 2 As shown, the present invention provides a ground robot repositioning device based on multi-sensor fusion, comprising:
[0141] Data acquisition module, used to acquire visual camera data, lidar data and IMU data;
[0142] A relocalization module for performing visual relocalization and laser relocalization based on visual camera data and lidar data, respectively;
[0143] A first second estimation value calculation module, configured to calculate a first position and posture estimation value and / or a second position and posture estimation value of the ground robot according to the visual camera data and / or the laser radar data if the visual relocation and / or the laser relocation is successful;
[0144] A third estimated value calculation module, used to calculate a third position and posture estimated value of the ground robot based on the laser radar data and the IMU data through an ESKF algorithm;
[0145] A fourth estimated value calculation module is used to obtain the position and attitude estimation value of the ground robot at the previous moment, obtain the laser point cloud data of M adjacent moments from the pre-generated high-precision map data, and stitch them to generate a local map; calculate the fourth position and attitude estimation value of the ground robot based on the laser radar data and the local map;
[0146] The estimated value fusion module is used to fuse and obtain a final position and attitude value based on the first position and attitude estimated value, the second position and attitude estimated value, the third position and attitude estimated value, and the fourth position and attitude estimated value.
[0147] Specifically, the final value of the position and posture obtained by fusion includes:
[0148] Get the system relocation flag;
[0149] If the system relocation flag is FALSE, retrieve the first position and attitude estimate and the second position and attitude estimate. If either estimate is not empty, use the non-empty estimate as the final position and attitude value, and set the system relocation flag to True. If both estimates are empty, the system relocation flag remains FALSE.
[0150] If the system repositioning flag is True, the difference between the third position and attitude estimation value and the fourth position and attitude estimation value is calculated. If the difference value exceeds the preset difference threshold, the system repositioning flag is set to FALSE; if the difference value does not exceed the preset difference threshold, the third position and attitude estimation value and the fourth position and attitude estimation value are fused through the G2O library to perform joint factor graph optimization to obtain the final position and attitude value.
[0151] It will be appreciated by those skilled in the art that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Furthermore, the present invention may 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-ROM, optical storage, etc.) containing computer-usable program code.
[0152] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, 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 generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.
[0153] These computer program instructions may also be stored in a computer readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture including an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.
[0154] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process in the computer or other programmable device. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.
[0155] The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the technical principles of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.
Claims
1. A ground robot relocalization method based on multi-sensor fusion, characterized in that: include: Obtain visual camera data, lidar data, and IMU data; Visual relocalization and laser relocalization are performed based on visual camera data and lidar data respectively; If the visual relocation and / or laser relocation is successful, a first position and posture estimation value and / or a second position and posture estimation value of the ground robot is calculated based on the visual camera data and / or the laser radar data; Based on the lidar data and IMU data, the ESKF algorithm is used to calculate the third position attitude estimate of the ground robot; Obtain the estimated position and posture value of the ground robot at the previous moment, obtain the laser point cloud data of M adjacent moments from the pre-generated high-precision map data, and stitch them together to generate a local map; Calculate a fourth position pose estimate of the ground robot based on the laser radar data and the local map; Based on the first position and attitude estimation value, the second position and attitude estimation value, the third position and attitude estimation value, and the fourth position and attitude estimation value, a final position and attitude value is obtained by fusing; The calculation process of the third position and posture estimation value includes: Construct the estimated state of the ground robot at time k : ; In the formula, are the position, velocity, attitude, accelerometer bias, gyroscope bias and gravity acceleration of the ground robot at time k; Estimated state quantity Derivative with respect to time, obtain the discrete error recursion formula of IMU data, and form the Jacobian matrix in the prediction process of the ESKF algorithm based on the discrete error recursion formula and , and construct the prediction equation: ; ; In the formula, is the predicted state error at time k relative to time k-1, is the predicted state error The corresponding prediction covariance matrix, is the prediction covariance matrix Correction value of is the predicted state error at time k-2 relative to time k-1 Correction value of The IMU data between time k-1 and time k are brought into the prediction equation for recursion to obtain the predicted state error. and the prediction covariance matrix ; Construct the observation equation for the prediction equation: ; In the formula, is the observed data, is the observation noise, The observation noise The covariance matrix of is an abstract function; Further calculation of Kalman gain : ; ; In the formula, is an intermediate variable; Based on Kalman gain Calculate the predicted state error and the prediction covariance matrix Correction value : ; ; Based on the corrected value Correction of estimated state , and from the corrected estimated state quantity Extract location from and posture as a third estimate.
2. The ground robot repositioning method based on multi-sensor fusion according to claim 1 is characterized in that: The obtaining of visual camera data, lidar data and IMU data also includes time aligning the visual camera data, lidar data and IMU data according to their timestamps.
3. The ground robot repositioning method based on multi-sensor fusion according to claim 1, characterized in that: If the visual relocalization is successful and the laser relocalization is unsuccessful, a first position pose estimate of the ground robot is calculated based on the visual camera data; If visual relocalization is unsuccessful and laser relocalization is successful, calculating a second position pose estimate of the ground robot based on the lidar data; If the visual relocalization is successful and the laser relocalization is successful, a first position and posture estimation value and a second position and posture estimation value of the ground robot are calculated according to the visual camera data and the laser radar data respectively; If visual relocalization is unsuccessful and laser relocalization is unsuccessful, repeat the following steps until visual relocalization and / or laser relocalization is successful: Laser relocalization and visual relocalization are performed based on the visual camera data and lidar data at the next moment respectively.
4. The ground robot repositioning method based on multi-sensor fusion according to claim 1, characterized in that: The visual relocation includes: Extract target feature points from visual camera data at the target moment, and cluster the target feature points into different categories of words; Combine the word type and the number of occurrences into a Bow descriptor; Based on the Bow descriptor, traverse the pre-built BoW descriptor library, find all visual image data with the same word type, and calculate the BoW similarity score; Perform GICP registration on the laser point cloud data corresponding to the visual image data with the highest BoW similarity score and the laser radar data at the target time; If the registration score meets the preset visual registration score threshold, the visual relocalization is successful; Calculate a first position and attitude estimation value based on the laser point cloud data and laser radar data corresponding to successful visual relocalization; The laser repositioning comprises: Calculate the SC descriptor based on the laser radar data at the target time; Based on the SC descriptor traversal and construction, the SC descriptor library is constructed to find the most similar N laser point cloud data and calculate the SC similarity scores respectively; Perform GICP registration on the laser point cloud data with the highest SC similarity score and the laser radar data at the target time; If the registration score meets the preset laser registration score threshold, the laser relocalization is successful; Calculate a second position attitude estimate value based on the laser point cloud data and laser radar data corresponding to the successful laser repositioning; The construction process of the SC descriptor library and the BoW descriptor library includes: Read the pre-generated high-precision map data, calculate the BoW descriptor for the visual image data at each moment and save the descriptor to the BoW descriptor library; calculate the SC descriptor for the laser point cloud data at each moment and save the descriptor to the SC descriptor library.
5. The ground robot repositioning method based on multi-sensor fusion according to claim 1, characterized in that: The splicing to generate the local map also includes: The local map is downsampled and filtered by SOR to obtain a smaller local map.
6. The ground robot repositioning method based on multi-sensor fusion according to claim 1, characterized in that: The calculating of the fourth position and posture estimation value of the ground robot based on the laser radar data and the local map comprises: Perform GICP registration of the LiDAR data with the laser point cloud data at each moment in the local map; When the obtained registration score satisfies a preset map registration score threshold, a fourth position attitude estimation value is calculated according to the number of laser point clouds and the laser radar data at the corresponding moment.
7. The ground robot repositioning method based on multi-sensor fusion according to claim 1, characterized in that: The fusion to obtain the final value of position and posture includes: Get the system relocation flag; If the system relocation flag is FALSE, retrieve the first position and attitude estimate and the second position and attitude estimate. If either estimate is not empty, use the non-empty estimate as the final position and attitude value, and set the system relocation flag to True. If both estimates are empty, the system relocation flag remains FALSE. If the system repositioning flag is True, the difference between the third position and attitude estimation value and the fourth position and attitude estimation value is calculated. If the difference value exceeds the preset difference threshold, the system repositioning flag is set to FALSE; if the difference value does not exceed the preset difference threshold, the third position and attitude estimation value and the fourth position and attitude estimation value are fused through the G2O library to perform joint factor graph optimization to obtain the final position and attitude value.
8. A ground robot repositioning device based on multi-sensor fusion, characterized in that: include: Data acquisition module, used to acquire visual camera data, lidar data and IMU data; A relocalization module for performing visual relocalization and laser relocalization based on visual camera data and lidar data, respectively; A first second estimation value calculation module, configured to calculate a first position and posture estimation value and / or a second position and posture estimation value of the ground robot according to the visual camera data and / or the laser radar data if the visual relocation and / or the laser relocation is successful; A third estimated value calculation module, used to calculate a third position and posture estimated value of the ground robot based on the laser radar data and the IMU data through an ESKF algorithm; A fourth estimated value calculation module is used to obtain the position and attitude estimation value of the ground robot at the previous moment, obtain the laser point cloud data of M adjacent moments from the pre-generated high-precision map data, and stitch them to generate a local map; calculate the fourth position and attitude estimation value of the ground robot based on the laser radar data and the local map; An estimation value fusion module, used for fusing and acquiring a final position and attitude value based on the first position and attitude estimation value, the second position and attitude estimation value, the third position and attitude estimation value, and the fourth position and attitude estimation value; The calculation process of the third position and posture estimation value includes: Construct the estimated state of the ground robot at time k : ; In the formula, are the position, velocity, attitude, accelerometer bias, gyroscope bias and gravity acceleration of the ground robot at time k; Estimated state quantity Derivative with respect to time, obtain the discrete error recursion formula of IMU data, and form the Jacobian matrix in the prediction process of the ESKF algorithm based on the discrete error recursion formula and , and construct the prediction equation: ; ; In the formula, is the predicted state error at time k relative to time k-1, is the predicted state error The corresponding prediction covariance matrix, is the prediction covariance matrix Correction value of is the predicted state error at time k-2 relative to time k-1 Correction value of The IMU data between time k-1 and time k are brought into the prediction equation for recursion to obtain the predicted state error. and the prediction covariance matrix ; Construct the observation equation for the prediction equation: ; In the formula, is the observed data, is the observation noise, The observation noise The covariance matrix of is an abstract function; Further calculation of Kalman gain : ; ; In the formula, is an intermediate variable; Based on Kalman gain Calculate the predicted state error and the prediction covariance matrix Correction value : ; ; Based on the corrected value Correction of estimated state , and from the corrected estimated state quantity Extract location from and posture as a third estimate.
9. The ground robot repositioning device based on multi-sensor fusion according to claim 8, characterized in that: The fusion to obtain the final value of position and posture includes: Get the system relocation flag; If the system relocation flag is FALSE, retrieve the first position and attitude estimate and the second position and attitude estimate. If either estimate is not empty, use the non-empty estimate as the final position and attitude value, and set the system relocation flag to True. If both estimates are empty, the system relocation flag remains FALSE. If the system repositioning flag is True, the difference between the third position and attitude estimation value and the fourth position and attitude estimation value is calculated. If the difference value exceeds the preset difference threshold, the system repositioning flag is set to FALSE; if the difference value does not exceed the preset difference threshold, the third position and attitude estimation value and the fourth position and attitude estimation value are fused through the G2O library to perform joint factor graph optimization to obtain the final position and attitude value.
Citation Information
Patent Citations
Laser and visual information fused robust positioning and mapping method and system
CN112985416A
ANFIS-ESKF-based ground mobile robot multi-sensor fusion positioning method
CN115060260A
Multi-sensor fusion map construction method and system for robot environment perception
CN116222543A
Method for generating texture information point cloud map based on panoramic camera and laser radar fusion
CN117237789A
Mobile robot positioning method based on multi-sensor fusion
CN117629212A
