Multi-source fusion navigation method and system based on three-dimensional target level dynamic-static decoupling

Abstract

The application relates to the technical field of navigation, and proposes a multi-source fusion navigation method and system based on three-dimensional target level dynamic and static decoupling, which comprises the following steps: guiding point cloud clustering based on two-dimensional target information in a target scene image to generate a three-dimensional target detection result with a space position and a category; according to a recognized dynamic target, dynamic points in visual observation and laser radar observation are removed based on dynamic feature discrimination constraints; based on obtained static observation information, visual feature constraints and laser radar point cloud geometric constraints are constructed, motion constraints provided by an inertial measurement unit are fused, and system pose estimation is carried out; according to a pose estimation result and static observation information after dynamic points are removed, map construction and updating based on observation consistency constraints are carried out. The application takes three-dimensional target level dynamic and static decoupling as a core, realizes visual semantic guided point cloud objectification and consistent removal of cross-modal dynamic points, and inhibits dynamic interference and map pollution in unified optimization and consistent mapping.

Description

Technical Field

[0001] This invention relates to the field of navigation-related technologies, specifically to a multi-source fusion navigation method and system based on three-dimensional target-level dynamic-static decoupling. Background Technology

[0002] The statements in this section are merely background information related to the present invention and do not necessarily constitute prior art.

[0003] SLAM (Simultaneous Localization and Mapping) technology is a core supporting means for navigation and environmental perception in autonomous systems such as intelligent driving, mobile robots, and UAV mapping. Traditional SLAM systems often rely on a single sensor, but in real-world complex environments, various sensors have inherent limitations, making it difficult to meet the dual requirements of high accuracy and robustness. Visual sensors can provide rich image texture and semantic information, which is helpful for feature extraction and scene understanding, but they are susceptible to factors such as changes in lighting, motion blur, and sparse textures, resulting in poor stability. LiDAR has accurate ranging and anti-lighting capabilities, and performs well in constructing geometric structures; however, it may still experience ranging distortion when faced with glass reflections, rain and fog obstructions, or sparse areas, and it lacks semantic understanding capabilities. Inertial Measurement Units (IMUs), while providing high-frequency motion priors and suitable for short-term dynamic compensation, are prone to drift and cannot achieve long-term accurate positioning independently.

[0004] To overcome the aforementioned problems, multi-source fusion SLAM technology has emerged. Existing LiDAR / vision / inertial fusion SLAM frameworks can improve positioning accuracy and robustness to some extent, but their core remains limited to joint estimation of geometric features, lacking efficient and accurate perception capabilities for dynamic targets in real-world scenes. Moving targets such as vehicles, pedestrians, and cyclists are common in dynamic environments. These targets disrupt the spatiotemporal consistency between LiDAR point clouds and visual features, leading to feature matching errors, point cloud degradation, unstable state estimation, and even system divergence. Existing multi-source SLAM systems generally lack spatiotemporal consistency modeling between vision and LiDAR in dynamic scenes, making it difficult to achieve collaborative recognition and removal of dynamic targets in both visual and point cloud modalities, thus causing positioning instability. During the map update phase, existing multi-source SLAM systems typically use visual and LiDAR observations separately for fusion processing, lacking effective differentiation and control over dynamic target observations acquired by different sensors. This results in moving targets being mistakenly written into the map under multimodal observations, gradually forming map pollution problems such as "ghosting" or "momentum." Summary of the Invention

[0005] To address the aforementioned issues, this invention proposes a multi-source fusion navigation method and system based on three-dimensional target-level dynamic-static decoupling. It guides point cloud clustering using two-dimensional target semantic information to generate and track categorized three-dimensional targets. Based on this, it achieves dynamic point removal consistent across visual and lidar modes, and integrates the removed static observations with IMU motion constraints into nonlinear optimization. Simultaneously, it combines observation consistency constraints to complete dynamic-static decoupling map construction and updating, thereby improving the stability of localization and mapping in dynamic scenes and suppressing map contamination.

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

[0007] The first aspect of this invention provides a multi-source fusion navigation method based on three-dimensional target-level dynamic-static decoupling, comprising the following steps:

[0008] Acquire images and LiDAR point cloud data of the target scene, guide point cloud clustering based on two-dimensional target information in the image, generate three-dimensional target detection results with spatial location and category, and track the three-dimensional target;

[0009] Based on the tracked and identified dynamic targets, and using dynamic feature discrimination constraints, dynamic points in visual and lidar observations are eliminated to obtain static observation information;

[0010] Based on the obtained static observation information, visual feature constraints and lidar point cloud geometric constraints are constructed, and motion constraints provided by the inertial measurement unit are fused to construct a multi-source fusion nonlinear optimization model. After solving the model, the pose estimation result of the navigation system is obtained.

[0011] Based on the pose estimation results at the current moment and the static observation information after removing dynamic points, a map is constructed and updated by decoupling dynamic and static elements based on observation consistency constraints.

[0012] A second aspect of the present invention provides a multi-source fusion navigation system based on three-dimensional target-level dynamic-static decoupling, comprising:

[0013] The 2D semantic-guided 3D target detection module is configured to acquire images and LiDAR point cloud data of the target scene, guide point cloud clustering based on 2D target information in the image, generate 3D target detection results with spatial location and category, and track 3D targets.

[0014] The dynamic point removal module is configured to remove dynamic points from visual and lidar observations based on dynamic feature discrimination constraints and the tracked and identified dynamic targets to obtain static observation information.

[0015] The pose estimation module is configured to construct visual feature constraints and lidar point cloud geometric constraints based on the obtained static observation information, fuse motion constraints provided by the inertial measurement unit, construct a multi-source fusion nonlinear optimization model, and obtain the pose estimation result of the navigation system after solving it.

[0016] The map building and updating module is configured to build and update a map based on the pose estimation results at the current moment and the static observation information after removing dynamic points, and to decouple the dynamic and static aspects based on observation consistency constraints.

[0017] A third aspect of the present invention provides a multi-source fusion navigation system based on three-dimensional target-level dynamic-static decoupling, comprising: a data acquisition device and a processor, wherein the processor is configured to execute the steps of the above-described multi-source fusion navigation method based on three-dimensional target-level dynamic-static decoupling.

