Specific scene elevation fine construction method based on multi-source heterogeneous probability fusion

Abstract

The invention relates to a specific scene elevation fine construction method based on multi-source heterogeneous probability fusion. The method comprises the following steps: synchronously acquiring multi-source data through an RGB sensor, a binocular infrared depth sensor and an inertial measurement unit; performing instance segmentation and depth aggregation on the RGB color image to obtain a scene instance segmentation result and first depth data; reconstructing an initial point cloud based on the binocular infrared depth image; carrying out pose calculation through a vision-inertia odometer, coupling loopback detection to optimize a camera track, and outputting a global consistent pose; fusing the first depth data with an initial point cloud to generate a segmentation point cloud with instance information; and finally, generating a cross-modal enhanced fusion point cloud based on the segmentation point cloud, performing multi-source heterogeneous probability fusion and Kalman filtering in combination with a global consistent pose, and outputting a decoupling elevation fine composition capable of distinguishing a low object scene. According to the method, the understanding of a scene environment is effectively enhanced, and the intelligence level of terrain elevation data generation is remarkably improved.

Description

Technical Field

[0001] This invention belongs to the field of robot environmental perception technology, and in particular relates to a method for precise elevation mapping of specific scenarios based on multi-source heterogeneous probabilistic fusion. Background Technology

[0002] In autonomous mobile robots, especially legged robots or robotic systems that need to operate in complex, unstructured environments, accurate perception and 3D reconstruction of the surrounding environment are prerequisites for achieving motion control. Currently, one of the mainstream technical solutions is to construct digital elevation maps using dense point cloud data acquired by 3D sensors (such as LiDAR or depth cameras). This elevation map discretizes the 3D space into a 2D grid, with each grid cell storing one or more elevation information points within that area, such as the highest point, lowest point, or average height. Based on this elevation map, the robot can assess the accessibility, stability, and roughness of the terrain, thereby planning safe foot contact points or travel trajectories.

[0003] However, existing elevation map-based navigation and planning methods have revealed several inherent and pressing technical bottlenecks in practical applications: elevation maps only focus on the height of the terrain, completely ignoring the physical properties of objects, such as material, stiffness, and stability. This causes robots to be unable to distinguish between low, solid platforms and fragile obstacles of the same height (such as cardboard boxes or tool trays), potentially placing support points on unsafe surfaces during planning, leading to robot instability or even tipping over, seriously threatening equipment and environmental safety; most systems rely excessively on single LiDAR data, failing to effectively integrate information from heterogeneous sensors such as RGB images and infrared depth sensors. The rich visual texture and color information provided by image sensors is crucial for determining the material of objects, and the effective fusion of multi-source information is key to improving the robustness of environmental models, especially their adaptability under special lighting conditions or when facing challenging surfaces such as transparent or highly reflective surfaces.

[0004] In summary, existing technologies have significant shortcomings in handling low-lying obstacles, deeply fusing multi-source information, and effectively utilizing scene instances. Therefore, there is an urgent need in this field for a novel environmental characterization method that can deeply integrate high-precision geometric information with physical and instance attributes to significantly improve the autonomous navigation capability, robustness, and safety of robots in complex environments. Summary of the Invention

[0005] The technical problem to be solved by the present invention is to provide a method for precise elevation construction in a specific scenario based on multi-source heterogeneous probabilistic fusion, which addresses the deficiencies in the existing technology.

[0006] The technical solution to achieve the purpose of this invention is as follows: A method for refining the elevation of a specific scene based on multi-source heterogeneous probabilistic fusion includes (1) a data acquisition and preprocessing module, (2) an instance depth estimation module, (3) a three-dimensional geometric reconstruction module, (4) a pose estimation module, and (5) a multi-source heterogeneous information fusion and elevation refinement module.

[0007] (1) Data acquisition and preprocessing module: An integrated multimodal sensing module is enabled, which includes: an RGB image sensor for capturing scene visual texture information; a binocular infrared depth sensor for analyzing the physical geometry of each point in the scene, which generates a binocular infrared depth image through a stereo matching algorithm, and simultaneously constructs an inherent uncertainty map characterizing the reliability of each depth value based on physical models such as matching confidence and measurement distance; and an inertial measurement unit for calculating the six-degree-of-freedom kinematic state of the sensor carrier; rigid registration and timestamp alignment of the spatiotemporal reference are performed on the heterogeneous and asynchronous observation data (including RGB color image, binocular infrared depth image, inherent uncertainty map and inertial measurement unit) output by the aforementioned sensors, thereby generating a spatiotemporally synchronized multimodal time series data frame with deterministic delay in a unified coordinate system; finally, this enhanced data frame is used as parallel input and synchronously distributed to the downstream three functional modules (2) instance depth estimation, (3) three-dimensional geometric reconstruction and (4) pose estimation.

[0008] (2) Instance Depth Estimation Module: Obtain a frame of synchronously aligned RGB color image and binocular infrared depth image from the spatiotemporally synchronized multimodal time series data of the data acquisition and preprocessing module, and obtain the depth scale factor depth_scale corresponding to the binocular infrared depth image for unit conversion; input the RGB color image into a preset target detection and instance segmentation network, the network processes the image to identify one or more target instances in the image, and outputs its category label, detection confidence, bounding box coordinates that define its two-dimensional position, and generates a pixel-level segmentation mask for each identified target instance, the mask is used to accurately distinguish the foreground pixels and background pixels of the target instance;

