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

Abstract

The present invention relates to the field of robotics, and discloses a mapping method, electronic device, and storage medium based on multi-sensor fusion. In the present invention, first, laser radar, millimeter-wave radar, vision, and motion inertial odometers 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, unstable data caused by sensor degradation or abnormality is automatically eliminated through real-time health assessment to ensure that only healthy data is used to generate a pose graph, thereby improving the robustness of data fusion. Finally, a factor graph is constructed using head IMU data, and global pose optimization is achieved by virtue of its short-term high-frequency stability and continuity. 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 robotics, and in particular to a mapping method, electronic equipment, and storage medium based on multi-sensor fusion. Background Art

[0002] Humanoid robots are widely used in services, healthcare, education, industry, and disaster relief. 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 being used in related fields to improve mapping accuracy and system robustness by integrating data from multiple sensors.

[0003] Currently, 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 scenarios 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 assessments on the multiple odometers, and after each real-time health assessment, deactivating unhealthy odometers based on 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 factor nodes and the healthy odometers as factor constraints to construct a factor graph and generate a pose graph; after the robot's collection is completed, generating a map based on 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 above-mentioned mapping method based on multi-sensor fusion.

[0008] In this invention, multi-sensor data is first collected to generate LiDAR, millimeter-wave radar, vision, and motion inertial odometry. This ensures that even when some sensors degrade due to insufficient lighting, noise interference, or extreme dynamic scenes, other sensors can still provide valid information. Secondly, real-time health assessment automatically eliminates unstable data caused by sensor degradation or anomalies, ensuring that only healthy data is used to generate the pose graph, thereby improving the robustness of data fusion. Finally, a factor graph is constructed using head IMU data, and its short-term high-frequency stability and continuity are leveraged to achieve global pose optimization. Even if other sensors partially degrade, the accuracy and consistency of overall positioning can be maintained. BRIEF DESCRIPTION OF THE DRAWINGS

[0009] One or more embodiments are exemplarily illustrated by pictures in the corresponding drawings. These exemplifications do not constitute limitations on the embodiments. Elements with the same reference numerals in the drawings are represented as similar elements. 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;

[0011] Figure 2 This is a technical flow chart of a mapping method based on multi-sensor fusion provided by the present invention;

[0012] Figure 3 This is a schematic diagram of health monitoring in a mapping method based on multi-sensor fusion provided by the present invention;

[0013] Figure 4 It is a schematic diagram of an electronic device provided by the present invention. DETAILED DESCRIPTION

[0014] Humanoid robots are widely used in services, healthcare, education, industry, and disaster relief. These applications require high-precision perception and autonomous positioning in complex and changing environments. To meet this demand, multi-sensor fusion mapping technology is widely used in related fields. By integrating data from multiple sensors such as lidar, inertial measurement units (IMUs), RGB-D (Red, Green, Blue-Depth) cameras, thermal infrared imagers, millimeter-wave radars, joint encoders, and plantar force sensors, it improves mapping accuracy and system robustness.

[0015] Existing multi-sensor fusion technologies typically combine data from only two or three sensors. For example, the combination of a lidar and an IMU (representative algorithms include FAST-LIO and LIO-SAM) or the fusion of a thermal infrared imager and an RGB-D camera (representative algorithms include ROVIO) can achieve good positioning and mapping results in conventional environments. However, due to the inherent limitations of each sensor, the low accuracy of a single sensor or the significant environmental interference with the data often negatively impacts the fusion algorithm:

[0016] 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.

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

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

[0019] 4. Although millimeter-wave radar is highly adaptable to some harsh environments, the speed and position it calculates can be unstable under data noise and non-ideal acquisition conditions.

[0020] 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 the installation accuracy.

[0021] Furthermore, different sensors vary in acquisition frequency, accuracy, and response characteristics, making direct fusion vulnerable to degradation of a single sensor's performance and the overall system impact. Especially in extreme or dynamic, complex scenarios, some sensors may fail or generate anomalous data. Effectively eliminating the impact of failed sensors and leveraging the complementary strengths of each sensor to achieve full-scenario, stable, and high-precision environmental perception are pressing technical challenges.

