Mapping method based on multi-sensor fusion, electronic equipment and storage medium

Abstract

The invention relates to the field of robots, and discloses a mapping method based on multi-sensor fusion, electronic equipment and a storage medium. According to the method, firstly, a laser radar, a millimeter-wave radar and a visual and motion inertial odometer are generated by collecting multi-sensor data, and it is ensured that other sensors can still provide effective information when part of the sensors are degraded due to insufficient illumination, noise interference or extreme dynamic scenes. And secondly, through real-time health degree evaluation, unstable data generated due to sensor degradation or abnormity is automatically eliminated, and it is ensured that only health data is used for generating a pose map, so that the robustness of data fusion is improved. And finally, a factor graph is constructed by utilizing head IMU data, global pose optimization is realized by virtue of short-time high-frequency stability and continuity of the factor graph, and even if other sensors are partially degraded, the accuracy and consistency of overall positioning can be maintained.

Description

Technical Field

[0001] The present invention relates to the field of robots, and in particular to a mapping method based on multi-sensor fusion, an electronic device and a storage medium. Background Art

[0002] Humanoid robots have been widely used in service, medical, education, industry and disaster relief, and these scenarios require robots to achieve high-precision perception and autonomous positioning in complex and changing environments. To meet this demand, multi-sensor fusion mapping technology is used in related fields to improve mapping accuracy and system robustness by integrating multiple sensor data.

[0003] At present, related multi-sensor fusion mapping technologies often use a combination of two or three sensors, such as lidar and IMU (Inertial Measurement Unit) or thermal infrared imager and RGB-D (Red Green Blue-Depth) camera. These solutions work well in conventional environments, but in specific or complex scenarios, some sensors are prone to performance degradation, resulting in poor stability of the fused data. Summary of the invention

[0004] The purpose of the present invention is to provide a mapping method, electronic device and storage medium based on multi-sensor fusion, so as to solve the problem of poor stability of multi-sensor combination in specific or complex scenes and improve the robustness and accuracy of data fusion.

[0005] To solve the above technical problems, the present invention provides a mapping method based on multi-sensor fusion, comprising: using a robot's multiple sensors to collect environmental data and preprocessing it to obtain multiple odometers; the multiple odometers include a lidar odometer, a millimeter-wave radar odometer, a visual odometer and a motion inertial odometer; performing real-time health assessment on the multiple odometers, and after each real-time health assessment, deactivating unhealthy odometers according to the results of the health assessment, and selecting odometers whose health meets the requirements in the results of the health assessment as healthy odometers; using the data of the robot's head IMU as a factor node and the healthy odometer as a factor constraint to construct a factor graph and generate a pose graph; after the robot's collection is completed, generating a map according to the pose graph.

[0006] The present invention also provides an electronic device, comprising: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the mapping method based on multi-sensor fusion as described above.

[0007] The present invention also provides a computer-readable storage medium storing a computer program, which, when executed by a processor, implements the mapping method based on multi-sensor fusion as described above.

[0008] In the present invention, firstly, laser radar, millimeter-wave radar, vision and motion inertial odometer are generated by collecting multi-sensor data to ensure that when some sensors are degraded due to insufficient light, noise interference or extreme dynamic scenes, other sensors can still provide effective information. Secondly, real-time health assessment automatically eliminates unstable data caused by sensor degradation or abnormality, ensuring that only healthy data is used to generate the pose graph, thereby improving the robustness of data fusion. Finally, the factor graph is constructed using the head IMU data, and the global pose optimization is achieved with its short-term high-frequency stability and continuity, so that the accuracy and consistency of the overall positioning can be maintained even if other sensors are partially degraded. BRIEF DESCRIPTION OF THE DRAWINGS

[0009] One or more embodiments are exemplarily described by pictures in the corresponding drawings, and these exemplified descriptions do not constitute limitations on the embodiments. Elements with the same reference numerals in the drawings represent similar elements, and unless otherwise stated, the figures in the drawings do not constitute proportional limitations.

[0010] Figure 1 is a schematic diagram of a mapping method based on multi-sensor fusion provided by the present invention; Figure 2 It is a technical flow chart of a mapping method based on multi-sensor fusion provided by the present invention; Figure 3 It is a schematic diagram of health monitoring in a mapping method based on multi-sensor fusion provided by the present invention; Figure 4 It is a schematic diagram of an electronic device provided by the present invention. DETAILED DESCRIPTION

[0011] Humanoid robots have been widely used in service, medical, education, industry and disaster relief. These application scenarios require robots to achieve high-precision perception and autonomous positioning in complex and changing environments. In order to meet this demand, multi-sensor fusion mapping technology is widely used in related fields to improve mapping accuracy and system robustness by integrating data from multiple sensors such as lidar, inertial measurement unit (IMU), RGB-D (Red Green Blue-Depth) camera, thermal infrared imager, millimeter wave radar, joint encoder, plantar force sensor, etc.