[0009] Using the generated segmentation mask as a spatial index, extract all valid depth values ​​that correspond to the foreground pixels of the target instance and are greater than zero from the binocular infrared depth image; perform a preset average aggregation operation on the extracted set of valid depth values ​​to obtain a robust aggregated depth value; multiply the aggregated depth value by the obtained depth scaling factor depth_scale and perform unit conversion to calculate the first depth data that can characterize the three-dimensional spatial position of the target instance;

[0010] Finally, the category label, detection confidence, bounding box coordinates, segmentation mask, and calculated first depth data of the target instance are encapsulated into a structured instance target data object, and the data object is output to the multi-source heterogeneous information fusion and elevation refinement module.

[0011] (3) Three-dimensional geometric reconstruction module: Obtain a frame of binocular infrared depth image that is synchronously aligned with the RGB color image from the spatiotemporal synchronous multimodal time series data of the data acquisition and preprocessing module, and obtain the depth scaling factor used to convert the depth unit into the measurement unit, as well as a set of preset camera intrinsic parameter matrix parameters containing focal length and principal point coordinates.

[0012] The pixel coordinates of the binocular infrared depth image are traversed, and for each pixel (u, v), the following operations are performed: the original depth value of the pixel is multiplied by the depth scaling factor to obtain the depth value in the measurement unit; the depth value is validated, the validation including at least removing invalid depth values ​​less than or equal to zero and exceeding a preset maximum distance threshold; for valid depth values ​​that pass the validation, the camera inverse projection model is applied, and based on the camera intrinsic parameter matrix parameters, the two-dimensional pixel coordinates (u, v) and their corresponding valid depth values ​​are solved into three-dimensional spatial point coordinates (X, v) in the camera coordinate system. cam Y cam Z cam ); encapsulate all generated 3D spatial point coordinates into a structured initial 3D point cloud; wherein, the 3D spatial point coordinates are stored in floating-point format.

[0013] (4) Pose estimation module: Receives the RGB color image, binocular infrared depth image and raw measurement data from the inertial measurement unit as input data, and performs timestamp synchronization, sensor extrinsic parameter calibration and gravity direction alignment on the input data to ensure global spatiotemporal consistency among the data sources;

[0014] Based on the original measurement data of the inertial measurement unit, the inertial measurement unit pre-integration model is applied to predict the relative motion of the sensor between two observation times, and the inertial measurement unit measurement residual term is constructed.

[0015] Two-dimensional visual feature points are extracted from the RGB color image and combined with the binocular infrared depth image. The feature points are then converted into three-dimensional spatial points through a camera inverse projection model, thereby constructing the visual observation residual term.

[0016] The loop closure detection process includes: comparing the two-dimensional visual feature points extracted from the RGB color image at the current observation time with the two-dimensional visual feature points stored at each observation time along the historical path to identify the overlap between the current position and the historical position; if an overlap is identified, it is determined that the current observation time and the historical observation time constitute a loop closure pair, and the closed-loop constraint information that can characterize the relative pose transformation between the loop closure pair is calculated, thereby constructing a global closed-loop constraint residual term based on the loop closure detection result;

[0017] A nonlinear optimization objective function is constructed and solved, which includes the measurement residual term of the inertial measurement unit, the visual observation residual term, and the global closed-loop constraint residual term based on the loop closure detection result, to obtain a globally optimal state vector containing the sensor position, attitude, velocity, and inertial measurement unit bias. Based on the globally optimal state vector, a globally spatiotemporally consistent sensor trajectory pose with centimeter-level accuracy and global spatiotemporal consistency is generated and output to the multi-source heterogeneous information fusion and elevation refinement module as a spatial reference benchmark for subsequent processing.

[0018] (5) Multi-source heterogeneous probabilistic fusion and elevation refinement module: First, based on the globally consistent spatiotemporal consistency of the sensor trajectory pose (T) wb ) and the external parameters between the camera and the IMU (T bc ), through coordinate system transformation P world =T wb ·T bc ·P camera The original point cloud in the camera coordinate system is remapped to the world coordinate system to obtain the remapped point cloud, where P world P represents the coordinate position of the point cloud in the world coordinate system. camera Point cloud coordinates representing the camera coordinate system; instance segmentation mask By projecting from 3D to 2D, instance labels are assigned to the remapped point cloud, generating one or more segmented point clouds. The segmented point cloud is divided into background point cloud and obstacle point cloud, and its generation process can be defined by the following model:

[0019]

[0020] Where π(·) is the projection function, τ seg T is the preset segmentation threshold. cb It is T bc The inverse transform, i.e., T cb =(T bc ) -1 ;

[0021] Secondly, a multi-source uncertainty model is constructed, which at least considers the depth uncertainty determined by the characteristics of the binocular infrared depth sensor. and instance uncertainty determined by the confidence of the segmentation network Based on the uncertainty model, a dynamic fusion weight w is generated for each data point. i Finally, by weighted summation, the multiple segmented point clouds are subjected to multi-source heterogeneous probabilistic fusion to generate a cross-modal enhanced fused point cloud P. fusion ;

[0022] An extended Kalman filter is applied to the cross-modal enhanced fusion point cloud to continuously update the elevation values ​​and their corresponding uncertainty variances, thereby generating a base elevation map containing an elevation layer and an uncertainty layer. The positions of the separated obstacle point clouds are projected onto the generated base elevation map, and an independent obstacle differentiation layer is generated to mark the positions of the dynamic obstacles. Finally, the elevation layer, uncertainty layer, and obstacle differentiation layer are integrated to output a scene decoupled elevation refinement map that accurately depicts the scene height.