[0018] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0019] This invention guides point cloud clustering using image-based 2D target information to generate 3D target detection results with spatial location and category, and continuously tracks 3D targets to accurately identify dynamic targets in the scene at the target level. Furthermore, based on the 3D target tracking results, dynamic feature discrimination constraints are constructed to simultaneously eliminate corresponding dynamic points in both visual and lidar observations. This achieves consistent processing of dynamic targets at the multimodal observation level, avoiding separate and fragmented participation of dynamic targets in state estimation in visual and point cloud environments. This effectively reduces the disruption of feature matching and geometric registration consistency caused by dynamic targets, improving the stability of the navigation system in dynamic environments.

[0020] To address the shortcomings of existing multi-source SLAM systems, which primarily rely on geometric features and lack robustness in complex environments, this embodiment constructs visual feature constraints, lidar point cloud geometric constraints, and integrates motion constraints provided by the inertial measurement unit (IMU) based on static observation information after removing dynamic points, forming a unified multi-source fusion nonlinear optimization model. This approach fully leverages the complementary advantages of vision, lidar, and IMU in terms of information dimension and temporal scale, enabling the system to obtain continuous and reliable pose estimation results even under complex conditions such as varying illumination, sparse texture, and point cloud degradation, thereby improving navigation accuracy and overall robustness.

[0021] During map updates, based on the pose estimation results at the current moment and the static observation information after removing dynamic points, an observation consistency constraint is introduced to decouple dynamic and static map construction and updates. Only static observations that satisfy spatiotemporal consistency are written into the map. In this way, the long-term cumulative impact of residual dynamic targets on the map can be effectively suppressed, and the incorrect embedding of moving targets into the map can be avoided, thereby improving the stability, accuracy, and reusability of the map.

[0022] The advantages of the present invention, as well as its additional advantages, will be described in detail in the following specific embodiments. Attached Figure Description

[0023] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute a limitation thereof.

[0024] Figure 1 This is a flowchart of the multi-source fusion navigation method of Embodiment 1 of the present invention;

[0025] Figure 2 This is a framework diagram of the SLAM algorithm in the multi-source fusion navigation method of Embodiment 1 of the present invention;

[0026] Figure 3 This is a schematic diagram of the three-dimensional detection and tracking process of Embodiment 1 of the present invention;

[0027] Figure 4 This is a flowchart of the multi-source feature consistency discrimination and localization optimization process under dynamic feature discrimination constraints in Embodiment 1 of the present invention. Detailed Implementation

[0028] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0029] It should be noted that the following detailed descriptions are exemplary and intended to provide further illustration of the invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains.

[0030] It should be noted that the terminology used herein is for describing particular embodiments only and is not intended to limit the exemplary embodiments of the present invention. As used herein, the singular form is intended to include the plural form as well, unless the context clearly indicates otherwise. Furthermore, it should be understood that when the terms "comprising" and / or "including" are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof. It should be noted that, without conflict, the various embodiments and features within those embodiments can be combined with each other. The embodiments will now be described in detail with reference to the accompanying drawings.

[0031] Example 1

[0032] In one or more of the technical solutions disclosed in the embodiments, such as Figures 1 to 4 As shown, a multi-source fusion navigation method based on three-dimensional target-level dynamic-static decoupling includes the following steps:

[0033] Step S1: Acquire images and LiDAR point cloud data of the target scene, guide point cloud clustering based on two-dimensional target information in the image, generate three-dimensional target detection results with spatial location and category, and track the three-dimensional target;

[0034] Step S2: Based on the tracked and identified dynamic targets, dynamic points in visual observation and lidar observation are eliminated based on dynamic feature discrimination constraints to obtain static observation information;

[0035] Step S3: Based on the obtained static observation information, construct visual feature constraints and lidar point cloud geometric constraints, fuse motion constraints provided by the inertial measurement unit, construct a multi-source fusion nonlinear optimization model, and solve it to obtain the pose estimation result of the navigation system.

[0036] Step S4: Based on the pose estimation results at the current moment and the static observation information after removing dynamic points, construct and update the map with dynamic-static decoupling based on the observation consistency constraint.

[0037] This embodiment guides point cloud clustering based on 2D target information from images to generate 3D target detection results with spatial location and category, and continuously tracks 3D targets to accurately identify dynamic targets in the scene at the target level. Furthermore, dynamic feature discrimination constraints are constructed based on the 3D target tracking results, and corresponding dynamic points are simultaneously removed from both visual and lidar observations. This achieves consistent processing of dynamic targets at the multimodal observation level, avoiding separate and fragmented participation of dynamic targets in state estimation in visual and point cloud environments. This effectively reduces the disruption of feature matching and geometric registration consistency caused by dynamic targets, improving the stability of the navigation system in dynamic environments. Addressing the problem that existing multi-source SLAM systems mainly rely on geometric features and lack robustness to complex environments, this embodiment, based on static observation information after removing dynamic points, simultaneously constructs visual feature constraints, lidar point cloud geometric constraints, and integrates motion constraints provided by the inertial measurement unit to form a unified multi-source fusion nonlinear optimization model. This approach fully leverages the complementary advantages of vision, lidar, and IMU in terms of information dimension and temporal scale, enabling the system to obtain continuous and reliable pose estimation results even under complex conditions such as varying illumination, sparse texture, and point cloud degradation, thereby improving navigation accuracy and overall robustness. During map updates, based on the pose estimation results at the current moment and static observation information after removing dynamic points, observation consistency constraints are introduced to decouple dynamic and static map construction and updates, writing only static observations that satisfy spatiotemporal consistency into the map. This method effectively suppresses the long-term cumulative impact of residual dynamic targets on the map, preventing moving targets from being incorrectly fixed in the map, thus improving map stability, accuracy, and reusability.