[0012] Existing multi-sensor fusion technology usually only fuses data from two or three sensors. For example, the combination of LiDAR and IMU (representative algorithms include FAST-LIO and LIO-SAM) or the fusion of thermal infrared imager and RGB-D camera (representative algorithm includes ROVIO) can achieve good positioning and mapping effects under normal conditions. However, due to the limitations of various sensors themselves, the low accuracy of a single sensor or the data being greatly disturbed by the environment often has a negative impact on the fusion algorithm: 1. Although lidar can provide high-precision depth information, data loss or increased errors may occur in extreme environments such as fog, dust, and strong reflections.

[0013] 2. Visual sensors (RGB-D cameras, infrared imagers) are greatly affected by lighting, occlusion, and noise, and are prone to degradation in low-light or high-contrast scenes.

[0014] 3. IMU has high accuracy and stability in the short term, but the long-term accumulated drift causes deviations in pose estimation.

[0015] 4. Although millimeter-wave radar has strong adaptability to some harsh environments, the speed and posture it calculates may be unstable under data noise and non-ideal acquisition conditions.

[0016] 5. Joint encoders and plantar force sensors are mainly used to estimate the internal state of the robot, but their data quality is also affected by the sensor itself and its installation accuracy.

[0017] In addition, there are differences in the acquisition frequency, accuracy and response characteristics between different sensors. When directly fused, the performance degradation of a certain sensor may easily affect the overall system. Especially in extreme or dynamic and complex scenarios, some sensors may fail or generate abnormal data. How to effectively eliminate the impact of failed sensors and how to use the complementary advantages of each sensor to achieve full-scenario, stable and high-precision environmental perception are technical problems that need to be solved urgently.

[0018] To make the purpose, technical scheme and advantages of the present invention clearer, the following will be described in detail with reference to the accompanying drawings to various embodiments of the present invention. However, it will be appreciated by those skilled in the art that in various embodiments of the present invention, many technical details are proposed to enable the reader to better understand the present application. However, even without these technical details and various changes and modifications based on the following embodiments, the technical scheme claimed in the present application can be implemented. The division of the following embodiments is for the convenience of description, and should not constitute any limitation on the specific implementation of the present invention. The various embodiments can be combined and referenced with each other under the premise of no contradiction.

[0019] An embodiment of the present invention relates to a mapping method based on multi-sensor fusion, which can be applied to robot control systems, embedded systems, smart phones, tablet computers, personal computers, drones and other intelligent terminal devices, and is particularly suitable for execution entities such as humanoid robots and various intelligent robots. The present invention uses the robot's multi-sensor to collect environmental data and pre-process it to obtain multiple odometers; the multiple odometers include laser radar odometers, millimeter wave radar odometers, visual odometers and motion inertial odometers; the multiple odometers are evaluated for real-time health, and after each real-time health evaluation, the unhealthy odometers are disabled according to the evaluation results, and the odometers that meet the health requirements are selected as healthy odometers. The head IMU data of the robot is used as a factor node, and the healthy odometer is used as a factor constraint to construct a factor graph and generate a pose graph; after the robot acquisition is completed, a map is generated according to the pose graph. In the present invention, laser radar, millimeter wave radar, vision and motion inertial odometers are first generated by collecting multi-sensor data to ensure that when some sensors are degraded due to insufficient light, noise interference or extreme dynamic scenes, other sensors can still provide effective information. Secondly, real-time health assessment automatically removes unstable data caused by sensor degradation or abnormality, ensuring that only healthy data is used to generate the pose graph, thereby improving the robustness of data fusion. Finally, the factor graph is constructed using head IMU data, and global pose optimization is achieved with its short-term high-frequency stability and continuity, which can maintain the accuracy and consistency of overall positioning even if other sensors are partially degraded.

[0020] The following is a specific description of the implementation details of a mapping method based on multi-sensor fusion of the present invention. The following content is only for the convenience of understanding the implementation details and is not necessary for the implementation of this solution.

[0021] The implementation steps of this embodiment are as follows: Figure 1 and Figure 2 As shown, it includes steps 110 to 150: Step 110, using multiple sensors of the robot to collect environmental data and perform preprocessing to obtain multiple odometers.

[0022] Specifically, the above-mentioned multi-sensors include laser radar, millimeter-wave radar, RGB-D camera, infrared thermal imager, head IMU, body part IMU, joint encoder and foot force sensor, etc. The above-mentioned multiple odometers include laser radar odometer, millimeter-wave radar odometer, visual odometer and motion inertial odometer.