[0023] The system of the method includes: a data acquisition and preprocessing module, which includes multimodal observation data including RGB color images, binocular infrared depth images and inertial measurement unit data, and performs spatiotemporal reference registration and timestamp alignment on the data to generate spatiotemporally synchronized data frames in a unified coordinate system;

[0024] A parallel information processing processor is configured to receive data frames from the data acquisition and preprocessing module and execute the following information processing units in parallel: an instance depth estimation unit for processing the RGB color image to generate target information with instance geometry annotation, the target information including at least the segmentation mask, detection confidence, and first depth data of each target instance; a 3D geometry reconstruction unit for constructing an initial 3D point cloud based on the depth image through coordinate system remapping; and a pose estimation unit for fusing the RGB image, binocular infrared depth image, and inertial measurement unit data, and calculating and outputting a globally spatiotemporally consistent sensor trajectory pose with global spatiotemporal consistency through visual-inertial odometry and loop closure detection optimization.

[0025] A multi-source heterogeneous information fusion and elevation refinement module is configured to receive the output from the parallelized information processing processor and, based on an uncertainty model, dynamically weight and fuse the first depth data in the target information with instance annotations with the initial 3D point cloud to generate a cross-modal enhanced fused point cloud. Based on the cross-modal enhanced fused point cloud and the sensor trajectory pose with global spatiotemporal consistency, an extended Kalman filter is used to finally construct and output a scene-decoupled elevation refinement map.

[0026] Compared with the prior art, the advantages of the present invention are as follows:

[0027] 1) An augmented reality map incorporating physical attributes was constructed, significantly improving safety. This invention goes beyond the pure geometric information of traditional elevation maps, instead deeply fusing multimodal perception data. This enables the robot to accurately distinguish between low, sturdy support surfaces and fragile obstacles, fundamentally avoiding the risk of instability or tipping over due to misjudgment, and greatly enhancing the robot's operational safety and robustness in complex human-robot coexistence environments.

[0028] 2) This invention achieves deep coupling and probabilistic fusion of multi-source heterogeneous information, improving the accuracy and robustness of environmental perception. A novel fusion framework is designed to perform pre-processing deep fusion of visual texture and color information from RGB cameras, as well as advantageous information from other sensors. This method overcomes the limitations of single sensors, especially in challenging scenarios such as varying lighting conditions and complex object surface materials. It can generate more accurate and complete environmental representations, significantly improving the all-weather, all-scene adaptability of perception.

[0029] 3) By deeply embedding high-dimensional instance understanding into the geometric map construction process, true scene cognition is achieved. This invention uses instance information as prior knowledge, integrating it pre-processed and deeply into the elevation map construction stage. This instance-driven mapping approach enables robots to move beyond simply seeing objects to understanding scenes, making it easier for them to make intelligent decisions that align with human logic and physical common sense. This is a crucial step towards higher-level autonomy. Attached Figure Description

[0030] Figure 1 This is an architecture diagram of a specific scene elevation refinement method based on multi-source heterogeneous probabilistic fusion according to the present invention;

[0031] Figure 2 This refers to the results of obstacle detection in the scene.

[0032] Figure 3 This represents the initial 3D point cloud of the scene.

[0033] Figure 4 Enhanced point cloud fusion for cross-modal scene visualization;

[0034] Figure 5 Decouple the scene and refine the elevation map; Detailed Implementation

[0035] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and preferred embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without inventive effort are within the scope of protection of this invention.

[0036] This invention provides a method for high-precision elevation reconstruction of specific scenarios based on multi-source heterogeneous probabilistic fusion. The core of this invention lies in a tightly coupled, uncertainty-aware fusion framework that, through a parallel processing architecture, collaboratively operates multiple core functional modules to achieve high-precision elevation reconstruction of specific scenarios, such as... Figure 1 As shown.

[0037] In this embodiment, the integrated multimodal sensing module uses a RealSense D435i depth camera manufactured by Intel Corporation. The camera integrates:

[0038] The system comprises an RGB image sensor, a pair of infrared image sensors for stereo vision (i.e., the binocular infrared depth sensor), and a high-performance inertial measurement unit (IMU) containing a three-axis accelerometer and a three-axis gyroscope. This sensing module is connected via a USB 3.1 interface to an NVIDIA Jetson AGX Orin embedded computing platform, which serves as the main processor. The entire system operates on a software framework based on the ROS 2 robotics operating system.

[0039] The method described in this invention comprises five main modules: (1) data acquisition and preprocessing module, (2) instance depth estimation module, (3) three-dimensional geometric reconstruction module, (4) pose estimation module, and (5) multi-source heterogeneous information fusion and elevation refinement module.

[0040] (1) Data Acquisition and Preprocessing Module

[0041] Initialization and Startup: After the system powers on, the realsense-ross driver node for ROS2 is started on the Jetson AGX Orin platform. This node initializes and activates all sensors of the D435i camera. The RGB image sensor is configured to output a 640x480 resolution color image at a frequency of 30Hz. The depth sensor, through its built-in ASIC processing unit, generates a 16-bit binocular infrared depth image with a resolution of 640x480 at a frequency of 30Hz based on a stereo matching algorithm for infrared images. Simultaneously, the inertial measurement unit is configured to output inertial measurement data at frequencies of 200Hz (gyroscope) and 63Hz (accelerometer).

[0042] Synchronous Construction of the Inherent Uncertainty Map: Simultaneously with the generation of the binocular infrared depth image, the RealSense SDK outputs a confidence value for each depth pixel, quantifying the reliability of stereo matching. In this embodiment, a custom ROS2 node subscribes to the original depth map and confidence map, and based on a preset physical model, combines the depth value d of each pixel with its corresponding confidence value c to calculate the uncertainty σ at that point. 2 Model σ is adopted. 2 =α·d 2 +(1-c)·γ, where α and γ are pre-calibrated noise coefficients. The uncertainty values ​​of all pixels together constitute an intrinsic uncertainty map with the same size as the depth map.