[0038] In dynamic scenes, dynamic blurring caused by high-speed object movement and deformation caused by object occlusion can lead to misidentification or omission of potential dynamic targets. Current deep learning methods based on LiDAR suffer from problems such as data redundancy, large model parameter count, high computational complexity, and limited effective feature learning. Furthermore, vision-based target detection faces the challenge of inaccurate target spatial position estimation when accurate depth information is lacking. Therefore, step 1 of this embodiment proposes an optimized 3D target detection method that integrates visual semantic priors with geometric constraints from LiDAR point clouds to improve the accuracy of 3D detection boxes and the stability of dynamic target tracking. First, prior constraints from visual detection results guide the generation of 3D detection boxes through LiDAR point cloud clustering, realizing the prior constraint of semantic information on geometric information and reducing redundant point cloud computing. Second, continuous prediction and updating of the 3D position and velocity of dynamic targets based on Kalman filtering enables high-precision tracking of dynamic targets.

[0039] In dynamic scenes, relying solely on 2D visual detection for dynamic target recognition is insufficient to obtain reliable spatial depth information, while relying solely on point cloud geometry is prone to misjudgment in complex scenarios such as occlusion, sparse point clouds, or morphological changes. To address this issue, step 1 of this embodiment constructs a method for 2D visual target detection and 3D LiDAR point cloud target perception and spatial representation, achieving the synergistic utilization of 2D semantics and 3D spatial information, thereby improving the accuracy of dynamic target recognition and its ability to adapt to complex scenes.

[0040] The 3D detection and tracking process in step 1 is as follows: Figure 3 As shown, step 1, which involves acquiring images and LiDAR point cloud data of the target scene, guiding point cloud clustering based on two-dimensional target information in the image, and generating three-dimensional target detection results with spatial location and category, includes the following steps:

[0041] Step S11: Acquire images and LiDAR point cloud data of the target scene, and use a target detection network to perform target recognition on the image to obtain a two-dimensional bounding box. and two-dimensional visual detection of target categories;

[0042] Step S12: Based on the preset camera intrinsic and extrinsic parameters, map the two-dimensional bounding box to the three-dimensional space to form a view frustum region, and perform clustering processing on the point cloud based on the view frustum region to obtain candidate three-dimensional targets;

[0043] Step S13: For candidate 3D targets, based on the 2D visual detection target category information obtained by target recognition in the image, perform semantic consistency screening and target bounding box optimization to generate 3D bounding boxes and target category information as 3D target detection results;

[0044] In the above steps, the progressive logic of two-dimensional semantic guidance, three-dimensional spatial constraints, and dynamic target precision is used to achieve collaborative perception of visual and lidar data, laying the foundation for subsequent dynamic target tracking.

[0045] In step S11, the acquired visual image, such as an RGB image, can be recognized using the YOLOv11n model network to obtain a two-dimensional bounding box. And 2D semantic information of target categories detected by two-dimensional vision;

[0046] In step S12, the two-dimensional bounding box is mapped to three-dimensional space using the camera's intrinsic and extrinsic parameter matrices to form the corresponding view frustum region. ; visual cone region This can be used to limit the processing range of subsequent point cloud clustering, the view frustum region. The calculation formula is as follows:

[0047] (1);

[0048] in, Represents a point in three-dimensional space. These are the projected coordinates of a point in three-dimensional space onto the image plane. For the camera's intrinsic and extrinsic parameters matrix, Let be the pose transformation matrix from the camera coordinate system to the radar coordinate system. This represents the horizontal boundary of a two-dimensional bounding box. This represents the vertical boundary of a two-dimensional bounding box.

[0049] like Figure 2 As shown, the acquired RGB image is processed with sparse optical flow to update the visual points. The updated image is then mapped from two dimensions to three dimensions, i.e., 2D-3D mapping, to obtain the view frustum region. ;

[0050] In step S12, based on the view frustum region A method for clustering point clouds to obtain candidate 3D targets includes the following steps:

[0051] Step S121: Process the input lidar point cloud data, i.e., the 3D point cloud. Preprocessing is performed, including noise filtering and point cloud downsampling, to obtain the preprocessed point cloud. ;

[0052] Specifically, the lidar scanning data is reconstructed. Based on the scan line number, azimuth angle, and sampling timing information of each point on the lidar, the original scan points are rearranged and organized to restore the point cloud to a data structure that conforms to the actual scanning order of the lidar, facilitating subsequent motion compensation and registration calculations. Based on the reconstructed lidar data and the prior state results obtained from IMU forward propagation, backward propagation is performed on the point cloud. Based on the backward propagated point cloud data, the geometric constraint relationship between the current frame point cloud and the local environmental plane features is constructed, and the point-to-plane error is calculated. The point cloud used for point-to-plane error calculation is then downsampled.

[0053] Step S122, Viewing cone region The point cloud data within the area was processed using the DBSCAN algorithm (Density-Based Spatial Clustering of Applications with Noise). Perform density clustering to form a candidate cluster set. Candidate three-dimensional targets are obtained;

[0054] The DBSCAN algorithm clustering formula is:

[0055] (2);

[0056] in, For the search radius, For point The set of neighborhood points within the radius. Density threshold;

[0057] In step S13, for candidate 3D targets, based on the target category information obtained from target recognition, semantic consistency screening and target bounding box optimization are performed to generate 3D bounding boxes and target category information as 3D target detection results. The method includes the following steps:

[0058] Step S131, Category Consistency Judgment: Target category information is detected based on two-dimensional vision. The consistency score between the cluster and the target category detected by 2D vision is calculated, the semantic consistency of the point cloud clustering results is filtered, and the initial 3D bounding box is obtained based on the filtered cluster.

[0059] For each cluster Calculate its relationship with category Consistency score The calculation formula is:

[0060] (3);

[0061] Only retain those that meet the requirements. Clustering is used to generate initial 3D bounding boxes. ;in, As the central location, For size, This is the direction angle.

[0062] Step S132: Adjust the center position and optimize the size of the initial 3D bounding box to make the bounding box consistent with the geometric features of the clustered point cloud and aligned with the target category information detected by 2D vision, so as to obtain the 3D target detection result.

[0063] Specifically, during the bounding box optimization process, a weighted merging strategy is applied to overlapping or redundant bounding boxes, as shown in the following formula:

[0064] (4);

[0065] in, and They represent the first and second parts to be merged. The and the first A three-dimensional bounding box, This represents the final 3D bounding box after fusion; and These are the weight coefficients for the corresponding bounding boxes, used to measure their reliability, and are usually given by the detection confidence score, i.e. , , and These represent the category confidence scores for the corresponding bounding boxes.