[0023] In step 110, if Figure 2The sensor module and odometer generation algorithm module on the left in the middle. The device uses multiple sensors (lidar, millimeter-wave radar, RGB-D camera, infrared thermal imager, head IMU, body IMU, joint encoder and foot force sensor, etc.) to work together, using multi-threading technology to synchronously collect environmental data and pre-process it to generate multiple odometers. These odometers are fused to obtain stable and accurate pose estimation for mapping and positioning. The following is a detailed description of the data processing and fusion process of each sensor.

[0024] First, the lidar obtains point cloud data by scanning the surrounding environment.

[0025] Specifically, after the LiDAR data is preprocessed, it mainly includes the denoising and registration process of the point cloud. In the denoising process, statistical methods such as Voxel Grid filtering or conditional random field filtering are used to remove outliers, which helps to reduce the impact of environmental noise and abnormal reflections on the point cloud data. Point cloud registration uses a point cloud registration algorithm based on normal information (Normal Iterative Closest Point, N-ICP algorithm), where the ICP algorithm is usually used to align two point cloud data sets by minimizing the error between point clouds. N-ICP combines normal information to provide better registration results when the point cloud surface features are more complex, the noise is larger, or the point cloud is sparse. It can be applied to many scenes that require high-precision point cloud registration, especially in scenes with obvious surface features or complex shapes.

[0026] Optionally, the above point cloud registration method may also use a traditional ICP algorithm or a point cloud registration algorithm based on deep learning, which may provide better performance in specific scenarios.

[0027] Then, the millimeter-wave radar data is fused with the robot's body IMU data to generate a millimeter-wave radar odometer.

[0028] Specifically, the millimeter-wave radar data is first filtered (such as Kalman filtering, etc.) to remove interference caused by environmental noise. Subsequently, the Doppler effect is used to estimate the speed of the millimeter-wave radar, thereby generating initial radar speed information. The speed information obtained by the preliminary calculation is removed by the random sample consensus algorithm (RANSAC) algorithm to ensure the reliability of the data. Finally, the radar speed processed by RANSAC is fused with the acceleration and angular velocity data of the IMU of the body part to finally generate the millimeter-wave radar odometer. The data of the millimeter-wave radar odometer has a higher frequency and stronger environmental adaptability, and can provide more stable data under severe weather conditions (such as high haze, heavy rain, etc.), so it can effectively supplement the shortcomings of lidar in specific environments.

[0029] Optionally, when the millimeter-wave radar data exhibits highly nonlinear dynamic system characteristics, the Kalman filter can be replaced by a particle filter to obtain more accurate results.

[0030] Then, the data from the RGB-D camera and the infrared thermal imager are fused to generate the visual odometry.

[0031] Specifically, first, the infrared imager data is filtered (such as median filtering) and histogram equalized to obtain enhanced thermal infrared data; then, the RGB-D camera data and thermal infrared data are aligned and feature matching is performed (such as Oriented Fast and Rotated Brief, ORB algorithm); finally, the RGB-D camera data and thermal infrared data are fused based on the feature matching results to generate the visual odometer. The RGB-D camera and the infrared thermal imager work together. The RGB-D camera provides visible light information for environmental mapping by capturing color images and depth images, while the infrared thermal imager supplements the perception capability in low-light or no-light environments. In this way, the images provided by the RGB-D camera and the infrared thermal imager can complement each other and enhance the robot's environmental perception capability, especially in low-light or high-contrast environments, and can maintain a high positioning accuracy.

[0032] Optionally, the fusion of the above-mentioned RGB-D camera and infrared thermal imager data can also use a deep learning model for image registration and feature fusion, such as using a convolutional neural network to automatically extract and fuse the features of RGB and infrared images to improve the robustness and accuracy of data fusion.

[0033] Finally, the kinematic inertial odometry estimates the robot’s posture and position during motion by fusing data from joint encoders, plantar force sensors, and IMUs of body parts.

[0034] Specifically, the joint encoder, the plantar force sensor and the body part IMU constitute a motion inertial odometer, which is mainly used to estimate the dynamic state of the robot in motion. The joint encoder provides the robot's joint angle and angular velocity information, the plantar force sensor measures the contact force between the robot and the ground, and the body part IMU provides the robot's acceleration and angular velocity data. By fusing these data, the precise position and posture of the robot in different motion states can be calculated. In this embodiment, the factor graph optimization method is used to integrate these sensor data and calculate the dynamic state of the robot to ensure that the robot can remain stable under fast motion or complex gait control.

[0035] In general, based on the data collection and preprocessing process in the above step 110, it is possible to ensure that the device collects valid data in all scenarios, thereby ensuring that when some sensors are degraded due to insufficient light, noise interference or extreme dynamic scenes, other sensors can still provide valid information, providing strong support for subsequent mapping.