[0043] Timestamp Alignment: The D435i camera supports hardware-level timestamp synchronization, ensuring a highly consistent time base for RGB color images, binocular infrared depth images, and inertial measurement unit (IMU) data streams. Within the ROS 2 framework, a message synchronization node subscribes to the aforementioned RGB color image, binocular infrared depth image, and IMU topics. This node packages the data sets with the closest timestamps (within a 5-millisecond error range) into a single synchronized data set.

[0044] Spatial reference registration: The RGB sensor, depth sensor, and inertial measurement unit are physically rigidly connected, and their relative positions and attitude relationships (i.e., extrinsic parameters T) are also considered. bc It has been precisely calibrated at the factory. This extrinsic parameter is embedded in a URDF file and broadcast via ROS2's TF2 coordinate transformation system. This allows any node in the system to query the precise transformation relationship between any two sensor coordinate systems at any time.

[0045] Data Frame Encapsulation and Distribution: After spatiotemporal alignment, a synchronized set of RGB color images, binocular infrared depth images, and inertial measurement unit data is encapsulated into a custom enhanced multimodal temporal data frame message containing all data content. This message is published through a unified ROS2 topic. The three downstream functional modules (2) instance depth estimation module, (3) 3D geometry reconstruction module, and (4) pose estimation module all act as independent ROS2 nodes, subscribing to this topic to receive parallel, fully preprocessed data inputs to perform their respective independent computation tasks.

[0046] (2) Instance Depth Estimation Module

[0047] The instance depth estimation module is implemented as a standalone ROS2 node. Upon initialization, this node loads a pre-trained instance segmentation model based on a self-built SwiftDeNet architecture. This model uses weights trained on a low-obstacle dataset and is able to identify various common targets and generate corresponding segmentation masks. The computing platform remains the aforementioned NVIDIA Jetson AGX Orin, utilizing its GPU for model inference acceleration.

[0048] Data Subscription and Input: Nodes subscribe to topics published by the data acquisition and preprocessing module. Once a data frame containing a synchronized RGB color image, a binocular infrared depth image, and a depth scale factor (depth_scale, provided by the D435i camera in this embodiment, with a value of 0.001) is received, a processing flow is triggered.

[0049] Object detection and segmentation: The RGB color image extracted from the data frame is input into the loaded SwiftDeNet network for a forward propagation. The network outputs a series of detected object instances. For each successfully identified object, the system parses the following four pieces of information: class label, detection confidence, bounding box coordinates, and segmentation mask.

[0050] Depth extraction and calculation based on segmentation masks: For each target instance and its corresponding segmentation mask obtained in the previous step, the system performs the following depth calculation:

[0051] The segmentation mask of this instance is used as a spatial index filter. This filter is applied to the synchronously received binocular infrared depth image, extracting only the depth values ​​corresponding to all mask values ​​of 1 (i.e., target foreground pixels), forming a depth value array. This depth value array is then filtered for validity, removing all invalid readings with values ​​of 0 or less, as these typically indicate a lack of depth information or sensor errors. To improve robustness against edge noise, this embodiment performs an average aggregation operation on the filtered valid depth value array to obtain a more stable aggregated depth value, mean_raw_depth. A depth scaling factor is applied for unit conversion to calculate the final 3D depth of this instance: First depth data (meters) = mean_raw_depth * depth_scale.

[0052] Structured data encapsulation and output: The category label, detection confidence, bounding box coordinates, calculated first depth data, and segmentation mask of each target instance obtained in the preceding steps are uniformly encapsulated into a custom ROS2 message type, where the detection results are as follows: Figure 2As shown. This message instance constitutes a complete instance target data object. All generated instance target data objects are organized into a list and published through a new ROS2 topic for the multi-source heterogeneous information fusion and elevation structure module to subscribe to and perform subsequent processing.

[0053] (3) 3D Geometric Reconstruction Module

[0054] Module Configuration and Input: The 3D geometric reconstruction module is implemented as an independent ROS2 node. This node subscribes to the topic published by the data acquisition and preprocessing module to obtain synchronized and aligned RGB color images, binocular infrared depth images, and depth scale factors. In addition, when starting up, this node loads the internal parameter matrix of the D435i camera (including focal length f x = 615, f y = 615 and principal point c x = 320, c y = 240) from a YAML configuration file.

[0055] Initial 3D Point Cloud Generation: Whenever a new frame of data is received from the input topic, the node first extracts the 16-bit binocular infrared depth image and the corresponding RGB color image. To balance computational efficiency and point cloud density, in this embodiment, a downsampling strategy with a step size of 2 is adopted, that is, every other pixel is processed.

[0056] Coordinate Inverse Projection Solution: In this embodiment, the NumPy library is used for efficient vectorized calculation. First, a two-dimensional grid with the same size as the downsampled image is created to obtain the set of coordinates (u, v) of all pixels to be processed. The original depth values corresponding to these pixel coordinates are extracted and multiplied by the depth scale factor depth_scale to obtain the real depth value depth in meters. Pixel points with depth values outside the preset range (e.g., 0.2 < depth < 8.0 m) are all filtered out to eliminate near noise and unreliable measurements in the distance. For all valid pixel points that pass the verification, the camera pinhole inverse projection model is applied for batch coordinate conversion:

[0057]