[0022] To make the purpose, technical solutions and advantages of the present invention clearer, each embodiment of the present invention will be described in detail below with reference to the accompanying drawings. However, it will be understood by those skilled in the art that in each embodiment of the present invention, many technical details are provided 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 solutions claimed in the present application can be implemented. The division of the following embodiments is for convenience of description and should not constitute any limitation on the specific implementation of the present invention. The various embodiments can be combined with each other and referenced to each other under the premise that there is no contradiction.

[0023] One 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, smartphones, tablets, personal computers, drones, and other intelligent terminal devices, and is particularly suitable for humanoid robots and various intelligent robots. The method uses a robot's multiple sensors to collect and preprocess environmental data to obtain multiple odometry meters. These odometry meters include a lidar odometry, a millimeter-wave radar odometry, a visual odometry meter, and a motion inertial odometry meter. The odometry meters are then evaluated for health in real time. After each real-time health evaluation, unhealthy odometry meters are deactivated based on the evaluation results, and odometry meters that meet health requirements are selected as healthy odometry meters. The robot's head IMU data is used as factor nodes, and the healthy odometry meters serve as factor constraints to construct a factor graph and generate a pose graph. After the robot completes the data collection, a map is generated based on the pose graph. In the present invention, the lidar, millimeter-wave radar, visual odometry, and motion inertial odometry meters are first generated by collecting multi-sensor data. This ensures that even if some sensors degrade due to insufficient lighting, noise interference, or extreme dynamic scenes, the remaining sensors can still provide valid information. Second, real-time health assessment automatically removes unstable data caused by sensor degradation or anomalies, ensuring that only healthy data is used to generate the pose graph, thereby improving the robustness of data fusion. Finally, a factor graph is constructed using head IMU data, leveraging its short-term, high-frequency stability and continuity to achieve global pose optimization. This maintains overall positioning accuracy and consistency even when other sensors are partially degraded.

[0024] The following is a detailed 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.

[0025] The implementation steps of this embodiment are as follows: Figure 1 and Figure 2 As shown, it includes steps 110 to 150:

[0026] Step 110 : Use multiple sensors of the robot to collect environmental data and perform preprocessing to obtain multiple odometers.

[0027] Specifically, the aforementioned multiple sensors include lidar, millimeter-wave radar, RGB-D camera, infrared thermal imager, head IMU, body part IMU, joint encoders, and plantar force sensors. The aforementioned multiple odometers include lidar odometer, millimeter-wave radar odometer, visual odometer, and motion inertial odometer.

[0028] In step 110, if Figure 2The sensor module and odometry generation algorithm module on the center left. The device uses multiple sensors (lidar, millimeter-wave radar, RGB-D camera, infrared thermal imager, head IMU, body IMU, joint encoders, and foot force sensors) to work together, using multi-threaded technology to synchronously collect and preprocess environmental data to generate multiple odometry data. These odometry data are then fused to obtain stable and accurate pose estimates for mapping and localization. The following is a detailed description of the data processing and fusion process for each sensor.

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

[0030] Specifically, after the LiDAR data is preprocessed, it mainly includes the point cloud denoising and registration process. 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). 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.

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

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

[0033] Specifically, the millimeter-wave radar data is first filtered (such as with a Kalman filter) to remove interference caused by environmental noise. The Doppler effect is then used to estimate the millimeter-wave radar velocity, generating initial radar velocity information. This initial velocity information is then processed using the Random Sample Consensus (RANSAC) algorithm to remove outliers and ensure data reliability. Finally, the RANSAC-processed radar velocity is fused with the acceleration and angular velocity data from the body's IMU to generate the millimeter-wave radar odometry. Millimeter-wave radar odometry data has a high frequency and strong environmental adaptability, providing more stable data in adverse weather conditions (such as heavy fog and heavy rain), effectively complementing the shortcomings of lidar in specific environments.

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

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