[0066] Finally, the optimized 3D bounding box The system outputs category information to form the detection results of three-dimensional targets in the scene, namely 3D semantic information, which can be directly used for subsequent dynamic target tracking, path planning or obstacle avoidance, to achieve efficient collaborative perception of visual semantic information and three-dimensional geometric information, thereby improving the accuracy and computational efficiency of three-dimensional target detection.

[0067] The steps described in this embodiment introduce semantic prior information based on visual detection results to constrain the clustering range of LiDAR point clouds, effectively reducing the participation of irrelevant region point clouds and lowering the computational complexity of clustering. Simultaneously, using purely mathematical methods such as DBSCAN for point cloud clustering and bounding box fitting eliminates the need for high-overhead deep learning models, avoiding the problems of large memory consumption, slow inference speed, and strong hardware dependence that are common in existing model-based learning methods in dynamic scenes.

[0068] Furthermore, by defining semantic consistency scores for filtering and optimizing bounding box parameters in conjunction with point cloud geometry, efficient coupling of target semantic information and spatial structural information is achieved, improving the accuracy and stability of 3D detection boxes. A redundant box merging strategy further enhances the compactness of detection results and overall perception quality, providing more accurate and cleaner 3D structural input for subsequent dynamic target tracking and map updates. This approach significantly reduces system computational resource consumption while maintaining detection accuracy, exhibiting good real-time performance and deployment flexibility, making it suitable for resource-constrained edge devices or mobile robot platforms.

[0069] The 3D target detection results identified through the above steps, based on the recognition and tracking of multiple frames of target images, can determine whether the target is a dynamic target;

[0070] In step 1, for 3D target tracking, a state estimation algorithm based on filtering can be used, such as Kalman filtering, extended Kalman filtering, and unscented Kalman filtering. In this embodiment, the Kalman filtering algorithm is used to continuously predict and update the 3D position and velocity of the 3D target, obtain the motion state of the 3D target, and identify dynamic targets.

[0071] The dynamic target state is defined by its position, velocity, and acceleration. The observation of each dynamic target is calculated based on the centroid of the point cloud within the corresponding 3D detection box, thereby ensuring that the input of the Kalman filter accurately reflects the actual position of the target in 3D space.

[0072] In the prediction phase, the Kalman filter uses a constant acceleration model to capture the velocity changes of a dynamic target. Its prediction step formula is as follows:

[0073] (5);

[0074] in, It represents the state of a dynamic target, which can include position, attitude, and velocity; The state transition matrix is ​​obtained based on constant acceleration dynamics. The noise of the motion model is represented by an adjustable parameter, and t represents time.

[0075] During the observation phase, the velocity and acceleration of the dynamic target are estimated based on the detected location:

[0076] (6);

[0077] in, Let represent the position, velocity, and acceleration of the dynamic target at time t, respectively. This represents the system time step. By combining the prediction model and the observation model, the standard Kalman filter equation can be used to estimate the state of each detected dynamic target, thus obtaining the final motion state. .

[0078] When the speed of a dynamic target exceeds a preset threshold, it is marked as dynamic. This method can not only perform high-precision tracking of a single target, but also achieve continuous tracking of dynamic targets in a multi-target environment through a state association algorithm, providing a stable and reliable input for subsequent dynamic feature removal in the system.

[0079] Step 1 of this embodiment achieves complementary utilization of cross-modal information by fusing 2D visual semantic priors with the geometric structure information of 3D LiDAR point clouds, significantly improving the 3D detection accuracy and scene adaptability of dynamic targets. The 2D bounding boxes and target category information provided by the visual detection module are used to construct the frustum constraint region, accurately guiding the LiDAR point cloud clustering process, reducing interference from invalid point clouds, and minimizing redundant computation. Simultaneously, the clustering results are combined to optimize the bounding boxes and perform semantic consistency screening, effectively improving the accuracy of 3D bounding boxes in center localization, size fitting, and category matching.

[0080] Furthermore, by dynamically updating the target state using a Kalman filter, continuous tracking and stable identification of multiple targets in complex dynamic environments can be achieved, overcoming the target drift and tracking loss problems caused by occlusion, masking, sparse point clouds, or category ambiguity in traditional methods. This method not only enhances the system's perception of dynamic objects but also provides accurate and robust input for subsequent modules such as dynamic point removal, path planning, and obstacle avoidance, thus improving the overall perception quality and operational stability of the system in dynamic scenes.

[0081] After completing the 3D detection and continuous tracking of dynamic targets, to avoid interference with the system's positioning accuracy, step S2 further introduces a multi-source feature consistency discrimination method based on dynamic feature discrimination constraints. This method removes dynamic feature points from visual and radar observations and, based on this, constructs a robust navigation and positioning optimization model to achieve high-precision navigation and positioning in complex dynamic environments. The multi-source feature consistency discrimination and positioning optimization based on dynamic feature discrimination constraints is as follows: Figure 4 As shown.

[0082] Visual image observation refers to the process of extracting information such as feature points and pixel coordinates from an image;

[0083] LiDAR point cloud observation refers to the process of acquiring information such as spatial location, distance, intensity, reflectivity, and velocity from lidar point cloud data.

[0084] Step S2: Based on the tracked and identified dynamic targets, and using dynamic feature discrimination constraints, dynamic points from visual and lidar observations are eliminated to obtain static observation information, i.e., static points. This includes the following steps:

[0085] Step S21: For visual feature points in the image, construct dynamic feature discrimination constraints based on the consistency change of the 3D depth between the projection area of ​​the 3D detection box of the dynamic target and the feature points, identify and remove potential dynamic points, and obtain static visual feature points, including the following steps:

[0086] Step S211: Project the 3D detection bounding box of the dynamic target onto the current image plane to form a 2D dynamic region mask. ;

[0087] Step S212: Place the pixel coordinates in the image into a two-dimensional dynamic region mask. Visual feature points within the area are identified as dynamic points and removed.

[0088] In the visual modality, the three-dimensional dynamic region corresponding to the dynamic target is first reprojected onto the current image plane using the aforementioned dynamic target 3D detection and tracking results, forming a two-dimensional dynamic region mask. For feature points obtained by the visual front-end during feature extraction and matching... Based on its pixel coordinates, it is determined whether it falls within any dynamic region mask; if it satisfies formula (7), the feature point is directly identified as a dynamic visual point and removed, thereby eliminating the interference of the detected dynamic target on visual feature matching.