[0058] Through this calculation, the set of three-dimensional coordinates (X cam , Y cam , Z cam ) of all valid points in the camera coordinate system is obtained.

[0059] Color information assignment and encapsulation: For each generated 3D point, its original 2D pixel coordinates (u, v) are used to index the corresponding (R, G, B) color value in the synchronized RGB color image. Then, the 3D coordinates (three 32-bit floating-point numbers) and RGB color information (packed into a 32-bit integer through bitwise operations) are encapsulated together into a structured initial 3D point cloud conforming to the ROS2 message format. This module ultimately publishes the data; the initial 3D point cloud of the scene is shown below. Figure 3 As shown.

[0060] (4) Pose estimation module

[0061] In this embodiment, the module is implemented as a tightly coupled visual-inertial odometry system based on sliding window graph optimization, which can fuse multi-source data and calculate sensor trajectory pose with global spatiotemporal consistency in real time.

[0062] Framework and State Definition: The pose estimation module runs as a standalone ROS2 node: vio_slam__node, using Ceres Solver as the backend nonlinear optimization library. The system needs to estimate the state vector x in real time, defined as:

[0063] x = [p] wb q wb v w b a b g ] T

[0064] This vector contains the position p of the sensor body relative to the world coordinate system. wb , attitude quaternion q wb The velocity v in the world coordinate system w And the accelerometer bias b of the inertial measurement unit a and gyroscope bias b g .

[0065] Feature processing and constraint construction: Feature points are extracted from each frame of RGB image using the ORB algorithm and tracked using the KLT optical flow method. For each successfully tracked feature point, its 3D coordinates are recovered using synchronized binocular infrared depth image information. These visual observations constitute the visual reprojection constraint in the optimization problem. This constraint aims to minimize the visual reprojection error r. v This is achieved by defining the deviation between the theoretical projected position and the actual observed position:

[0066]

[0067] in, For the k-th 3D point The actual pixel observations on the j-th frame image, where π(·) is the camera projection function, and T wb This refers to the sensor pose.

[0068] Motion constraint generation: The system collects all raw inertial measurement unit measurement data (acceleration) between two keyframes. and angular velocity ω t The system calculates a relative motion constraint using pre-integration theory of the inertial measurement unit. This constraint summarizes the pose, velocity, and offset changes between two keyframes, and its core incremental calculation includes:

[0069] Attitude increment:

[0070] Speed ​​increment:

[0071] Position increment:

[0072] These increments form the measurement constraints of the inertial measurement unit in the optimization problem.

[0073] Sliding window graph optimization: The core of the backend is to maintain a sliding window containing N latest keyframes, and to jointly optimize all states within the window by solving a nonlinear least squares problem. Its objective function is:

[0074] This function jointly minimizes the three residuals:

[0075] Prior residual (||r) p -H p x|| 2 ): Information from the old state of the window that has been marginalized and moved out of the window.

[0076] Inertial measurement unit measurement residual (∑||r) B || 2 ): From the pre-integration constraint of the inertial measurement unit.

[0077] Visual observation residual (∑||r) c || 2 ): From visual reprojection constraints.

[0078] in The square of the Mahalanobis norm, where P is the covariance matrix. H: Hessian matrix or information matrix.

[0079] To keep computational costs constant, the oldest keyframes are marginalized when a new keyframe is added to the window. This process is mathematically implemented using the Schur complement, and the updated information matrix takes the following form:

[0080]

[0081] Loop closure detection and global optimization: A parallel loop closure detection thread uses DBoW2 for scene recognition. When the scene similarity between the current keyframe and a historical keyframe exceeds a threshold, a loop closure is confirmed. At this point, the system adds a pose graph constraint between these two non-contiguous keyframes and triggers a global pose graph optimization to eliminate accumulated trajectory drift.

[0082] This module outputs a globally optimized, spatiotemporally consistent sensor trajectory pose with centimeter-level accuracy. It provides a unified and reliable spatial reference benchmark for all downstream modules.

[0083] (5) Multi-source heterogeneous information fusion and elevation precision module

[0084] Module and Data Flow Configuration: The input to this process comes from multiple upstream modules: Initial 3D Point Cloud (P camera ); Instance segmentation mask Sensor pose (T) wb ) and external parameters (T) bc (and the static extrinsic parameters between the camera and the inertial measurement unit body provided by the TF2 coordinate transformation system.)

[0085] Coordinate system remapping: When a node receives a new initial 3D point cloud, it first converts it from the ROS message format to the PointCloud data structure in the PCL library for efficient geometric calculations. The node then queries the TF2 system to obtain the global pose transformation matrix T that is closest to the timestamp of the point cloud. wb and a fixed extrinsic transformation matrix T bc The complete transformation matrix T is calculated using matrix multiplication. wc =T wb ·T bc The entire point cloud P camera The mathematical model for accurately transforming from the camera coordinate system to the world coordinate system is as follows:

[0086] P world =T wc ·P camera

[0087] The output of this step is a dense point cloud P in a globally uniform coordinate system. world .

[0088] Instance label assignment: The goal of this step is to assign a label to P. world Each 3D point in the image is assigned a label to its corresponding object category. The system first integrates all received instance segmentation masks. A temporary label map with the same resolution as the original RGB image is constructed in memory. The value of each pixel in the map corresponds to its instance ID. The system then assigns a label to P. world For each 3D point p in the matrix, an inverse projection operation is performed. This operation first involves an inverse transformation T. cb =(T bc ) -1 Transform point p from the world coordinate system back to the camera coordinate system, and then calculate its pixel coordinates (u, v) on the 2D image by applying the projection function π(·) using the camera intrinsic parameters. Use the calculated pixel coordinates to index the previously constructed "label map" and query the instance ID to which the point belongs.