[0036] Step 120, performing real-time health assessment on the multiple odometers, after each real-time health assessment, deactivating unhealthy odometers according to the results of the health assessment, and selecting an odometer whose health meets the requirements in the results of the health assessment as a healthy odometer.

[0037] The specific implementation process of step 120 is as follows Figure 2 Health monitoring module and Figure 3 As shown in the flow chart, it includes steps 121 to 124, specifically: Step 121 , during each real-time health evaluation, degradation monitoring is performed on the multiple odometers, and a degradation monitoring result is generated.

[0038] To achieve this goal, the system monitors the health of each sensor through different degradation detection methods during each real-time health assessment. These methods include: 1. Monitor the characteristic values ​​of the LiDAR odometer and the millimeter wave odometer. Figure 2 In the monitoring indicator submodule of the health monitoring module, the monitoring indicators for the laser radar odometer and the millimeter wave odometer are eigenvalues ​​and eigenvectors. Specifically, for the laser radar odometer and the millimeter wave radar odometer, the degradation factor formed during the registration of the feature point cloud is monitored to determine whether failure or obvious degradation occurs. This method is mainly based on the optimization problem: .in, A is the observation matrix, x are state variables (such as robot pose), b is the measurement vector (the point cloud feature constraint of LiDAR or millimeter wave radar). This optimization problem corresponds to an overdetermined linear equation system. To analyze the degradation scenario, the system adds a linear equation with unknown direction as a disturbance term to the original equations, which is expressed as: .in, c is the vector of "unknown direction" or "extra constraint" added, x ∗ is the original optimal solution, δd is the distance between the perturbed equation and the original optimal solution.

[0039] By defining the degradation factor , can quantify the degree to which the optimal solution deviates from the original result after a certain disturbance is applied to the above optimization problem. Among them, δx Represents state variables x In the disturbance δd If the degradation factor D If it is less than the predefined threshold, it means that the state of the LiDAR or millimeter wave radar is seriously degraded, and the current point cloud registration result cannot reliably reflect the real environment. The predefined threshold can be obtained through simulation analysis of degradation scenes, ensuring effective detection of degradation under different environmental interferences.

[0040] Optionally, for setting the degradation factor, in addition to determining the threshold through simulation analysis, machine learning or deep learning methods can also be used, such as training a neural network model to automatically learn the degradation critical point based on the sensor's historical data and real-time data to predict its health status, so as to better adapt to the hardware characteristics of different sensors and different types of scenarios.

[0041] 2. Monitor the visual odometer in information matrix. Figure 2 In the monitoring indicator submodule of the health monitoring module, the monitoring indicator for the visual odometer is the D-optimality method. Specifically, for the visual odometer, the degradation judgment is performed using the D-Optimality method. D-Optimality uses the information matrix to quantify the constraint ability of the observation data on the state variables. The information matrix is ​​defined as .in, H is the Jacobian matrix corresponding to the observation model, W is the observation noise weight matrix. The size of the information matrix determinant directly reflects the observability of the observation to the state parameters. When the determinant value is larger, the observation ability is stronger; conversely, when the determinant value is close to zero, it means that the data of the visual odometer is insufficient to constrain the state estimation and degradation occurs. When the system detects that the determinant shows a significant downward trend or is continuously in a low value state, the visual odometer can be considered to be degraded.

[0042] Optionally, for the D-Optimality method, an adaptive weighting strategy can be used in combination with scene recognition (such as indoor, outdoor, and light intensity) to dynamically set the warning value of the information matrix determinant to further improve the accuracy and efficiency of degradation detection.

[0043] 3. Monitor the posterior covariance matrix indicators of the motion inertial odometer. Figure 2In the monitoring indicator module of the health monitoring module, the monitoring indicator for the motion inertial odometer is the posterior covariance matrix; among them, the posterior covariance matrix indicators include eigenvalues, determinants and condition numbers. Specifically, the motion inertial odometer uses the factor graph method to estimate the internal state of the humanoid robot, and the internal state includes but is not limited to the posture of each joint of the robot, the position of the center of mass, and the posture and speed of the whole body. The factor graph method realizes state estimation by constructing the maximum a posteriori probability (MAP) problem, which can be expressed in mathematical form as:

[0044] in, x is the robot state to be estimated (including joint angles, acceleration, angular velocity, etc.), z The factor graph models these sensor measurements and state variables, and finally obtains the optimal solution by optimizing the variables in the joint probability distribution. x * During this optimization process, the system will also obtain a posterior covariance matrix P , which is used to characterize the confidence distribution of the state estimate.