[0089] (7);

[0090] Step S213: For masks that do not fall into the two-dimensional dynamic region The visual feature points are calculated using a multi-view geometry method to determine their 3D positions as actual positions, and the current position is predicted based on the previous position and the current camera pose.

[0091] For visual feature points that do not fall within the 2D dynamic region mask, a depth consistency constraint (i.e., depth constraint) is introduced for further discrimination. Specifically, a multi-view geometry method is used to obtain the 3D spatial position of the feature point at time t. And based on the three-dimensional position of the feature points at the previous time step. and current camera pose change Predict its three-dimensional position at the current moment:

[0092] (8);

[0093] in, The three-dimensional position of the feature point predicted at time t;

[0094] The three-dimensional positions of feature points are calculated using a multi-view geometry method as their actual positions. The calculation method is as follows:

[0095] ;

[0096] in, Let n be the camera projection matrix. For camera projection matrix, This refers to the camera's external parameters.

[0097] Through joint optimization using multi-view observations, the 3D position of feature points can be obtained by minimizing the reprojection error:

[0098] ;

[0099] in, For the first i The three-dimensional homogeneous coordinates of the feature points Let be the pixel coordinates of the feature point in the nth frame of the image.

[0100] Step S214: Calculate the depth residual between the predicted position and the actual position. If the residual exceeds the threshold, the current feature point is regarded as a potential dynamic point and is removed to obtain a static visual feature point that satisfies the static assumption.

[0101] The static assumption is a prerequisite for the SLAM method. It is a definition that the feature points or landmarks in the map are fixed, do not move on their own, do not suddenly appear / disappear, and do not deform; what is moving is the sensor or the robot itself.

[0102] The depth residual between the predicted location and the actual observed location is calculated using the following formula:

[0103] (9);

[0104] Construct depth consistency constraints based on depth residuals, when depth residuals Exceeding the preset threshold If the feature point violates the static scene assumption, it is considered a potential dynamic interference point and is removed.

[0105] This embodiment, through the above implementation method, uses a joint discrimination method based on three-dimensional dynamic target projection constraints and depth consistency constraints. Without relying on optical flow or motion consistency analysis, it can effectively eliminate visual interference features introduced by dynamic objects such as pedestrians and vehicles, retaining only high-consistency visual features that satisfy the static assumptions for subsequent pose estimation and positioning optimization, thereby significantly improving the stability and positioning accuracy of the system in complex dynamic environments.

[0106] Step S22: For points in the lidar point cloud, construct motion feature discrimination constraints based on the spatial constraints of the 3D detection box of the dynamic target and the consistency change between the radial velocity of the point cloud and the predicted velocity of the target, identify and remove potential dynamic point clouds, and obtain the static lidar point cloud, including the following steps:

[0107] Step S221: Based on the tracked and identified dynamic targets obtained in step S1, transform the current frame's LiDAR point cloud data to the same local coordinate system. If a point falls into any dynamic target detection box... If it is inside, it is marked as a candidate dynamic point cloud;

[0108] In the lidar mode, the dynamic target 3D detection and continuous tracking results mentioned above are used to identify and remove dynamic points in the lidar point cloud. First, the lidar point cloud is... Transform to the same local coordinate system, and based on the 3D detection box corresponding to each dynamic target. Determine whether the lidar point falls within the spatial range of any dynamic target; if the lidar point If the condition satisfies formula (10), then it is considered as a candidate dynamic point and enters the further discrimination process;

[0109] (10);

[0110] Step S222: Calculate the observed radial velocity of the candidate dynamic point cloud and calculate the residual with the projected velocity of the predicted target velocity in the line of sight direction. If the velocity residual is lower than the threshold, the current candidate dynamic point is a dynamic point and is removed.

[0111] Specifically, for lidar points located within the 3D detection frame of a dynamic target, a velocity consistency constraint is further introduced for fine-grained discrimination. Lidar points typically carry radial velocity observation information, denoted as radial velocity. Simultaneously, based on the dynamic target state estimate obtained from the Kalman filtering in step S1, the predicted velocity of the corresponding target at the current moment can be obtained. The target velocity is projected onto the radar line-of-sight to obtain the predicted radial velocity. :

[0112] (11);

[0113] in, Indicates the first The predicted radial velocity of each target, For this goal at time The three-dimensional predicted velocity vector; Indicates the first A point cloud at any time spatial coordinates, This represents the position of the lidar sensor in the local coordinate system. Then, the residual between the observed radial velocity and the predicted radial velocity at the radar point is calculated:

[0114] (12);

[0115] in, Indicates the first The velocity residual at each point For the actual radial velocity measured by lidar, To predict radial velocity.

[0116] When velocity residual Less than the preset threshold If the point is considered to be in the same motion state as the dynamic target, it is identified as a dynamic point and removed.

[0117] Step S223: For point clouds that do not fall into any dynamic target 3D detection box, remove points whose radial velocity deviates from the static assumption as dynamic points;

[0118] For point cloud points that do not fall within any dynamic target 3D detection bounding box, supplementary discrimination is performed based on their radial velocity magnitude. If the radial velocity of a LiDAR point significantly deviates from the static scene assumption, it is considered a potential dynamic point and removed, ultimately yielding a static LiDAR point cloud. The static scene assumption refers to an assumption that the environment around the sensor is static and that surrounding ground features do not change, i.e., their velocity is 0.

[0119] This embodiment uses the joint discrimination method based on three-dimensional spatial constraints and velocity consistency constraints to effectively remove lidar points corresponding to dynamic targets such as pedestrians and vehicles, retaining only stable and reliable static structural points, thus providing high-quality geometric observation constraints for subsequent positioning optimization.

[0120] In step S2 above, a cross-modal information constraint mechanism is proposed for dynamic point removal. By introducing a combination of visual dynamic masking and depth consistency residuals, the accurate identification and removal of dynamic interference in image feature points is achieved. This method not only uses the identified dynamic target projection region to initially screen out explicit dynamic feature points, but also further identifies pseudo-static feature points corresponding to potential moving targets through depth residuals, thereby improving the accuracy and robustness of visual dynamic point removal.