[0036] Specifically, the infrared imager data is first filtered (e.g., using median filtering) and histogram equalization to generate enhanced thermal infrared data. Next, the RGB-D camera data and thermal infrared data are aligned and feature matched (e.g., using the Oriented Fast and Rotated Brief (ORB) algorithm). Finally, the RGB-D camera and thermal infrared data are fused based on the feature matching results to generate the visual odometry. The RGB-D camera and thermal infrared imager work together: the RGB-D camera provides visible light information for environmental mapping by capturing color and depth images, while the thermal infrared imager supplements perception capabilities in low-light or no-light environments. In this way, the images provided by the RGB-D camera and thermal infrared imager complement each other, enhancing the robot's environmental perception capabilities, particularly in low-light or high-contrast environments, while maintaining high positioning accuracy.

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

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

[0039] Specifically, the joint encoders, plantar force sensors, and body IMUs constitute a motion inertial odometer, which is mainly used to estimate the dynamic state of the robot during motion. The joint encoders provide the robot's joint angle and angular velocity information, the plantar force sensors measure the contact force between the robot and the ground, and the body IMUs provide 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.

[0040] In general, the data collection and preprocessing process in step 110 can ensure that the device collects valid data in all scenarios. This ensures that when some sensors degrade due to insufficient light, noise interference, or extreme dynamic scenes, other sensors can still provide valid information, providing strong support for subsequent mapping.

[0041] Step 120 , performing a real-time health assessment on the plurality of odometers. After each real-time health assessment, deactivating unhealthy odometers based on the results of the health assessment, and selecting an odometer whose health meets the requirements from the results of the health assessment as a healthy odometer.

[0042] 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:

[0043] Step 121 : During each real-time health evaluation, degradation monitoring is performed on the plurality of odometers, and a degradation monitoring result is generated.

[0044] To achieve this, the system monitors the health of each sensor using different degradation detection methods during each real-time health assessment. These methods include:

[0045] 1. Monitor the characteristic values of the LiDAR odometer and the millimeter wave odometer. Figure 2 In the health monitoring module, the monitoring indicators submodule for both the LiDAR and millimeter-wave odometers uses eigenvalues and eigenvectors. Specifically, the degradation factors generated during the registration of feature point clouds are used to determine if the LiDAR and millimeter-wave odometers have failed or are significantly degraded. This method is 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.

[0046] 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. δx Represents state variables x In the disturbance δd If the degradation factor D If the value is less than a predefined threshold, it indicates that the lidar or millimeter-wave radar is severely degraded and the current point cloud registration result cannot reliably reflect the real environment. This predefined threshold can be derived through simulation analysis of degraded scenarios, ensuring effective detection of degradation under different environmental interference conditions.

[0047] 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.

[0048] 2. Monitor the visual odometry using the information matrix determinant. Figure 2 In the monitoring indicator submodule of the health monitoring module, the monitoring indicator of the visual odometry is the D-optimization method. Specifically, for the visual odometry, 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 magnitude of the information matrix determinant directly reflects the observability of the state parameters. A larger determinant indicates stronger observation capability; conversely, when the determinant approaches zero, it indicates that the visual odometry data is insufficient to constrain the state estimate, resulting in degradation. When the system detects a significant downward trend in the determinant or a persistently low value, the visual odometry is considered degraded.

[0049] 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.

[0050] 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 odometry is the posterior covariance matrix; the posterior covariance matrix indicators include eigenvalues, determinants, and condition numbers. Specifically, the motion inertial odometry uses a factor graph method to estimate the internal state of the humanoid robot. 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 velocity of the entire body. The factor graph method achieves state estimation by constructing a maximum a posteriori probability (MAP) problem, which can be expressed mathematically as follows:

[0051]

[0052] in, x is the robot state to be estimated (including joint angles, accelerations, angular velocities, etc.), z The factor graph models these sensor measurements and state variables, and ultimately optimizes the variables in the joint probability distribution to obtain the optimal solution. x * During the optimization process, the system will also obtain a posterior covariance matrix P , which is used to characterize the confidence distribution of state estimation.

[0053] If the observation constraints in some directions of the system are insufficient or there is noise interference, the posterior covariance matrix P Specifically, if the eigenvalue distribution shows excessively large eigenvalues in certain directions, it indicates insufficient observation information in that direction or sensor data degradation. In this case, the kinematic inertial odometry can be judged to be degraded.