[0045] If the observation constraints in some directions of the system are insufficient or there is noise interference, the posterior covariance matrix P Specifically, if there are eigenvalues ​​that are too large in some directions in the eigenvalue distribution, it means that the amount of observation information in that direction is insufficient or the sensor data is degraded. In this case, it can be determined that the kinematic inertial odometer is degraded.

[0046] Optionally, if there are more dimensions of motion inertial observation information (for example, adding more types of force sensors or multi-degree-of-freedom joint encoders), the dimension of the posterior covariance matrix will also increase accordingly. In addition to eigenvalues, determinants, and condition numbers, the system can introduce other matrix decomposition methods (such as singular value decomposition) to capture degradation features, so that the motion inertial odometer still has accurate degradation detection capabilities in more complex dynamic environments.

[0047] Step 122: combining the degradation monitoring result with a common index to calculate the health of the multiple odometers.

[0048] Specifically, Figure 2In the general indicator submodule of the health detection module, the system combines the degradation monitoring results with the general indicator comprehensive score, and can use a weighted strategy to quantitatively calculate the health of each odometer. Among them, the general indicators include the sensor's acquisition frequency and posture change rate: the acquisition frequency reflects the sensor's response speed to environmental changes. If the acquisition frequency is low, the system may not be able to capture subtle changes in the environment or the robot's movement in time, resulting in a lag in the positioning results. Therefore, the scoring weight of the acquisition frequency is 10 points; the posture change rate reflects the sensor's adaptability during the robot's dynamic movement, and the posture change rate has a scoring weight of 5 points. If the posture change rate is significantly different from the actual movement, it means that the odometer data may have problems such as degradation and inaccuracy.

[0049] Step 123, after obtaining the health of the multiple odometers each time, set the odometer whose health is lower than a preset threshold among the multiple odometers as an unhealthy odometer, deactivate the unhealthy odometer and clear the historical data of the unhealthy odometer, and reinitialize and evaluate the health of the sensor corresponding to the unhealthy odometer until the unhealthy odometer returns to normal.

[0050] Specifically, Figure 2 In the switching logic submodule of the priority queue and processing logic module, the system will deactivate the odometers below the preset threshold each time the health of each odometer is obtained to prevent degraded or failed sensors from affecting subsequent data fusion and positioning accuracy. In order to completely eliminate the negative impact that an unhealthy odometer may have, the system will also clear the historical data of the odometer. This helps prevent historical error information from "polluting" subsequent state estimates. In addition, if Figure 2 The dotted line connecting the priority queue and the processing logic module and the odometer generation module is the restart step. The system will reinitialize the corresponding sensors, including calibrating the external parameters of the lidar, updating the noise model of the millimeter wave radar, resetting the ORB feature extraction or information matrix in the visual odometer, and reloading the initial state of the factor graph optimization part in the motion inertial odometer. This reinitialization process will be accompanied by further health assessment until the odometer is detected to be restored to a usable state before it is allowed to re-participate in the fusion.

[0051] Step 124, selecting an odometer whose health meets the requirements from the results of the health assessment as a healthy odometer.

[0052] In the above step 124, if Figure 2In the priority queue and processing logic module, the system needs to make priority selections among all the results of health assessments and select healthy odometers for subsequent data fusion. The selection rule for healthy odometers is as follows: in the results of the health assessment, if the number of odometers whose health meets the preset threshold is greater than or equal to two, then the first two odometers with the highest health are selected as healthy odometers; in the results of the health assessment, if the number of odometers whose health meets the preset threshold is one or zero, then the single odometer with the highest health is selected as the healthy odometer. Through this rule, on the one hand, the effective fusion of high-precision data from multiple sensors can be guaranteed, and on the other hand, the basic positioning capability is retained when large-scale sensor degradation occurs.

[0053] Prior to this, Figure 2 The priority queue and predefined queue submodule in the processing logic module can predefine a queue, and the order from high to low is: lidar odometer, visual odometer, millimeter wave radar odometer and motion inertial odometer. It is used as a reference for odometer selection during initial startup.

[0054] Based on this design, in practical applications, if multiple sensors are in a healthy state at the same time, the two optimal sensors of high-precision sensors (such as lidar, visual odometer) and auxiliary sensors (such as millimeter-wave radar, motion inertial odometer) can be used to constrain each other, thereby maintaining robust positioning performance in extreme environments or fast dynamic scenes. When the sensor degradation is serious and there is only one healthy odometer left in the system, the sensor can also be used to temporarily maintain the most basic pose estimation. After the other odometers are reinitialized and restored to health, they can be rejoined to the fusion queue to improve overall accuracy and stability.

[0055] Step 130, using the robot's head IMU data as a factor node and the health odometer as a factor constraint, constructing a factor graph and generating a pose graph.