[0121] In the lidar mode, the system utilizes the residual relationship between the observed radial velocity of radar points and the predicted velocity of the target to construct velocity consistency discrimination constraints, performing fine-grained dynamic identification and filtering of point clouds falling within the dynamic detection box. Compared to traditional coarse-grained removal strategies based on spatial range or simplified motion consistency methods, the dynamic point removal scheme in this embodiment can achieve more refined and accurate multimodal interference point identification, effectively avoiding interference from dynamic targets on front-end feature matching and back-end state estimation.

[0122] This embodiment significantly improves the localization robustness and estimation stability of the SLAM system in complex dynamic environments by achieving collaborative screening and suppression under both visual and lidar modes, providing a solid data foundation for high-reliability navigation and map building.

[0123] Step S3: Based on the obtained static points, construct visual feature constraints and lidar point cloud geometric constraints, fuse motion constraints provided by the inertial measurement unit, construct a multi-source fusion nonlinear optimization model, and solve it to obtain the pose estimation result of the navigation system.

[0124] System pose refers to the attitude and position of a moving entity equipped with a sensor array in the world coordinate system, i.e., the spatial pose of the sensor carrier. The moving entity is the entity that performs autonomous navigation, which can be a vehicle, robot, or drone, etc.

[0125] The sensor suite includes a visual image sensor, a lidar sensor, and an inertial measurement unit. The visual image sensor is used to acquire images; the lidar sensor is used to acquire lidar point cloud data.

[0126] After removing dynamic points from both the visual and lidar modes, the system retains only multi-source observation information that satisfies the static scene assumption, which is used to construct a robust localization optimization model. By fusing visual feature constraints, lidar point cloud geometric constraints, and motion constraints provided by the inertial measurement unit, the system pose is jointly estimated under a unified optimization framework, thereby achieving high-precision localization in complex dynamic environments.

[0127] Suppose the system is at time 10:00. position T for:

[0128] (13);

[0129] in, and They represent the system at time t. The rotation matrix and translation vector.

[0130] In step S2, the static observation information includes static visual feature points and static lidar point clouds;

[0131] For the static visual feature points removed in step S21, i.e., the static observation information of vision (visual map points), a visual reprojection residual term based on reprojection error is constructed. As a visual feature constraint, the formula is:

[0132] (14);

[0133] in, For the current frame number The observed pixel coordinates of a visual feature point in the image. For its corresponding three-dimensional space point, Let be the transformation matrix of the camera in the current frame. This represents the camera projection model.

[0134] For the static lidar point cloud filtered in step S22, i.e., the static observation information of the point cloud (radar map points), a geometrically consistent point cloud geometric residual is constructed as a geometric constraint for the lidar point cloud, including point-to-plane residual or point-to-point residual. The formula for calculating the point-to-plane residual is as follows:

[0135] (15);

[0136] in, Indicates the current frame number The residual distance from a point cloud point to the plane. For the current frame number Point cloud points, For the current frame number The pose transformation matrix of each point cloud point. This is the corresponding static reference point in the local map. This is the normal vector at the static reference point;

[0137] The formula for calculating point-to-point residuals is:

[0138] ;

[0139] in, Indicates the current frame number The point-to-point residual of a point cloud. The current frame number The transformation matrix for the rotation and displacement of a point cloud point;

[0140] Furthermore, based on the motion constraints provided by the inertial measurement unit (IMU), the inertial residuals between poses at adjacent time points are constructed, i.e., the IMU motion residuals:

[0141] (16);

[0142] in, Indicates at time IMU motion residuals Indicates at time The relative pose is obtained by IMU pre-integration. and These represent pose error and pose composite operation, respectively.

[0143] Based on the above multi-source static constraints, a unified nonlinear least squares optimization objective function is constructed as follows:

[0144] (17);

[0145] in, , , Let represent the covariance matrices for vision, LiDAR, and inertial constraints, respectively. To further improve the system's robustness in complex environments, a robust kernel function can be introduced during the optimization process to suppress residual anomaly constraints.

[0146] The pose estimation in the above steps only integrates multi-source static observation information after dynamic removal, effectively avoiding the impact of erroneous constraints introduced by dynamic objects on pose estimation. A unified nonlinear least squares optimization model is constructed, integrating visual feature constraints, LiDAR point cloud geometric constraints, and motion constraints provided by the inertial measurement unit, achieving joint estimation of multi-source static observation information. This optimization model relies only on highly consistent observation data after dynamic feature discrimination constraint removal, effectively suppressing mismatches and false constraints introduced by dynamic targets, significantly improving the system's positioning accuracy and estimation robustness in complex dynamic environments. It maintains stable, continuous, and high-precision positioning performance even in complex dynamic scenarios such as dense pedestrian areas and frequent vehicle movement, providing a reliable pose foundation for subsequent map construction, path planning, and autonomous decision-making.

[0147] After completing dynamic target detection, dynamic point removal, and robust localization optimization, the system can obtain high-precision pose estimation results and multi-source static observation information after dynamic interference suppression at every moment. Based on the above results, step S4 further carries out map construction and continuous updating under dynamic interference suppression to ensure that the generated map only reflects the stable static structure in the environment and avoids dynamic objects affecting the ground. Figure 1 This impacts consistency and long-term availability.

[0148] In step S4, based on the pose estimation results at the current moment and the static observation information after removing dynamic points, a dynamic-static decoupling map is constructed and updated based on observation consistency constraints, including the following steps:

[0149] Step S41, Static Mapping: The static visual feature points and static lidar point cloud obtained in step S2 are converted to the global coordinate system through the current pose estimation result to generate a static map that reflects the stable structure in the environment.

[0150] Specifically, in the map building stage, static visual feature points and static LiDAR point clouds that have passed the consistency judgment in step S2 are selected as the input for map building.

[0151] Step S411: Select the static visual feature points filtered in step S2. Based on the optimal pose estimation at the current moment The coordinates are uniformly transformed to the global coordinate system and merged into the global sparse or semi-dense visual map to obtain the first map;

[0152] Step S412: Convert the static lidar point cloud The pose transformation is mapped to the map coordinate system and fused into the first map to obtain a dense geometric structure map, i.e., a static map;