[0054] 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 characteristics, so that the motion inertial odometry still has the ability to accurately detect degradation in more complex dynamic environments.

[0055] Step 122 : combining the degradation monitoring result with a general indicator to calculate the health of the multiple odometers.

[0056] Specifically, if Figure 2In the general indicator submodule of the health detection module, the system combines degradation monitoring results with a comprehensive score of general indicators, using a weighted strategy to quantitatively calculate the health of each odometry meter. General indicators include the sensor's acquisition frequency and pose change rate. The acquisition frequency reflects the sensor's response speed to environmental changes. If the acquisition frequency is too low, the system may not be able to capture subtle changes in the environment or during robot motion, resulting in delayed positioning results. Therefore, the acquisition frequency score is weighted with 10 points. The pose change rate reflects the sensor's adaptability during the robot's dynamic motion, and the pose change rate score is weighted with 5 points. If the pose change rate differs significantly from the actual motion, it indicates that the odometry data may be degraded or inaccurate.

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

[0058] Specifically, if 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. To completely eliminate the negative impact that may be caused by unhealthy odometers, 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 dashed line connecting the priority queue and processing logic modules with the odometry generation module represents the restart step. The system reinitializes the corresponding sensors, including extrinsic calibration of the lidar, updating the noise model of the millimeter-wave radar, resetting the ORB feature extraction or information matrix in the visual odometry, and reloading the initial state of the factor graph optimization component in the kinematic inertial odometry. This reinitialization process is accompanied by further health assessments until the odometry is detected to be in a usable state before it is allowed to re-enter the fusion process.

[0059] Step 124 : Select an odometer whose health status meets the requirements from the health status assessment results as a healthy odometer.

[0060] 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 health assessment results 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 top 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 can be retained when large-scale sensor degradation occurs.

[0061] Before this, if Figure 2 The predefined queue submodule in the priority queue and processing logic module can predefine a queue in the following order: lidar odometry, visual odometry, millimeter-wave radar odometry, and motion inertial odometry. This serves as a reference for odometry selection during initial startup.

[0062] Based on this design, in practical applications, if multiple sensors remain healthy, the optimal two sensors—high-precision sensors (such as lidar and visual odometry) and auxiliary sensors (such as millimeter-wave radar and kinematic inertial odometry)—can be mutually constrained, maintaining robust positioning performance in extreme environments or fast-moving scenarios. If sensor degradation is severe and only one healthy odometry remains in the system, that sensor can be used to temporarily maintain a basic pose estimate. Once the other odometry sensors are reinitialized and restored to health, it can be rejoined to the fusion queue to improve overall accuracy and stability.

[0063] In step 130 , the robot's head IMU data is used as a factor node and the health odometry is used as a factor constraint to construct a factor graph and generate a pose graph.

[0064] In step 130, if Figure 2 The selected healthy odometer data is used in the subsequent factor graph construction. Furthermore, the head IMU is selected as a factor node due to its relatively stable installation location and reduced dependence on the external environment. It can continue to provide relatively smooth and continuous attitude measurements in scenarios with visual or radar degradation. The details are as follows:

[0065] 1. Accuracy: IMUs provide smooth measurements that, despite the presence of noise, are generally not affected by too many outliers. This allows IMU state estimates to be very accurate within the constraints of other sensors such as vision and radar.

[0066] 2. Bias Constraints: IMU biases (such as those of the accelerometer and gyroscope) can be effectively constrained by using observations from other sensors, such as visual inertial odometry and LiDAR inertial odometry. This interaction makes the IMU state estimate more reliable.

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

[0068] like Figure 2 In the odometry selection module of the final state output module, after treating the IMU data as a factor node, the system selects the corresponding measurement results from the healthy odometry selected in step 120 as factors and adds them to the factor graph. The fusion process includes: first, the system performs time-series alignment on the data of the healthy odometry, maps the odometry measurements of the corresponding timestamps to the coordinate system where the IMU is located or the global coordinate system, and generates constraint factors in the factor graph. Secondly, for the lidar odometry and millimeter-wave radar odometry, the pose estimation or point cloud registration results they provide are used as constraint factors to supplement the spatial matching information between nodes in the factor graph; for the visual odometry, the relative pose of the visual odometry (or the constraint between key frames) is used as an observation to improve the accuracy of the pose estimation and its adaptability to the environment; for the kinematic inertial odometry, 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.