[0056] In step 130, if Figure 2 The selected healthy odometer data will be used for the subsequent factor graph construction. In addition, the reason for choosing the head IMU as the factor node is that it has a relatively stable installation position and is less dependent on the external environment. It can continue to provide relatively smooth and continuous attitude measurement in scenarios with visual degradation or radar degradation. The details are as follows: 1. Accuracy: IMUs provide smooth measurements, and despite the presence of noise, their measurements are usually not affected by too many outliers. This makes the state estimation of IMUs very accurate under the constraints of other sensors (such as vision and radar).

[0057] 2. Bias constraints: The bias of the IMU (such as the bias of the accelerometer and gyroscope) can be effectively constrained by observations from other sensors such as visual inertial odometry and LiDAR inertial odometry. This interaction makes the state estimation of the IMU more reliable.

[0058] 3. Environmental adaptability: In complex and perception-constrained environments (such as low light, long corridors, or heavy dust environments), the use of IMU makes the system more robust to environment-related issues (such as geometric or visual degradation).

[0059] like Figure 2 In the odometer selection module of the final state output module, after treating the IMU data as a factor node, the system will select the corresponding measurement results from the healthy odometer selected in step 120 as factors and add them to the factor graph. The fusion process includes: first, the system performs time-series alignment on the data of the healthy odometer, maps the odometer measurement of the corresponding timestamp to the coordinate system or the global coordinate system where the IMU is located, and generates constraint factors in the factor graph. Secondly, for the lidar odometer and millimeter-wave radar odometer, the pose estimation or point cloud registration results provided by them are used as constraint factors to supplement the spatial matching information between nodes in the factor graph; for the visual odometer, the relative pose of the visual odometer (or the constraint between key frames) is used as an observation to improve the accuracy of pose estimation and adaptability to the environment; for the motion inertial odometer, the results of its internal estimation of the center of mass position and joint state can be used to constrain the posture of the head IMU and the overall motion of the robot in the form of factors.

[0060] After the comprehensive effect of the multi-sensor factors in the above step 130, the factor graph optimization will simultaneously process the continuous measurements of the IMU (such as pre-integration or incremental observation) and the discrete constraints of each health odometer, and finally solve the optimal posture of the robot at each moment under a unified optimization framework to obtain a globally consistent posture trajectory.

[0061] Step 140: perform loop closure monitoring based on the health odometer, and optimize the pose graph based on the loop closure monitoring result.

[0062] Specifically, the above-mentioned loop monitoring and optimization method includes: if the health odometer includes the lidar odometer or the millimeter wave radar odometer, the loop monitoring is performed on the current frame data and the loop frame data through the IPC point cloud registration algorithm; if the health odometer includes a visual odometer, the loop monitoring is performed on the current frame data and the loop frame data through a random sampling consistency algorithm; the current frame data after the loop monitoring is added to the factor graph as a loop factor constraint, and the pose graph is optimized.

[0063] After entering step 140, the system will give priority to selecting the odometer in a healthy state to perform loop monitoring. Figure 2The loop monitoring module of the health odometer includes a laser radar odometer or a millimeter wave radar odometer. The system will align the current frame point cloud with the loop frame point cloud through ICP (Iterative Closest Point) or its improved algorithm (such as N-ICP) to determine whether there is enough similarity between the two to form a loop. After the point cloud is aligned, if the alignment error is within an acceptable range, it means that the current frame and the historical frame are in the same area; otherwise, it means that there is a significant difference or noise interference between the current frame and the historical frame, and the loop detection condition is not met. If the health odometer includes a visual odometer, the system will use RANSAC to match feature points between the current frame image and the loop frame image. Specifically, the system first extracts image features such as ORB from the RGB-D camera or infrared thermal imager data, and uses descriptor matching to determine whether there are suspicious loop candidate frames. If the number of matching feature points is sufficient and the proportion of RANSAC verified internal points reaches the threshold, it means that the current frame and the historical frame correspond to the same physical scene and meet the loop condition.

[0064] The above loop detection factor e loop The relative pose relationship between two frames of images or point clouds is calculated as follows:

[0065] in, and are the poses of the current frame and the loop frame respectively, The relative pose between two frames is obtained through ICP alignment or RANSAC verification. The main functions of the loop factor in the factor graph include: 1. Correcting accumulated errors: During long-term operation, the positioning results of lidar odometers, millimeter-wave radar odometers, visual odometers, and other sensors may drift. The loop detection factor can correct this drift through closed-loop constraints.

[0066] 2. Improve the land Figure 1 Consistency: When the robot passes through the same area multiple times, the loop closure factor constrains these repeatedly visited areas to remain consistent on the global map to reduce the offset or deformation of overlapping areas.