[0089] The mathematical model for this process can be summarized as follows: For each instance i, generate its corresponding segmentation point cloud. This includes all the masks projected onto it. Three-dimensional points within the range:

[0090]

[0091] Where, τ seg A preset segmentation threshold is used (0.5 in this embodiment). All successfully labeled 3D points are integrated to form a complete segmented point cloud with rich instance information. Each point in the point cloud contains not only XYZ coordinates and RGB color, but also an additional field to store its instance ID or category label.

[0092] Multi-source uncertainty modeling: To assess the reliability of each input 3D data point, the system first constructs a multi-source uncertainty model. This model is used for each 3D point p. i Calculate the overall uncertainty variance. The variance is weighted by the following two main components:

[0093] Deep uncertainty This uncertainty stems from the physical limitations of the depth sensor itself. This embodiment employs a quadratic polynomial model to model it, which is obtained by fitting the sensor's factory calibration data:

[0094]

[0095] Where d is the depth value at that point (in meters), and c1 and c2 are pre-calibrated sensor-specific noise figures. This model reflects the physical characteristic that the error in depth measurement increases quadratically with increasing distance.

[0096] Instance uncertainty This uncertainty stems from the identification confidence of the upstream instance segmentation network. For a segmentation confidence of P...conf The uncertainty of a point is modeled as follows:

[0097]

[0098] Here, λ and κ (κ=2) are preset hyperparameters. The design of this model ensures that points with higher segmentation confidence introduce less instance uncertainty; conversely, points with low confidence correspond to extremely high uncertainty.

[0099] The total uncertainty at each point is calculated as a weighted sum of these two parts: α is a fusion factor used to balance the importance of geometric and instance information.

[0100] Generation of dynamic fusion weights: Based on the above uncertainty model, the system generates dynamic fusion weights for each data point p. i Generate a dynamic fusion weight w i (t). This weight is a comprehensive indicator used to determine the discourse power of this data point in subsequent fusion steps, and its calculation formula is as follows:

[0101]

[0102] The formula is obtained by multiplying three parts: the uncertainty term. The reciprocal of the total uncertainty variance of a point. The smaller the uncertainty, the higher the weight. Spatial proximity term: a Gaussian decay function, where P ref It is a reference point, σ spatial This is the spatial decay bandwidth. It gives higher weight to data closer to the reference point. The time freshness term: an exponential decay function, where t... current -t i It represents the age of the data points, T. time It is the time decay constant. It gives higher weight to newer data.

[0103] Weighted fusion and cross-modal point cloud generation: The goal of this step is to generate a cross-modal enhanced fused point cloud P. fusion This point cloud serves as the direct input for subsequent elevation map construction. Cross-modal augmented fusion of point clouds, such as... Figure 4 As shown.

[0104] Elevation state estimation based on extended Kalman filter: The system independently applies an extended Kalman filter to each cell of the internally maintained 2D rasterized elevation map for temporal state estimation, continuously updating the elevation values ​​and corresponding uncertainty variances of the cross-modal enhanced fusion point cloud, thereby generating a base elevation map containing an elevation layer and an uncertainty layer. State vector definition: For any raster cell in the map, its state vector x at time k is... kDefined as:

[0105] x k =[h grid , σ terrain ] T

[0106] This vector contains not only the grid elevation value h of the cell. grid It also includes the standard deviation σ describing local terrain features. terrain .

[0107] Scene decoupling and static background separation: Before outputting the final map, this embodiment performs scene decoupling once, projects the positions of the separated obstacle point clouds onto the generated base elevation map, and generates an independent obstacle differentiation layer to mark the positions of the dynamic obstacles; finally, the elevation layer, uncertainty layer and obstacle differentiation layer are integrated to output a scene decoupling elevation fine-mapping map that accurately depicts the scene height.

[0108] Through the detailed process described in this embodiment, the system not only effectively fuses multi-source data with temporal characteristics through extended Kalman filtering, but also generates an accurate, reliable, and downstream task-friendly elevation-determined map through scene decoupling. The scene-decoupled elevation-determined map is shown below. Figure 5 As shown.

[0109] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Any aspects of the present invention not described in detail are well-known techniques to those skilled in the art.

[0110] The preferred embodiments of the present invention have been described in detail above. It should be understood that those skilled in the art can make numerous modifications and variations based on the concept of the present invention without creative effort. Therefore, all technical solutions that can be obtained by those skilled in the art based on the concept of the present invention through logical analysis, reasoning, or limited experimentation on the basis of existing technology should be within the scope of protection defined by the claims.

Claims