[0153] Removing dynamic points during the map input stage can effectively avoid map pollution problems caused by dynamic objects, such as ghosting and ghosting.

[0154] Step S42, Map Update and Optimization: For static maps, based on observation consistency and spatial location residuals, dynamically adjust the confidence of map points and remove unstable points;

[0155] Specifically, during map updates, to ensure the long-term stability and consistency of the map structure, an update strategy based on observation consistency is introduced. This applies to static map points already existing in the map. When it is continuously observed by new static observations in multiple time steps, and its spatial location residual satisfies:

[0156] (18);

[0157] If a map point is considered to have high stability, its position is updated with weighted values. If a map point is not re-observed by static observation in multiple consecutive time steps, or its residuals continue to exceed the threshold, it is identified as an unstable point and its confidence is gradually reduced until it is removed from the map.

[0158] Step S43: Decouple dynamic target information from static map content to form a layered map representation that combines a static structure map with dynamic target status, thereby achieving long-term stable environmental mapping in complex dynamic environments and obtaining an improved static voxel map.

[0159] A further technical solution is to decouple and store dynamic target information from static map content to form a layered map representation that includes static structure map and dynamic target status, thereby achieving long-term stable environmental mapping in complex and dynamic environments.

[0160] Specifically, dynamic targets only participate in the target detection and tracking process as temporal-aware information and do not participate in static map fusion; while the map only retains structural elements in the environment that have long-term stability.

[0161] This embodiment utilizes the map construction and updating method under dynamic interference suppression described above. The system can continuously construct a consistent and stable environmental map in complex dynamic scenarios such as dense pedestrian areas and frequent vehicle movement. This effectively avoids the impact of dynamic targets on map quality and provides solid support for the long-term operation and highly reliable decision-making of the autonomous system.

[0162] This embodiment also includes a navigation and positioning module, which, after completing the removal of dynamic feature points, uses the visual feature points after removing dynamic interference, point cloud feature points, and the improved static voxel map generated by the local mapping module to estimate and continuously correct the current pose of the mobile platform in real time, thereby obtaining stable and accurate navigation and positioning results.

[0163] The system acquires the static LiDAR point cloud of the current frame after dynamic feature point stripping. The point cloud update unit registers the static LiDAR point cloud of the current frame with the improved static voxel map to obtain the point cloud update result. The system also acquires the static visual feature points of the current frame after dynamic feature point stripping. Based on the point cloud update result, the visual update unit performs visual correction on the current pose to obtain the visual update result. The odometry trajectory unit receives the visual update result and associates it with the historical pose sequence to generate the navigation trajectory and positioning output for the current moment.

[0164] The navigation method in this embodiment, based on high-precision pose estimation and dynamic interference suppression, utilizes only static multimodal observation data after consistency judgment for map construction and updating, completely avoiding the risk of dynamic targets being incorrectly fused into the static map. By introducing a map point update strategy based on observation consistency, the system can dynamically adjust the confidence level of map points, maintaining a stable and clean map representation over the long term. Simultaneously, a dynamic-static information decoupling storage mechanism is adopted, managing dynamic target information and static structural maps in a hierarchical manner to ensure the long-term validity of map content and the stability of system operation. This not only improves the navigation accuracy and robustness of multi-source fusion SLAM systems in dynamic environments but also significantly enhances the usability and long-term consistency of environmental maps, providing strong technical support for applications such as intelligent driving and robot navigation.

[0165] Example 2

[0166] Based on Embodiment 1, this embodiment provides a multi-source fusion navigation system based on three-dimensional target-level dynamic-static decoupling, including:

[0167] The 2D semantic-guided 3D target detection module is configured to acquire images and LiDAR point cloud data of the target scene, guide point cloud clustering based on 2D target information in the image, generate 3D target detection results with spatial location and category, and track 3D targets.

[0168] The dynamic point removal module is configured to remove dynamic points from visual and lidar observations based on dynamic feature discrimination constraints and the tracked and identified dynamic targets to obtain static observation information.

[0169] The pose estimation module is configured to construct visual feature constraints and lidar point cloud geometric constraints based on the obtained static observation information, fuse motion constraints provided by the inertial measurement unit, construct a multi-source fusion nonlinear optimization model, and obtain the pose estimation result of the navigation system after solving it.

[0170] The map building and updating module is configured to build and update a map based on the pose estimation results at the current moment and the static observation information after removing dynamic points, and to decouple the dynamic and static aspects based on observation consistency constraints.

[0171] It should be noted that each module in this embodiment corresponds one-to-one with each step in embodiment 1, and their specific implementation process is the same, so it will not be repeated here.

[0172] Example 3

[0173] Based on Embodiment 1, this embodiment provides a multi-source fusion navigation system based on three-dimensional target-level dynamic-static decoupling, including: a data acquisition device and a processor, wherein the processor is configured to execute the steps of the multi-source fusion navigation method based on three-dimensional target-level dynamic-static decoupling described in Embodiment 1.

[0174] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

[0175] While the specific embodiments of the present invention have been described above in conjunction with the accompanying drawings, this is not intended to limit the scope of protection of the present invention. Those skilled in the art should understand that various modifications or variations that can be made by those skilled in the art without creative effort based on the technical solutions of the present invention are still within the scope of protection of the present invention.

Claims