[0067] 3. Optimize the pose graph: In the factor graph optimization process, the loop detection factor provides global constraints, improves the correction capability of the backend optimization for pose estimation, and enhances the global pose optimization effect.

[0068] After completing the geometric verification of the frames that satisfy the loop constraints, the system will inject these loop factors into the factor graph optimization process to further accurately correct the global pose graph. After this optimization process, the robot's historical trajectory and current trajectory are consistent at the global level, and the accumulated errors are corrected, which significantly improves the overall accuracy of the map and the ability to understand the scene.

[0069] Step 150, after the robot completes the collection, a map is generated according to the posture graph.

[0070] Specifically, Figure 2 In the map generation submodule of the final state output module, the system has obtained the optimized pose graph of each moment or key frame (i.e., the pose of each sensor frame in the global reference coordinate system) through the factor graph optimization mentioned above. Next, these optimized poses are used to consistently project the environmental perception data of each frame (such as lidar point cloud, visual image and depth information, etc.) into a unified coordinate system to finally generate a global map. Moreover, the posterior pose obtained after global optimization of the factor graph not only corrects the deviation caused by sensor noise and cumulative error, but also maintains consistency between multiple visits to the same area, so that the final output map has good coherence and accuracy in the global range.

[0071] In the present invention, firstly, laser radar, millimeter-wave radar, vision and motion inertial odometer are generated by collecting multi-sensor data to ensure that when some sensors are degraded due to insufficient light, noise interference or extreme dynamic scenes, other sensors can still provide effective information. Secondly, real-time health assessment automatically eliminates unstable data caused by sensor degradation or abnormality, ensuring that only healthy data is used to generate the pose graph, thereby improving the robustness of data fusion. Finally, the factor graph is constructed using the head IMU data, and the global pose optimization is achieved with its short-term high-frequency stability and continuity, so that the accuracy and consistency of the overall positioning can be maintained even if other sensors are partially degraded.

[0072] The steps of the above method are divided only for clear description. When implemented, they can be combined into one step or some steps can be split and decomposed into multiple steps. As long as they include the same logical relationship, they are all within the protection scope of the present invention. Adding insignificant modifications to the algorithm or process or introducing insignificant designs without changing the core design of the algorithm and process are all within the protection scope of the invention.

[0073] In addition, the examples mentioned in the above embodiments can be freely combined, and any combination can be understood as an embodiment. The "embodiment" or "example" appearing in various positions in the specification does not necessarily refer to the same embodiment, nor is it an independent or alternative embodiment that is mutually exclusive with other embodiments. It can be understood by those skilled in the art that the embodiments described herein can be combined with other embodiments.

[0074] Another embodiment of the present invention relates to an electronic device, such as Figure 4 As shown, It includes at least one processor 501; and a memory 502 that is communicatively connected to the at least one processor; wherein the memory 502 stores instructions that can be executed by the at least one processor 501, and the instructions are executed by the at least one processor 501 so that the at least one processor 501 can execute the mapping method based on multi-sensor fusion as described above.

[0075] The memory 502 and the processor 501 are connected in a bus manner, and the bus may include any number of interconnected buses and bridges, and the bus connects various circuits of one or more processors 501 and the memory 502 together. The bus can also connect various other circuits such as peripheral devices, voltage regulators, and power management circuits, which are well known in the art and are therefore not further described herein. The bus interface provides an interface between the bus and the transceiver. The transceiver can be one element or multiple elements, such as multiple receivers and transmitters, providing a unit for communicating with various other devices on a transmission medium. The data processed by the processor 501 is transmitted on a wireless medium via an antenna, and further, the antenna also receives data and transmits the data to the processor 501.

[0076] The processor 501 is responsible for managing the bus and general processing, and can also provide various functions, including timing, peripheral interfaces, voltage regulation, power management and other control functions. The memory 502 can be used to store data used by the processor 501 when performing operations.

[0077] Another embodiment of the present invention relates to a computer-readable storage medium storing a computer program, which implements the above method embodiment when executed by a processor.

[0078] That is, those skilled in the art can understand that all or part of the steps in the above-mentioned embodiment method can be completed by instructing the relevant hardware through a program, and the program is stored in a storage medium, including several instructions to enable a device (which can be a single-chip microcomputer, chip, etc.) or a processor to execute all or part of the steps of the method described in each embodiment of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk and other media that can store program codes.

[0079] Those skilled in the art will appreciate that the above embodiments are specific embodiments for implementing the present invention, and that in actual applications, various changes may be made in form and detail without departing from the spirit and scope of the present invention.

Claims