1. A method for refining elevation in a specific scenario based on multi-source heterogeneous probabilistic fusion, characterized in that, It includes (1) a data acquisition and preprocessing module, (2) an instance depth estimation module, (3) a 3D geometric reconstruction module, (4) a pose estimation module, and (5) a multi-source heterogeneous information fusion and elevation refinement module. (1) Data acquisition and preprocessing module: An integrated multimodal sensing module is enabled, which includes: an RGB image sensor for capturing scene visual texture information; a binocular infrared depth sensor for analyzing the physical geometry of each point in the scene, which generates a binocular infrared depth image through a stereo matching algorithm, and simultaneously constructs an inherent uncertainty map characterizing the reliability of each depth value based on physical models such as matching confidence and measurement distance; and an inertial measurement unit for calculating the six-degree-of-freedom kinematic state of the sensor carrier; rigid registration and timestamp alignment of the spatiotemporal reference are performed on the heterogeneous and asynchronous observation data (including RGB color image, binocular infrared depth image, inherent uncertainty map and inertial measurement unit) output by the aforementioned sensors, thereby generating a spatiotemporally synchronized multimodal time series data frame with deterministic delay in a unified coordinate system; finally, this enhanced data frame is used as parallel input and synchronously distributed to the downstream three functional modules (2) instance depth estimation, (3) three-dimensional geometric reconstruction and (4) pose estimation. (2) Instance depth estimation module: acquires RGB color image information from the data acquisition and preprocessing module, and outputs target information with instance annotations and its first depth data to the multi-source heterogeneous information fusion and elevation refinement module through the instance segmentation network. (3) Three-dimensional geometric reconstruction module: acquire the binocular infrared depth map from the data acquisition and preprocessing module, and construct the initial three-dimensional point cloud to multi-source heterogeneous information fusion and elevation refinement module based on the binocular infrared depth map; (4) Pose estimation module: The RGB color image, binocular infrared depth image and inertial measurement unit data from the data acquisition and preprocessing module are fused together. The pose is calculated by visual-inertial odometry and coupled with loop closure detection to optimize the camera trajectory. The sensor trajectory pose with global spatiotemporal consistency is output to the multi-source heterogeneous information fusion and elevation refinement module. (5) Multi-source heterogeneous probabilistic fusion and elevation refinement module: Based on the first depth data, the initial three-dimensional point cloud is remapped and segmented to generate a segmented point cloud; The segmented point cloud is fused with an inherent uncertainty map that characterizes the reliability of the data through multi-source heterogeneous probabilistic fusion, wherein the fusion weights are dynamically adjusted according to the inherent uncertainty map to generate a cross-modal enhanced fused point cloud. Based on the cross-modal enhanced fusion point cloud and the sensor trajectory pose of the globally spatiotemporally consistent sensor, a scene decoupled elevation fine-mapping map is constructed and output using the extended Kalman filter method.

2. A method for refining elevation in a specific scene based on multi-source heterogeneous probabilistic fusion according to claim 1, wherein the method acquires RGB color image information from a data acquisition and preprocessing module, and outputs target information with instance annotations and its first depth data to a multi-source heterogeneous information fusion and elevation refinement module through an instance segmentation network, characterized in that: A frame of synchronously aligned RGB color image and binocular infrared depth image is obtained from the spatiotemporally synchronized multimodal time-series data of the data acquisition and preprocessing module, and the depth scale factor depth_scale corresponding to the binocular infrared depth image for unit conversion is obtained; the RGB color image is input into a preset target detection and instance segmentation network, which processes the image to identify one or more target instances in the image, and outputs its category label, detection confidence, bounding box coordinates defining its two-dimensional position, and generates a pixel-level segmentation mask for each identified target instance. The mask is used to accurately distinguish the foreground pixels and background pixels of the target instance. Using the generated segmentation mask as a spatial index, extract all valid depth values ​​that correspond to the foreground pixels of the target instance and are greater than zero from the binocular infrared depth image; perform a preset average aggregation operation on the extracted set of valid depth values ​​to obtain a robust aggregated depth value; multiply the aggregated depth value by the obtained depth scaling factor depth_scale and perform unit conversion to calculate the first depth data that can characterize the three-dimensional spatial position of the target instance; Finally, the category label, detection confidence, bounding box coordinates, segmentation mask, and calculated first depth data of the target instance are encapsulated into a structured instance target data object, and the data object is output to the multi-source heterogeneous information fusion and elevation refinement module.

3. A method for refining elevation in a specific scene based on multi-source heterogeneous probabilistic fusion according to claim 1, wherein the method acquires a binocular infrared depth map from a data acquisition and preprocessing module, and constructs an initial 3D point cloud to multi-source heterogeneous information fusion and elevation refinement module based on the binocular infrared depth map, characterized in that: A frame of binocular infrared depth image synchronized with the RGB color image is obtained from the spatiotemporally synchronized multimodal time series data of the data acquisition and preprocessing module. A depth scaling factor for converting depth units to measurement units is also obtained, as well as a set of preset camera intrinsic parameter matrix parameters including focal length and principal point coordinates. The pixel coordinates of the binocular infrared depth image are traversed, and the following operations are performed for each pixel (u, v): the original depth value of the pixel is multiplied by the depth scaling factor to obtain the depth value in the measurement unit. The depth value is validated, including at least removing invalid depth values ​​that are less than or equal to zero or exceed a preset maximum distance threshold. For valid depth values ​​that pass the validation, the camera inverse projection model is applied, and based on the camera intrinsic parameter matrix parameters, the two-dimensional pixel coordinates (u, v) and their corresponding valid depth values ​​are calculated into three-dimensional spatial point coordinates (X, v) in the camera coordinate system. cam Y cam Z cam ); encapsulate all generated 3D spatial point coordinates into a structured initial 3D point cloud; wherein, the 3D spatial point coordinates are stored in floating-point format.