1. A multi-source fusion navigation method based on three-dimensional target-level dynamic-static decoupling, characterized in that, Includes the following steps: Acquire images and LiDAR point cloud data of the target scene, guide point cloud clustering based on two-dimensional target information in the image, generate three-dimensional target detection results with spatial location and category, and track the three-dimensional target; Based on the tracked and identified dynamic targets, and using dynamic feature discrimination constraints, dynamic points in visual and lidar observations are eliminated to obtain static observation information; Based on the obtained static observation information, visual feature constraints and lidar point cloud geometric constraints are constructed, and motion constraints provided by the inertial measurement unit are fused to construct a multi-source fusion nonlinear optimization model. After solving the model, the pose estimation result of the navigation system is obtained. Based on the pose estimation results at the current moment and the static observation information after removing dynamic points, a map is constructed and updated by decoupling dynamic and static elements based on observation consistency constraints. The process of generating 3D target detection results with spatial location and category includes the following steps: Acquire images and LiDAR point cloud data of the target scene, and use a target detection network to perform target recognition on the images to obtain two-dimensional bounding boxes and two-dimensional visual detection target categories; Based on preset camera intrinsic and extrinsic parameters, the two-dimensional bounding box is mapped to three-dimensional space to form a view frustum region. The point cloud is then clustered based on the view frustum region to obtain candidate three-dimensional targets. For candidate 3D targets, based on the 2D visual detection target category information obtained from image target recognition, semantic consistency screening and target bounding box optimization are performed to generate 3D bounding boxes and target category information, which are used as 3D target detection results. A method for clustering point clouds based on view frustum regions to obtain candidate 3D targets includes the following steps: Preprocess the input lidar point cloud data; For the point cloud data within the view frustum region, the DBSCAN algorithm is used for density clustering to form a candidate cluster set, thus obtaining candidate 3D targets; For candidate 3D targets, based on the target category information obtained from target recognition, semantic consistency screening and target bounding box optimization are performed to generate 3D bounding boxes and target category information as 3D target detection results; The method for performing semantic consistency screening and target bounding box optimization to generate 3D bounding boxes and target category information as 3D target detection results includes the following steps: Based on the target category information detected by two-dimensional vision, the consistency score between the cluster and the target category detected by two-dimensional vision is calculated. The semantic consistency of the point cloud clustering results is filtered. Based on the filtered clusters, the initial three-dimensional bounding box is fitted. The initial 3D bounding box is optimized by adjusting its center position and size to make the bounding box consistent with the geometric features of the clustered point cloud and aligned with the target category information detected by 2D vision, thus obtaining the 3D target detection result.

2. The multi-source fusion navigation method based on three-dimensional target-level dynamic-static decoupling as described in claim 1, characterized in that: Based on the tracked and identified dynamic targets, and using dynamic feature discrimination constraints, dynamic points from visual and lidar observations are eliminated to obtain static observation information, including the following steps: For visual feature points in the image, dynamic feature discrimination constraints are constructed based on the consistency change of the 3D depth between the projection area of ​​the 3D detection box of the dynamic target and the feature points. Potential dynamic points are identified and eliminated to obtain static visual feature points. For points in the lidar point cloud, motion feature discrimination constraints are constructed based on the spatial constraints of the three-dimensional detection box of the dynamic target and the consistency changes between the radial velocity of the point cloud and the predicted velocity of the target. Potential dynamic points are identified and eliminated to obtain the static lidar point cloud.

3. The multi-source fusion navigation method based on three-dimensional target-level dynamic-static decoupling as described in claim 1, characterized in that: A method for identifying and eliminating potential dynamic points to obtain static visual feature points in an image, based on the consistency change in the 3D depth between the projection region of the 3D detection box of the dynamic target and the feature points, includes the following steps: The 3D detection bounding box of the dynamic target is projected onto the current image plane to form a 2D dynamic region mask; Visual feature points whose pixel coordinates fall within a two-dimensional dynamic region mask are identified as dynamic points and removed. For visual feature points that do not fall into the two-dimensional dynamic region mask, the three-dimensional position of the feature points is calculated using a multi-view geometry method as the actual position, and the current position is predicted based on the position of the previous time step and the current camera pose. Calculate the depth residual between the predicted position and the actual position. If the residual exceeds the threshold, the current feature point is regarded as a potential dynamic point and is removed, thus obtaining a static visual feature point that satisfies the static assumption.

4. The multi-source fusion navigation method based on three-dimensional target-level dynamic-static decoupling as described in claim 1, characterized in that: A method for identifying and eliminating potential dynamic points in a lidar point cloud, based on the spatial constraints of the 3D detection box of the dynamic target and the consistency change between the radial velocity of the point cloud and the predicted velocity of the target, includes the following steps: Based on the tracking and identification of dynamic targets, the current frame of LiDAR point cloud data is transformed into the same local coordinate system. If a point falls into any dynamic target detection box, it is marked as a candidate dynamic point cloud. Calculate the observed radial velocity of the candidate dynamic point cloud and calculate the residual with the projected velocity of the predicted target velocity in the line of sight direction. If the velocity residual is lower than the threshold, the current candidate dynamic point is considered a dynamic point and is removed. For point clouds that do not fall within any dynamic target 3D detection bounding box, points whose radial velocity deviates from the static assumption are discarded as dynamic points.

5. The multi-source fusion navigation method based on three-dimensional target-level dynamic-static decoupling as described in claim 1, characterized in that: Static observation information includes static visual feature points and static lidar point clouds; For the static visual feature points after removal, a visual reprojection residual term based on the reprojection error is constructed as a visual feature constraint. For the static lidar point cloud after removal, a geometric residual of the point cloud based on geometric consistency is constructed as a geometric constraint of the lidar point cloud.

6. A multi-source fusion navigation system based on three-dimensional target-level dynamic-static decoupling, based on the multi-source fusion navigation method based on three-dimensional target-level dynamic-static decoupling as described in claim 1, characterized in that, include: The 2D semantic-guided 3D target detection module is configured to acquire images and LiDAR point cloud data of the target scene, guide point cloud clustering based on 2D target information in the image, generate 3D target detection results with spatial location and category, and track 3D targets. The dynamic point removal module is configured to remove dynamic points from visual and lidar observations based on dynamic feature discrimination constraints and the tracked and identified dynamic targets to obtain static observation information. The pose estimation module is configured to construct visual feature constraints and lidar point cloud geometric constraints based on the obtained static observation information, fuse motion constraints provided by the inertial measurement unit, construct a multi-source fusion nonlinear optimization model, and obtain the pose estimation result of the navigation system after solving it. The map building and updating module is configured to build and update a map based on the pose estimation results at the current moment and the static observation information after removing dynamic points, and to decouple the dynamic and static aspects based on observation consistency constraints.

7. A multi-source fusion navigation system based on three-dimensional target-level dynamic-static decoupling, characterized in that, include: A data acquisition device and a processor, the processor being configured to perform the steps of the multi-source fusion navigation method based on three-dimensional target-level dynamic-static decoupling as described in any one of claims 1-5.

Images