1. A mapping method based on multi-sensor fusion, characterized in that: include: Use the robot's multiple sensors to collect environmental data and pre-process it to obtain multiple odometers; The multiple odometers include a laser radar odometer, a millimeter wave radar odometer, a visual odometer and a motion inertial odometer; Performing real-time health evaluation on the multiple odometers, after each real-time health evaluation, deactivating unhealthy odometers according to the results of the health evaluation, and selecting an odometer whose health meets the requirements in the results of the health evaluation as a healthy odometer; The data of the robot's head IMU is used as a factor node, and the health odometer is used as a factor constraint to construct a factor graph and generate a pose graph; After the robot completes the collection, a map is generated according to the posture graph.

2. The mapping method based on multi-sensor fusion according to claim 1, characterized in that: The method further comprises: After constructing the factor graph and generating the pose graph, loop closure monitoring is performed based on the health odometer, and the pose graph is optimized based on the result of the loop closure monitoring.

3. The mapping method based on multi-sensor fusion according to claim 2, characterized in that: The performing loop monitoring based on the health odometer and optimizing the posture graph based on the result of the loop monitoring includes: If the health odometer includes the laser radar odometer or the millimeter wave radar odometer, the loop monitoring is performed on the current frame data and the loop frame data through the IPC point cloud registration algorithm; If the health odometer includes a visual odometer, the loop monitoring is performed on the current frame data and the loop frame data by using a random sampling consistency algorithm; The current frame data after the loop closure monitoring is added to the factor graph as a loop closure factor constraint, and the pose graph is optimized.

4. The mapping method based on multi-sensor fusion according to claim 1, characterized in that: The multi-sensor includes a laser radar, a millimeter-wave radar, an RGB-D camera, an infrared thermal imager, the head IMU, the body IMU, a joint encoder and a plantar force sensor; The method uses multiple sensors to collect environmental data and pre-processes the data to obtain multiple odometers, including: Performing point cloud registration based on normal information on the laser radar data to generate the laser radar odometer; fusing the data of the millimeter-wave radar with the data of the body IMU to generate the millimeter-wave radar odometer; Performing data fusion on the data of the RGB-D camera and the infrared thermal imager to generate the visual odometer; The data of the joint code, the plantar force sensor and the body IMU are fused to generate the motion inertial odometer.

5. The mapping method based on multi-sensor fusion according to claim 4 is characterized in that: The step of fusing the data of the millimeter-wave radar with the data of the body IMU to generate the millimeter-wave radar odometer includes: Filtering and denoising the data of the millimeter wave radar; Performing Doppler velocity estimation on the de-noised millimeter-wave radar data to generate an initial radar velocity; Removing outliers from the initial radar speed by a random sampling consistency algorithm to generate a final radar speed; The final radar velocity is fused with the data of the body IMU and point cloud registration is performed to generate the millimeter wave radar odometer.

6. The mapping method based on multi-sensor fusion according to claim 1, characterized in that: The performing real-time health evaluation on the plurality of odometers, and after each real-time health evaluation, disabling unhealthy odometers according to the result of the health evaluation, comprises: During each real-time health evaluation, performing degradation monitoring on the plurality of odometers and generating degradation monitoring results; Combining the degradation monitoring results with general indicators to calculate the health of the multiple odometers; wherein the general indicators include the acquisition frequency and posture change rate of the sensor; After obtaining the health of the multiple odometers each time, the odometer whose health is lower than a preset threshold among the multiple odometers is set as an unhealthy odometer, the unhealthy odometer is disabled and the historical data of the unhealthy odometer is cleared, and the sensor corresponding to the unhealthy odometer is reinitialized and the health is evaluated until the unhealthy odometer returns to normal.

7. The mapping method based on multi-sensor fusion according to claim 6, characterized in that: The step of performing degradation monitoring on the plurality of odometers comprises: Performing characteristic value monitoring on the laser radar odometer and the millimeter wave radar odometer; Performing information matrix determinant monitoring on the visual odometer; The motion inertial odometer is subjected to a posteriori covariance matrix indicator monitoring; wherein the posteriori covariance matrix indicator includes an eigenvalue, a determinant, and a condition number.

8. The mapping method based on multi-sensor fusion according to claim 1, characterized in that: The step of selecting an odometer whose health degree meets the requirements from the result of the health degree assessment as a healthy odometer comprises: In the result of the health evaluation, if the number of odometers whose health meets the preset threshold is greater than or equal to two, the first two odometers with the highest health are selected as healthy odometers; In the result of the health evaluation, if the number of odometers whose health meets the preset threshold is one or zero, the single odometer with the highest health is selected as the healthy odometer.

9. An electronic device, characterized in that: include: at least one processor; as well as, a memory communicatively connected to the at least one processor; wherein, The memory stores instructions that can be executed by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the mapping method based on multi-sensor fusion as described in any one of claims 1 to 8.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the mapping method based on multi-sensor fusion according to any one of claims 1 to 8 is implemented.

Images