[0069] After the combined effect of the multi-sensor factors in 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 odometry, and ultimately solve the optimal posture of the robot at each moment under a unified optimization framework to obtain a globally consistent posture trajectory.

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

[0071] Specifically, the above-mentioned loop monitoring and optimization method includes: if the health odometry includes the lidar odometry or the millimeter-wave radar odometry, 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 odometry includes a visual odometry, the loop monitoring is performed on the current frame data and the loop frame data through the 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.

[0072] After entering step 140, the system will give priority to selecting the odometer in a healthy state to perform loop monitoring. Figure 2The loop detection module, if the health odometry includes a lidar or millimeter-wave radar odometry, registers the current frame's point cloud with the loop closure frame's point cloud using ICP (Iterative Closest Point) or a modified algorithm (such as N-ICP) to determine if the two are sufficiently similar to form a loop. After point cloud registration, if the alignment error is within an acceptable range, the current frame and the historical frame are in the same region. Otherwise, there is significant difference or noise between the current and historical frames, and the loop closure condition is not met. If the health odometry includes a visual odometry, the system uses RANSAC to match feature points between the current frame and the loop closure frame. 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 any suspicious loop candidate frames. If a sufficient number of matching feature points is present and the RANSAC-verified inlier ratio reaches a threshold, the current frame and the historical frame correspond to the same physical scene, satisfying the loop closure condition.

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

[0074]

[0075] 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:

[0076] 1. Correcting accumulated errors: During long-term operation, the positioning results of lidar odometry, millimeter-wave radar odometry, visual odometry, and other sensors may drift. The loop detection factor can correct this drift by applying closed-loop constraints.

[0077] 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 offset or deformation of overlapping areas.

[0078] 3. Optimizing the pose graph: In the factor graph optimization process, loop closure factors provide global constraints, improve the backend optimization's ability to correct pose estimation, and enhance the global pose optimization effect.

[0079] After geometrically verifying frames that satisfy loop closure constraints, the system injects these loop closure factors into the factor graph optimization process, further precisely correcting the global pose graph. This optimization process achieves global consistency between the robot's historical and current trajectories, correcting accumulated errors and significantly improving the overall map accuracy and scene understanding.

[0080] Step 150: After the robot completes the acquisition, a map is generated based on the pose graph.

[0081] Specifically, if Figure 2 In the map generation submodule of the final state output module, the system uses the factor graph optimization described above to obtain an optimized pose graph for each moment or keyframe (i.e., the pose of each sensor frame in the global reference coordinate system). These optimized poses are then used to consistently project each frame's environmental perception data (such as lidar point clouds, visual images, and depth information) into a unified coordinate system, ultimately generating a global map. Furthermore, the posterior poses obtained through global factor graph optimization not only correct for deviations caused by sensor noise and accumulated errors, but also maintain consistency across multiple visits to the same area, resulting in a globally coherent and accurate map output.

[0082] In this invention, multi-sensor data is first collected to generate LiDAR, millimeter-wave radar, vision, and motion inertial odometry. This ensures that even when some sensors degrade due to insufficient lighting, noise interference, or extreme dynamic scenes, other sensors can still provide valid information. Secondly, real-time health assessment automatically eliminates unstable data caused by sensor degradation or anomalies, ensuring that only healthy data is used to generate the pose graph, thereby improving the robustness of data fusion. Finally, a factor graph is constructed using head IMU data, and its short-term high-frequency stability and continuity are leveraged to achieve global pose optimization. Even if other sensors partially degrade, the accuracy and consistency of overall positioning can be maintained.

[0083] The steps of the above method are divided only for the purpose of clear description. During implementation, 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 within the scope of protection of the present invention. Adding insignificant modifications or introducing insignificant designs to the algorithm or process without changing the core design of the algorithm and process are all within the scope of protection of the invention.