4. A method for precise elevation mapping in a specific scenario based on multi-source heterogeneous probabilistic fusion as described in claim 1, wherein the method fuses RGB color images, binocular infrared depth images, and inertial measurement unit data from a data acquisition and preprocessing module, performs pose calculation through visual-inertial odometry, and couples loop closure detection to optimize camera trajectory, outputting a globally spatiotemporally consistent sensor trajectory pose to a multi-source heterogeneous information fusion and elevation mapping module, characterized in that: The system receives the RGB color image, the binocular infrared depth image, and the raw measurement data from the inertial measurement unit as input data, and performs timestamp synchronization, sensor extrinsic parameter calibration, and gravity direction alignment on the input data to ensure global spatiotemporal consistency among the data sources. Based on the original measurement data of the inertial measurement unit, the inertial measurement unit pre-integration model is applied to predict the relative motion of the sensor between two observation times, and the inertial measurement unit measurement residual term is constructed. Two-dimensional visual feature points are extracted from the RGB color image and combined with the binocular infrared depth image. The feature points are then converted into three-dimensional spatial points through a camera inverse projection model, thereby constructing the visual observation residual term. The loop closure detection process includes: comparing the two-dimensional visual feature points extracted from the RGB color image at the current observation time with the two-dimensional visual feature points stored at each observation time along the historical path to identify the overlap between the current position and the historical position; if an overlap is identified, it is determined that the current observation time and the historical observation time constitute a loop closure pair, and the closed-loop constraint information that can characterize the relative pose transformation between the loop closure pair is calculated, thereby constructing a global closed-loop constraint residual term based on the loop closure detection result; A nonlinear optimization objective function is constructed and solved, which includes the inertial measurement unit measurement residual, the visual observation residual, and the global closed-loop constraint residual based on the loop closure detection results, to obtain a globally optimal state vector containing sensor position, attitude, velocity, and inertial measurement unit bias. Based on the globally optimal state vector, a globally spatiotemporally consistent sensor trajectory pose with centimeter-level accuracy and global spatiotemporal consistency is generated and output to the multi-source heterogeneous information fusion and elevation refinement module as a spatial reference benchmark for subsequent processing.

5. A method for refining elevation in a specific scene based on multi-source heterogeneous probabilistic fusion as described in claim 1, wherein the method performs remapping and segmentation processing on the initial three-dimensional point cloud based on the first depth data to generate a segmented point cloud; The segmented point cloud is then fused with an inherent uncertainty map representing the reliability of the data through multi-source heterogeneous probabilistic fusion, wherein... Based on the inherent uncertainty graph, the fusion weights are dynamically adjusted to generate a cross-modal enhanced fusion point cloud; Based on the cross-modal enhanced fusion point cloud and the globally spatiotemporally consistent sensor trajectory pose, a scene decoupled elevation fine-mapping map is constructed and output using the extended Kalman filter method. The key feature is that: First, the sensor trajectory pose (T) based on the globally consistent spatiotemporal consistency of the aforementioned global consistency. wb ) and the external parameters between the camera and the IMU (T bc ), through coordinate system transformation P world =T wb ·T bc ·P camera The original point cloud in the camera coordinate system is remapped to the world coordinate system to obtain the remapped point cloud, where P world P represents the coordinate position of the point cloud in the world coordinate system. camera Point cloud coordinates representing the camera coordinate system; instance segmentation mask By projecting from 3D to 2D, instance labels are assigned to the remapped point cloud, generating one or more segmented point clouds. The segmented point cloud is divided into background point cloud and obstacle point cloud, and its generation process can be defined by the following model: Where π(·) is the projection function, τ seg T is the preset segmentation threshold. cb It is T bc The inverse transform, i.e., T cb =(T bc ) -1 ; Secondly, a multi-source uncertainty model is constructed, which at least considers the depth uncertainty determined by the characteristics of the binocular infrared depth sensor. and instance uncertainty determined by the confidence of the segmentation network Based on the uncertainty model, a dynamic fusion weight w is generated for each data point. i Finally, by weighted summation, the multiple segmented point clouds are subjected to multi-source heterogeneous probabilistic fusion to generate a cross-modal enhanced fused point cloud P. fusion ; An extended Kalman filter is applied to the cross-modal enhanced fusion point cloud to continuously update the elevation values ​​and their corresponding uncertainty variances, thereby generating a base elevation map containing an elevation layer and an uncertainty layer. The positions of the separated obstacle point clouds are projected onto the generated base elevation map, and an independent obstacle differentiation layer is generated to mark the positions of the dynamic obstacles. Finally, the elevation layer, uncertainty layer, and obstacle differentiation layer are integrated to output a scene decoupled elevation refinement map that accurately depicts the scene height.

6. A system for implementing the method according to any one of claims 1 to 5, characterized in that: A data acquisition and preprocessing module includes multimodal observation data, including RGB color images, binocular infrared depth images, and inertial measurement unit data, and performs spatiotemporal reference registration and timestamp alignment on them to generate spatiotemporally synchronized data frames in a unified coordinate system; A parallel information processing processor is configured to receive data frames from the data acquisition and preprocessing module and execute the following information processing units in parallel: an instance depth estimation unit for processing the RGB color image to generate target information with instance annotations, the target information including at least the segmentation mask, detection confidence and first depth data of each target instance; A three-dimensional geometric reconstruction unit is used to construct an initial three-dimensional point cloud based on the depth image by coordinate system remapping; And a pose estimation unit, used to fuse the RGB image, binocular infrared depth image and inertial measurement unit data, and through visual-inertial odometry and loop closure detection optimization, calculate and output sensor trajectory pose with global spatiotemporal consistency; A multi-source heterogeneous information fusion and elevation refinement module is configured to receive the output from the parallelized information processing processor, apply an extended Kalman filter to the cross-modal enhanced fusion point cloud to generate a base elevation map containing an elevation layer and an uncertainty layer; project the positions of the separated obstacle point cloud onto the generated base elevation map and generate an independent obstacle differentiation layer to mark the positions of the dynamic obstacles; finally, integrate the elevation layer, uncertainty layer and obstacle differentiation layer to output a scene decoupled elevation refinement map that accurately depicts the scene height.

Images