[0084] 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 places in the specification does not necessarily refer to the same embodiment, nor does it mean that it is an independent or alternative embodiment that is mutually exclusive with other embodiments. It will be understood by those skilled in the art that the embodiments described herein can be combined with other embodiments.

[0085] Another embodiment of the present invention relates to an electronic device, such as Figure 4 As shown,

[0086] The system includes at least one processor 501; and a memory 502 in communication with 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 to enable the at least one processor 501 to execute the above-mentioned mapping method based on multi-sensor fusion.

[0087] The memory 502 and processor 501 are connected using a bus. The bus can include any number of interconnected buses and bridges, connecting various circuits of one or more processors 501 and memory 502. The bus can also connect various other circuits such as peripheral devices, voltage regulators, and power management circuits. These are all well known in the art and are therefore not described further herein. The bus interface provides an interface between the bus and the transceiver. The transceiver can be a single component or multiple components, such as multiple receivers and transmitters, providing a unit for communicating with various other devices over a transmission medium. Data processed by the processor 501 is transmitted over a wireless medium via an antenna. Furthermore, the antenna receives data and transmits it to the processor 501.

[0088] 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.

[0089] 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.

[0090] That is, those skilled in the art will understand that all or part of the steps in the above-mentioned embodiments can be implemented by instructing the relevant hardware through a program. The program is stored in a storage medium and includes a number of instructions for causing a device (which may be a single-chip microcomputer, chip, etc.) or a processor to execute all or part of the steps of the methods described in the various embodiments of this application. The aforementioned storage medium includes: a USB flash drive, a mobile hard drive, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk, etc., various media that can store program code.

[0091] 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 thereto 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: Using the robot's multiple sensors to collect environmental data and pre-process 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; wherein the multiple sensors include at least a head IMU; performing a real-time health assessment on the plurality of odometers, and after each real-time health assessment, deactivating unhealthy odometers based on the results of the health assessment, and selecting an odometer that meets the health requirements according to the results of the health assessment as a healthy odometer; The data of the head IMU of the robot is used as a factor node, and the health odometry is used as a factor constraint to construct a factor graph and generate a pose graph; After the robot completes the acquisition, generating a map based on the pose graph; Among them, in the result of the health assessment, if the number of odometers whose health meets the preset threshold is greater than or equal to two, the top two odometers with the highest health are selected as healthy odometers; in the result of the health assessment, 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.

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 closure monitoring based on the health odometer and optimizing the pose graph based on the result of the loop closure 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, performing the loop monitoring 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 also includes a lidar, a millimeter-wave radar, an RGB-D camera, an infrared thermal imager, a body IMU, a joint encoder, and a plantar force sensor; The method uses multiple sensors to collect environmental data and pre-process it to obtain multiple odometers, including: Performing point cloud registration on the laser radar data based on normal information to generate the laser radar odometry; fusing the data of the millimeter-wave radar with the data of the body IMU to generate the millimeter-wave radar odometer; fusing data from the RGB-D camera and the infrared thermal imager to generate the visual odometry; 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, 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; Perform Doppler velocity estimation on the denoised millimeter-wave radar data to generate the initial radar velocity; removing outliers from the initial radar velocity using a random sampling consistency algorithm to generate a final radar velocity; 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 assessment on the plurality of odometers and, after each real-time health assessment, disabling unhealthy odometers based on the results of the health assessment, includes: During each real-time health assessment, 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 status of the multiple odometers each time, the odometer whose health status is lower than a preset threshold among the multiple odometers is set as an unhealthy odometer, the unhealthy odometer is deactivated and the historical data of the unhealthy odometer is cleared, and the sensor corresponding to the unhealthy odometer is reinitialized and the health status 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 performing degradation monitoring on the plurality of odometers includes: Performing characteristic value monitoring on the laser radar odometer and the millimeter wave radar odometer; Performing information matrix determinant monitoring on the visual odometry; The motion inertial odometer is monitored for a posteriori covariance matrix indicators, wherein the a posteriori covariance matrix indicators include eigenvalues, determinants, and condition numbers.

8. 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 7.

9. 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 7 is implemented.

Images