Hemispherical field of view laser radar weighted NDT positioning method

Abstract

The invention relates to a hemispherical field of view laser radar weighted NDT positioning method. The hemispherical field of view laser radar weighted NDT positioning method comprises the following steps: S1, dynamic target sensing and point cloud dynamic probability calculation; S2, point cloud weighting and NDT target function construction; S3, wheel speed data fusion and pose calculation; according to the hemispherical field of view laser radar weighted NDT positioning method, the dynamic probability output by dynamic target sensing and multi-target tracking can be converted into the point-level weight, interference of dynamic points on positioning is suppressed, and the positioning precision and convergence are improved. Meanwhile, in combination with chassis wheel speed data, prior information is provided for positioning by calculating and fusing odometer information, so that the positioning stability is enhanced.

Description

A hemispherical field-of-view lidar weighted NDT localization method Technical Field

[0001] This invention relates to the field of autonomous driving technology, and in particular to a hemispherical field-of-view lidar weighted NDT positioning method. Background Technology

[0002] In autonomous driving systems, LiDAR is a crucial environmental perception sensor. Common localization methods estimate vehicle pose by matching the current frame point cloud with a prior point cloud map. The NDT algorithm represents the map using a voxel Gaussian distribution and optimizes pose by maximizing likelihood or minimizing residuals. The NDT algorithm exhibits strong robustness and real-time performance, making it suitable for online localization in complex environments.

[0003] However, in practical applications, there are numerous dynamic obstacles (such as vehicles and pedestrians). These dynamic points have a significant impact on localization matching, easily leading to increased localization errors, matching failures, or poor convergence. Traditional methods typically employ the method of eliminating dynamic points, but this approach is prone to mistakenly deleting important static structural points, especially in hemispherical sensor deployments where constraints are fewer, easily causing localization degradation.

[0004] By comparing and analyzing existing technologies, the following main problems can be summarized, which are also the problems that the technical solution of this invention aims to solve: 1. Existing dynamic point processing often identifies points first and then deletes them, rather than using dynamic probability weighting, which easily leads to the accidental deletion of static structural points, thereby reducing the stability and accuracy of registration.

[0005] 2. Existing NDTs mostly treat all points as static matching objects, lacking a weighted design for dynamic probabilities, and cannot effectively suppress the interference of dynamic points on the optimization function in dynamic scenarios.

[0006] 3. Existing methods mostly use vehicle motion information as the initial pose estimate, without effectively integrating it with point cloud matching at the level of registration optimization objectives or regularization terms.

[0007] 4. Under dynamic interference or limited field of view, existing methods lack a robust mechanism to adaptively adjust dynamic suppression and prior weights, thereby reducing localization robustness.

[0008] Therefore, a new positioning method is needed that can work robustly in dynamic environments and combine wheel speed information with point cloud matching to improve positioning accuracy and convergence speed. Summary of the Invention

[0009] Therefore, the technical problem to be solved by the present invention is to overcome the following problems in the prior art: 1. How to suppress the interference of dynamic points on NDT matching through point-level weights in a dynamic environment.

[0010] 2. How to avoid the accidental deletion of static structural points due to excessive suppression of dynamic points when the field of view is limited, thus causing positioning degradation.

[0011] 3. How to effectively integrate wheel speed data into the NDT optimization process to improve positioning accuracy and convergence speed, especially in dynamic environments.

[0012] 4. How to output results for point cloud matching even when point cloud matching is degenerate.

[0013] To address the aforementioned technical problems, this invention provides a hemispherical field-of-view lidar weighted NDT localization method, comprising the following steps: S1, Dynamic target perception and point cloud dynamic probability calculation: Real-time dynamic target detection is performed, and each dynamic target is continuously tracked to identify dynamic obstacles in the environment; S2, Point cloud weighting and NDT objective function construction: The point-level dynamic probability is calculated for each point cloud point, the point-level dynamic probability is converted into a point-level weight, and the point-level weight is introduced into the optimization objective function of NDT matching; S3, Wheel speed data fusion and pose estimation: Wheel speed data from the vehicle chassis is used as... Based on the vehicle's prior motion information, the initial relative pose of the vehicle is calculated; S4, NDT point cloud matching and optimization: The vehicle's prior motion information is incorporated into the optimization objective function of NDT matching, and the objective function is solved during the optimization process to obtain a more accurate vehicle pose; S5, degradation detection and adaptive adjustment mechanism: The quality indicators of point cloud matching are monitored in real time. When matching degradation is detected, the system can adaptively adjust the weight of the vehicle's prior motion information and the intensity of dynamic target suppression; S6, output localization results: The current localization information is output based on the optimized vehicle pose results.

[0014] The hemispherical field-of-view lidar weighted NDT positioning method of this invention can convert the dynamic probabilities of dynamic target perception and multi-target tracking outputs into point-level weights, suppressing the interference of dynamic points on positioning and improving positioning accuracy and convergence. Simultaneously, this invention combines chassis wheel speed data and calculates and fuses odometer information to provide prior information for positioning, thereby enhancing positioning stability.

[0015] In one embodiment of the present invention, step S1 includes processing the original lidar point cloud using a deep learning-based target detection model to detect dynamic targets in the environment in real time, and then using a multi-target tracking algorithm to continuously track each detected dynamic target.

[0016] In one embodiment of the present invention, step S2 above includes, S2-1, processing the current frame point cloud set. any point in the middle For each target trajectory, calculate the attribution strength of that point to the target. When the point is within the tracking target bounding box, the outward expansion is... Radar distance can vary from point to point Adaptive change, when point season ;in, The distance from the point to the center of the box is a function. For attenuation parameters, To track the target bounding box; when a point is not within the target bounding box, take... .

[0017] In one embodiment of the present invention, step S2 above includes, S2-2, for each target The dynamic confidence level of the target is obtained by fusing its detection confidence level, tracking confidence level, velocity information, and tracking status. ,Right now: ;in, To test the confidence level, To track confidence levels, For the target velocity modulus, For reference speed, For state-added items, Limit the results to Within the range.

[0018] In one embodiment of the present invention, step S2 above includes S2-3: fusing the influence of multiple targets on the same point to obtain the point-level dynamic probability of that point. Maximum fusion is employed. .

[0019] In one embodiment of the present invention, step S2 above includes, S2-4, introducing point-level weights into the optimization objective function of NDT point cloud matching, setting... For the current frame number A point, via pose parameters After transformation, it falls into the map voxel. The corresponding residual vector is The covariance of voxels is The weighted NDT objective function can then be expressed as: By minimizing The pose estimate of the current frame can be obtained, where the weights By directly adjusting the contribution of each point to the objective function, the interference of dynamic points on optimization is effectively suppressed, while static structural points still play a dominant role in the registration process.

[0020] In one embodiment of the present invention, step S3 includes: calculating the relative pose of the vehicle by combining the wheel speed data of the chassis, wherein the wheel speed data provides prior information about the vehicle's motion, and generating an initial pose estimate of the second frame point cloud by calculating the pose change at the previous moment; the initial pose estimate can be obtained in the following manner: initial pose ;in, This represents the amount of pose change estimated based on wheel speed data.

[0021] In one embodiment of the present invention, step S4 above includes: adding wheel speed prior information to the objective function through a regularization term to form an optimization objective that includes dynamic point weighting and motion prior; that is: .

[0022] In one embodiment of the present invention, the adaptive adjustment mechanism in step S5 above includes: when point cloud matching is detected to be degraded, the system will automatically adjust the optimization strategy: increase the weight of wheel speed prior to ensure that the optimization process relies more on wheel speed prior information; reduce the suppression strength of dynamic points to ensure that static points will not be deleted by mistake when dynamic interference is strong, which would lead to a decrease in positioning performance.

[0023] Compared with the prior art, the hemispherical field-of-view lidar weighted NDT positioning method of the present invention has the following advantages: 1. Significant effect on dynamic point cloud interference suppression.

[0024] The method proposed in this invention, which dynamically weights the probability of each point, can accurately assign appropriate weights to each point cloud, effectively suppressing the interference of dynamic points and avoiding the risk of mistakenly deleting static structural points. By calculating the dynamic probability of each point and assigning lower weights to dynamic points, the impact of dynamic obstacles on point cloud matching is significantly reduced. The weighting mechanism of dynamic points can effectively prevent the accidental deletion of static structural points due to the removal of dynamic points, thereby ensuring the stable operation of the positioning system.

[0025] 2. Improved positioning accuracy and convergence speed.

[0026] This invention combines chassis wheel speed data with NDT point cloud matching optimization to improve positioning accuracy and convergence speed by providing more accurate prior information for pose estimation. By fusing wheel speed data, especially in dynamic environments, the accuracy of pose estimation can be significantly improved, avoiding positioning deviations caused by initial pose errors. The prior information on wheel speed provides a more accurate initial estimate for NDT registration, thus significantly accelerating the convergence speed of the positioning algorithm.

[0027] 3. High robustness and stability in dynamic environments.

[0028] This invention introduces a dynamic probability weighting mechanism and an adaptive adjustment strategy to automatically adjust the dynamic point suppression strength and prior weights during the matching process based on the current dynamic environment. This ensures that the system can still operate stably and output accurate positioning results even under strong dynamic interference. The dynamic point weighting mechanism effectively suppresses the influence of dynamic obstacles on positioning, avoiding instability or non-convergence in dynamic environments. The system automatically adjusts the weights of the wheel speed prior and the dynamic point suppression strength based on matching degradation, maintaining high positioning stability in dynamic environments. Attached Figure Description

[0029] To more clearly illustrate the technical solutions of the embodiments of this application, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0030] Figure 1 is a flowchart of the hemispherical field-of-view lidar weighted NDT positioning method of the present invention; Figure 2 is an architecture diagram of the hemispherical field-of-view lidar weighted NDT positioning system of the present invention; Figure 3 is a flowchart of point-level dynamic probability and weight generation of the present invention; Figure 4 is a flowchart of weighted NDT iterative solution of the present invention; Figure 5 is a flowchart of degradation detection and adaptive adjustment of the present invention. Detailed Implementation

[0031] The embodiments of this application will now be described in detail with reference to the accompanying drawings.

[0032] The following specific examples illustrate the implementation of this application. Those skilled in the art can easily understand other advantages and effects of this application from the content disclosed in this specification. Obviously, the described embodiments are only a part of the embodiments of this application, and not all of them. This application can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed based on different viewpoints and applications without departing from the spirit of this application. It should be noted that, in the absence of conflict, the following embodiments and features in the embodiments can be combined with each other. Based on the embodiments in this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0033] It should be noted that the following description covers various aspects of embodiments within the scope of the appended claims. It will be apparent that the aspects described herein can be embodied in a wide variety of forms, and any particular structure and / or function described herein is merely illustrative. Based on this application, those skilled in the art will understand that one aspect described herein can be implemented independently of any other aspect, and two or more of these aspects can be combined in various ways. For example, any number and aspects set forth herein can be used to implement the device and / or practice the method. Additionally, this device and / or method can be implemented using structures and / or functionalities other than one or more of the aspects set forth herein.

[0034] It should also be noted that the illustrations provided in the following embodiments are only schematic representations of the basic concept of this application. The drawings only show the components related to this application and are not drawn according to the actual number, shape and size of the components in the actual implementation. In the actual implementation, the form, quantity and proportion of each component can be arbitrarily changed, and the layout of the components may also be more complex.

[0035] Additionally, specific details are provided in the following description to facilitate a thorough understanding of the examples. However, those skilled in the art will understand that practice can be carried out without these specific details.

[0036] In dynamic environments, LiDAR-based online positioning systems face the challenge of dynamic point cloud interference from dynamic obstacles (such as pedestrians and vehicles). In existing technologies, without effective processing of dynamic points, these points introduce incorrect matching information, leading to decreased positioning accuracy or even positioning failure. However, simply removing dynamic points may result in insufficient static structural constraints, especially in scenarios with numerous dynamic obstacles or sparse static features, which can actually reduce the robustness and accuracy of the positioning. Therefore, effectively suppressing the interference of dynamic point clouds while avoiding insufficient static structural constraints due to point cloud removal is a key technical challenge for online positioning in dynamic environments.

[0037] Therefore, the hemispherical field-of-view lidar weighted NDT localization method of the present invention includes the following steps: S1, Dynamic target perception and point cloud dynamic probability calculation: Real-time dynamic target detection is performed, and each dynamic target is continuously tracked to identify dynamic obstacles in the environment; S2, Point cloud weighting and NDT objective function construction: The point-level dynamic probability of each point cloud point is calculated, the point-level dynamic probability is converted into point-level weights, and the point-level weights are introduced into the optimization objective function of NDT matching; S3, Wheel speed data fusion and pose estimation: Based on the wheel speed data of the vehicle chassis as the prior information of vehicle motion, the pose estimation is performed. S4. Initial relative pose of the vehicle; S5. NDT point cloud matching and optimization: The vehicle's prior motion information is incorporated into the optimization objective function of NDT matching, and the objective function is solved during the optimization process to obtain a more accurate vehicle pose; S6. Degradation detection and adaptive adjustment mechanism: The quality index of point cloud matching is monitored in real time. When matching degradation is detected, the system can adaptively adjust the weight of the vehicle's prior motion information and the strength of dynamic target suppression; S7. Output localization results: The current localization information is output based on the optimized vehicle pose result. This information can be used for subsequent path planning, control decision-making and other tasks.

[0038] The core working principle of this invention lies in the precise identification and quantification of interference in dynamic point clouds, combined with robust optimization using multi-source information.

[0039] First, the dynamic target perception and point cloud dynamic probability calculation module uses target detection and tracking technology to identify dynamic obstacles in the environment in real time and calculates a continuous point-level dynamic probability for each point cloud point. This probability value reflects the likelihood that the point belongs to a dynamic target, thereby quantifying the degree of interference in the dynamic point cloud.

[0040] Secondly, the point cloud weighting and weighted NDT objective function construction mechanism transforms the aforementioned point-level dynamic probabilities into point-level weights, which are then incorporated into the optimization objective function of NDT point cloud matching. By assigning lower weights to dynamic points, the system significantly reduces the influence of these dynamic points when optimizing pose, thereby effectively suppressing the interference of dynamic point clouds on registration accuracy. This weighted suppression rather than simple elimination approach avoids the problem of insufficient static structural constraints caused by a reduction in the number of point clouds.

[0041] Meanwhile, the wheel speed data fusion and pose estimation mechanism utilizes the wheel speed data of the vehicle chassis to provide an accurate initial pose estimate. This prior information serves as a good starting point for NDT optimization, significantly improving the algorithm's convergence speed and accuracy, especially in scenarios where point cloud features are not obvious or dynamic interference is strong.

[0042] Next, a weighted NDT point cloud matching and optimization mechanism fuses the weighted NDT objective function with the wheel velocity prior information through a regularization term, forming a comprehensive optimization objective function. By minimizing this function, the system can simultaneously consider dynamic point suppression and motion prior, thereby obtaining more accurate and robust pose estimation in dynamic environments.

[0043] Finally, a degradation detection and adaptive adjustment mechanism serves as an intelligent safeguard, monitoring matching quality in real time. When matching degradation is detected, the system can adaptively adjust its optimization strategy. For example, it can increase the weight of wheel speed priors to enhance system stability, or appropriately weaken the dynamic point suppression strength to ensure that enough points still participate in matching under extreme dynamic environments, thereby further improving the system's robustness and reliability in complex environments.

[0044] Step S1 above includes processing the original LiDAR point cloud using a deep learning-based target detection model to detect dynamic targets in the environment (such as pedestrians, vehicles, etc.) in real time. Subsequently, a multi-target tracking algorithm (such as a Kalman filter) is used to continuously track each detected dynamic target.

[0045] By explicitly employing advanced deep learning object detection models and multi-object tracking algorithms, high-precision and robust identification and tracking of dynamic obstacles are ensured. This accurate dynamic object identification forms the basis for subsequent point-level dynamic probability calculations, thus guaranteeing the effectiveness of weighted suppression of dynamic point clouds. Inaccurate perception and tracking will negatively impact subsequent dynamic point processing.

[0046] Step S2 above includes, S2-1, processing the current frame point cloud set. any point in the middle For each target trajectory, calculate the attribution strength of that point to the target. When the point is within the tracking target bounding box, the outward expansion is... Radar distance can vary from point to point Adaptive change, when point season ;in, The distance from the point to the center of the box is a function. For attenuation parameters, To track the target bounding box; when a point is not within the target bounding box, take... .

[0047] This exponential decay formula provides a continuous and differentiable way to quantify the strength of point cloud points belonging to dynamic targets. It ensures that points closer to the center of the dynamic target's bounding box have a higher belonging strength, which aligns with intuitive understanding. This precise quantification makes the calculation of `q(p)` more refined and accurate, thus providing a reliable basis for subsequent dynamic point weighted suppression and avoiding errors that may arise from simple binary judgment.

[0048] The above step S2 includes, S2-2, for each target The dynamic confidence level of the target is obtained by fusing its detection confidence level, tracking confidence level, velocity information, and tracking status. ,Right now: ;in, To test the confidence level, To track confidence levels, For the target velocity modulus, For reference speed, For state-added items, Limit the results to Within the range.

[0049] By fusing multi-source information (detection, tracking, velocity, and state) into a single confidence score, this formula provides a comprehensive and robust assessment of the dynamics and reliability of tracked targets. This multifaceted information fusion helps filter out unreliable detection or tracking instances, ensuring that only highly reliable dynamic targets significantly impact point-level dynamic probabilities. This avoids erroneous dynamic point suppression caused by transient or low-confidence detections.

[0050] Step S2 above includes S2-3, fusing the influence of multiple targets on the same point to obtain the point-level dynamic probability of that point. Maximum fusion is employed. .

[0051] The `max` operator ensures that a point's dynamic probability is determined by its strongest association with any credible dynamic target. This prevents the point cloud's dynamic probability from being diluted by weakly associated or untrusted targets, thus accurately reflecting the probability that the point most likely belongs to a dynamic entity. This precise calculation is crucial for generating appropriate point-level weights, ensuring that dynamic points are effectively suppressed while avoiding oversuppression of points that may be weakly associated with multiple targets but are essentially static.

[0052] Step S2 above includes S2-4, introducing point-level weights into the optimization objective function of NDT point cloud matching, assuming... For the current frame number A point, via pose parameters After transformation, it falls into the map voxel. The corresponding residual vector is The covariance of voxels is The weighted NDT objective function can then be expressed as: By minimizing The pose estimate of the current frame can be obtained, where the weights By directly adjusting the contribution of each point to the objective function, the interference of dynamic points on optimization is effectively suppressed, while static structural points still play a dominant role in the registration process.

[0053] The above formula directly integrates the dynamic point suppression mechanism into the NDT optimization process. By multiplying the standard NDT residuals by weights, the contribution of points identified as dynamic (with lower weights) to minimizing the overall error is significantly reduced, thus effectively "reducing" their influence. This allows the optimization process to prioritize matching static features while still considering dynamic points, avoiding the problem of insufficient static constraints that might result from completely removing dynamic points. This is the core mathematical embodiment of the present invention's method of balancing dynamic suppression and static constraint preservation. When the NDT algorithm attempts to align the current LiDAR scan with a pre-built map, points from static buildings will have a strong impact on the alignment calculation, while points from moving pedestrians will have a very weak impact.

[0054] Step S3 above includes: combining the wheel speed data of the chassis to calculate the relative pose of the vehicle. The wheel speed data provides prior information about the vehicle's motion. By calculating the pose change at the previous moment, an initial pose estimate of the second frame point cloud is generated. This initial pose estimate can be obtained in the following way: initial pose ;in, This represents the amount of pose change estimated based on wheel speed data.

[0055] The above formula provides a robust and computationally inexpensive method for generating good initial guesses for NDT optimization. By utilizing the continuous and relatively accurate motion information provided by wheel speed data, initial pose estimation significantly improves the convergence speed and accuracy of the NDT algorithm, especially in scenarios where LiDAR features are not obvious or dynamic interference is strong. A good initial pose estimation can reduce the optimizer's search space and decrease the risk of it getting trapped in local optima.

[0056] Step S4 above includes: adding prior wheel speed information to the objective function through a regularization term to form an optimization objective that includes dynamic point weighting and motion priors; that is: .

[0057] This integrated objective function addresses two main challenges simultaneously: dynamic interference (via weights) and the potential sparsity / ambiguity of LiDAR features. The regularization term acts as a soft constraint, guiding the optimization results to align with vehicle odometer readings, thereby enhancing robustness and preventing drift, especially when LiDAR data alone may be insufficient or ambiguous.

[0058] The adaptive adjustment mechanism in step S5 above includes: when point cloud matching degradation is detected, the system will automatically adjust the optimization strategy: 1. Increase the weight of wheel speed prior to ensure that the optimization process relies more on wheel speed prior information, thereby enhancing the stability of the positioning system; 2. Reduce the suppression intensity of dynamic points to ensure that static points are not mistakenly deleted when dynamic interference is strong, which would lead to a decrease in positioning performance.

[0059] The aforementioned automatic adjustment and optimization strategy provides crucial robustness and fault tolerance. By dynamically adjusting the influence of wheel speed priors and the strength of dynamic point suppression, the system maintains stable and reliable positioning even in extreme or challenging environments, avoiding failures that might occur with fixed parameters. Increasing the weight of wheel speed priors leverages the inherent stability of the odometry when visual features are poor, while weakening dynamic suppression prevents over-filtering in highly dynamic scenes, ensuring the minimum point set required for matching. This intelligent adaptation significantly improves the overall reliability and operating range of the system.

[0060] This invention improves the classicity and robustness of NDT point cloud matching by introducing a dynamic probability weighted suppression mechanism to minimize the influence of dynamic points. Simultaneously, by fusing chassis wheel speed data with the point cloud matching process, it further enhances point cloud accuracy and convergence speed. This method is applicable to online LiDAR positioning systems in dynamic environments and can effectively avoid positioning degradation caused by dynamic point interference.

[0061] The implementation scheme of the present invention can be adjusted according to different application environments, conditions and needs to ensure that ideal results are achieved in practical applications.

[0062] As shown in Figure 1, the present invention first acquires the current frame point cloud collected by the hemispherical lidar, and simultaneously acquires the wheel speed encoder data and the prior NDT map. The point cloud is then preprocessed by denoising, removing invalid points (NaN), truncating distance, and removing vehicle points to improve the stability and real-time performance of subsequent detection, tracking and matching.

[0063] After preprocessing, dynamic target detection is performed on the point cloud, outputting information such as target category, target bounding box, and confidence score. Furthermore, multi-target tracking is used to obtain temporal state variables of the target, including position, velocity, tracking status, confidence score, and trajectory consistency, thereby improving the continuity of dynamic target recognition and mitigating the impact of missed and false detections on localization. Subsequently, the detection and tracking results are mapped onto each point cloud, and the dynamic probability of each point is calculated, generating point-level weights accordingly. Points with higher dynamic probabilities are assigned smaller weights to reduce their contribution in subsequent optimizations, while points with lower dynamic probabilities maintain larger weights to avoid the localization degradation caused by the erroneous deletion of static structural points and insufficient hemispherical field-of-view constraints resulting from traditional "point deletion" strategies.

[0064] Simultaneously, the system calculates the pose increment within adjacent scanning cycles based on wheel speed encoder data and generates an initial pose estimate for the current frame matching based on the pose of the previous moment. This is used to narrow the NDT optimization search space and improve convergence speed. Then, a weighted NDT optimization objective is constructed based on point-level weights, and the current frame pose is obtained through iterative solving. By incorporating dynamic points into the objective function as weights, the weighting mechanism can suppress the interference of dynamic points on the optimization objective, thereby improving matching stability and positioning accuracy in dynamic environments. Finally, degradation detection is performed on the matching process and results. When degradation is detected, the dynamic suppression strength and prior constraint strength are adaptively adjusted, for example, by increasing the wheel speed prior related weights and appropriately weakening dynamic suppression to avoid insufficient constraints due to over-suppression. Degraded results are output when necessary. Finally, the smoothed positioning pose and quality indicators are output for path planning and control.

[0065] As shown in Figure 2, the positioning system of the present invention realizes online positioning of hemispherical lidar in a modular manner. The whole system includes a dataset processing module, a dynamic target detection module, a multi-target tracking module, a weighted NDT matching module, a wheel speed increment module, and a filtering prediction module. It can also be set to link degradation detection and adaptive adjustment logic with the weighted NDT matching process. The dataset processing module preprocesses the LiDAR point cloud, including operations such as denoising, removing NaN points, distance threshold pruning, and removing vehicle point clouds, to output higher-quality point cloud data that is more relevant to localization. The dynamic target detection module performs target detection on the preprocessed point cloud, obtaining information such as bounding boxes, categories, and confidence levels of dynamic targets such as vehicles and pedestrians. The multi-target tracking module performs time-series tracking based on the detection results, outputting target position, velocity, tracking status, and reliability, which reduces the impact of missed detections and false detections on dynamic discrimination and provides stable input for point-level dynamic probabilities. The weighted NDT matching module receives the point cloud with point-level weights and the motion priors provided by the wheel speed increment module, constructs and solves the weighted NDT optimization to output pose and quality indicators. The wheel speed increment module calculates the pose increment of adjacent cycles based on the wheel speed encoder and generates initial matching values, which can also be used as prior constraints in optimization when necessary. The filtering prediction module fuses and smooths the matched output poses to obtain the final pose results for use by upper-level planning and control.

[0066] As shown in Figure 3, the point-level dynamic probability and weight generation process is centered on "target-level information pointification," converting the output of dynamic target detection and multi-target tracking into the dynamic probability of each point and further obtaining point-level weights. The system first establishes the association between points and targets based on the target bounding box and tracking trajectory, which can be achieved by using criteria such as point falling into the bounding box, distance from the point to the target center, or other association strength measures. Then, it calculates the point-level dynamic probability by integrating information such as target detection confidence, tracking stability, target velocity, and state, and performs amplitude limiting or smoothing on the dynamic probability to enhance robustness. Based on this, the dynamic probability is mapped to point-level weights, giving points with higher dynamic probabilities smaller weights and points with lower dynamic probabilities larger weights. This suppresses dynamic point interference without directly deleting points and avoids insufficient static structural constraints due to excessive culling under hemispherical field-of-view conditions.

[0067] As shown in Figure 4, the weighted NDT iterative solution process is characterized by initial wheel velocity guidance and point-level weights to suppress dynamic interference. After obtaining the initial pose, the system matches the current frame weighted point cloud with the prior NDT map, first performing voxel cell query or local construction to establish the correspondence between points and voxels; then calculating the residual term corresponding to each point and introducing point-level weights for weighted accumulation to form an optimization equation; then solving for the pose increment and updating the pose through iterative methods such as Gauss-Newton or LM, repeating the process until the convergence condition is met or the iteration limit is reached; finally, the current frame pose and quality indicators used to evaluate the reliability of the matching are output. By introducing point-level weights in the residual construction and equation accumulation stages, the influence of dynamic points on the optimization objective is suppressed, while static structural points can still provide the main geometric constraints, thereby improving the convergence and positioning accuracy in dynamic scenes.

[0068] As shown in Figure 5, the degradation detection and adaptive adjustment process ensures the continuity and stability of localization under conditions such as severe dynamic occlusion, limited field of view, or insufficient effective constraints. The system determines whether the matching has degraded based on the quality index output from the weighted NDT iterative solution, such as residual statistical characteristics, convergence state, information matrix, or covariance-related indicators. When no degradation occurs, the system outputs the pose according to the conventional parameters and enters the filtering prediction. When degradation is detected, the adaptive adjustment strategy adjusts key parameters to improve robustness, such as strengthening the constraint strength of motion priors, appropriately weakening the dynamic point suppression strength to avoid excessive suppression leading to insufficient constraints, and triggering a re-optimization to restore the matching. If the re-optimization still cannot obtain reliable results, it can output a degraded pose based on wheel speed calculation or maintain continuous output. Subsequently, the filtering prediction module performs smoothing processing and outputs the final pose and quality index.

[0069] Imagine an autonomous vehicle driving on a busy city street, using LiDAR for real-time positioning.

[0070] A car's LiDAR scanner captures static buildings and curbs, as well as moving pedestrians, bicycles, and other vehicles. If the localization system treats all these points equally, moving objects can cause the car's estimated location to "drift" or bounce irregularly, as the algorithm tries to match points whose relative positions are constantly changing. If the system simply removes all points identified as moving, there may not be enough static points to accurately determine the car's position on a very congested street, leading to poor localization accuracy or even location failure.

[0071] The technical solution of the present invention to solve the above problems is as follows: 1. Dynamic target perception and dynamic probability calculation of point cloud: When an autonomous vehicle is driving on the street, its lidar continuously scans the environment.

[0072] The dynamic target perception module (e.g., using a deep learning-based target detection model) processes the raw LiDAR point cloud to identify dynamic objects such as "pedestrians", "cars", and "bicycles".

[0073] Multi-object tracking algorithms (e.g., Kalman filters) track these detected objects over time, assigning a unique ID to each moving entity.

[0074] For each point in the LiDAR scan, the system checks whether it falls within the bounding box of a tracked dynamic object (e.g., a pedestrian). If the point is within the pedestrian's bounding box, its attribution strength to that object is calculated (e.g., based on the point's distance from the center of the bounding box).

[0075] Simultaneously, the system assesses the pedestrian's dynamic confidence level. If the pedestrian is consistently detected and tracked with high confidence and moves at a typical walking speed, the confidence level is high. If detection is unstable or movement is irregular, the confidence level may be lower.

[0076] Finally, for each point, a point-level dynamic probability is calculated.

[0077] 2. Point cloud weighting and construction of weighted NDT objective function: Based on the point-level dynamic probability, the system assigns a weight to each point.

[0078] These weights are integrated into the NDT objective function. When the NDT algorithm attempts to align the current LiDAR scan with a pre-built map, points from static buildings have a strong influence on the alignment calculation, while points from moving pedestrians have a very weak influence. This effectively “suppresses” noise introduced by dynamic objects without completely discarding useful static information that may be mixed in.

[0079] 3. Wheel speed data fusion and pose estimation: At the same time, the vehicle's internal sensors (wheel speedometers) provide information on how far the vehicle has moved and in which direction since the last lidar scan.

[0080] These wheel speed data are used to calculate an initial guess of the car's current pose.

[0081] 4. Weighted NDT Point Cloud Matching and Optimization: The NDT algorithm then performs optimization using weighted point clouds (from step S2) and initial pose guesses (from step S3). The objective function now includes both a weighted point cloud matching term and a regularization term that penalizes deviations from the wheel velocity prior.

[0082] The optimization process iteratively adjusts the vehicle's pose until the weighted point cloud is optimally aligned with the map, while maintaining consistency with the wheel speed odometer. This combined approach ensures both accuracy (from LiDAR matching) and robustness (from wheel speed odometer and dynamic suppression).

[0083] 5. Degradation Detection and Adaptive Adjustment: Suppose a car enters a very congested intersection where almost all visible points belong to moving vehicles and pedestrians, resulting in sparse static features. The system may detect low NDT matching confidence (a "degradation" event).

[0084] In response, an adaptive adjustment mechanism will be activated: it may increase the weight of wheel speed priors. This means the system will temporarily rely more on its internal motion sensors, resulting in more stable positioning even if the LiDAR data is unclear.

[0085] It may also slightly reduce the suppression strength of dynamic points. For example, instead of giving dynamic points a weight of 0.1, it increases it to 0.3. This ensures that even in extremely dynamic environments, there are still enough points involved in matching to prevent complete localization failure, even if it means a slight temporary reduction in accuracy.

[0086] Once the car has passed through the congested intersection, the static characteristics become rich again, and the system will return to its normal operating parameters.

[0087] This example demonstrates how the combination of dynamic point weighting, wheel speed fusion, and adaptive adjustment enables autonomous vehicles to maintain accurate and robust positioning even in challenging dynamic urban environments.

[0088] Based on the above-described solution, Specific Implementation 1 is: Indoor autonomous vehicle positioning (airport / hospital scenario).

[0089] This embodiment applies to autonomous vehicles equipped with hemispherical LiDAR with a field of view of approximately 180 degrees. The vehicle operates in indoor environments such as airports and hospitals, which include static structures such as walls, pillars, and shelves, as well as dynamic obstacles such as pedestrians and mobile devices. The vehicle is also equipped with wheel speed encoders and, if necessary, an IMU for attitude compensation or filtering fusion.

[0090] Referring to Figure 2, each module is run with the following parameters as an example: 1. Data preprocessing (module 1).

[0091] Statistical outlier removal filtering was employed, with a neighborhood size of 20 points and a standard deviation factor of 1.0; NaN points (invalid values) were removed; a maximum distance threshold of 30 meters was set; and the vehicle's point cloud was removed based on the vehicle's 3D model bounding box (vehicle dimensions example: length 5.0 meters, width 2.2 meters, height 2.0 meters). The preprocessing reduced the point cloud volume by an average of approximately 15%.

[0092] 2. Dynamic target detection (Module 2).

[0093] A point cloud target detection algorithm is used to identify targets such as pedestrians and small transport vehicles. A pre-trained model can be used and adapted to the scale of indoor targets; for example, pedestrian targets are 0.5m x 0.5m x 1.7m in size, and small transport vehicles are 1.5m x 0.8m x 1.2m in size. Initial dynamic probabilities are calculated for points within the target's bounding box; for example, the dynamic probability for a pedestrian point can be set to 0.9 and updated during subsequent tracking.

[0094] 3. Multi-target tracking (Module 3).

[0095] A tracking method combining Kalman filtering and data association is employed to maintain the target's position, velocity, and tracking status. When target occlusion causes two consecutive frames to be undetected, compensation prediction is performed based on historical velocity vectors. For targets with velocities less than 0.1 m / s for more than 2 seconds, they can be marked as static and their dynamic probability reduced (e.g., to below 0.2) to avoid misjudgment.

[0096] 4. Point-level weights (corresponding to Figure 3).

[0097] Map detection and tracking results to the point level.

[0098] 5. Weighted NDT matching (corresponding to Figure 4, module 4).

[0099] The voxel grid size can be 1.0 meter; point-level weights are incorporated into residual accumulation and the pose is solved iteratively. Compared with unweighted NDT, it can significantly reduce trajectory jitter and improve convergence stability under dynamic disturbances such as pedestrian crossings and vehicle intersections.

[0100] 6. Wheel speed increment (Module 5).

[0101] The wheel speed data reading frequency is 50Hz; the wheel radius is 0.35 meters and the wheel track is 1.8 meters. The differential speed model is used to calculate the relative displacement and heading changes within a point cloud scanning cycle of 0.1 seconds, which is used as the initial pose input for NDT, thereby reducing the search space and accelerating convergence.

[0102] 7. Filtering Prediction (Module 6).

[0103] An extended Kalman filter is used to fuse and smooth the NDT output pose. The state variables can be 6-dimensional (position and Euler angles), and the observation is the NDT pose output. The smoothed pose can be used for control and planning.

[0104] 8. Degeneracy and Adaptation (corresponding to Figure 5).

[0105] Monitor matching quality metrics (e.g., information matrix or covariance eigenvalues); when degradation is detected (e.g., minimum eigenvalue less than 1e-4), increase the prior correlation weight of wheel speed and appropriately weaken the dynamic suppression strength to avoid excessive suppression leading to insufficient constraints; if necessary, perform re-optimization or output wheel speed prediction degradation results.

[0106] Example of implementation results: When pedestrians cross the road, the trajectory fluctuation amplitude is reduced by about 50% compared with the traditional NDT after adopting the weighted mechanism; the number of iterations is reduced by an average of about 35% after combining wheel speed prior, and the convergence time is shortened by about 40%; the densely populated hall can continue to operate stably for more than 4 hours without location loss or serious jumps.

[0107] Specific embodiment 2 is: Industrial warehouse robot positioning and navigation.

[0108] This embodiment applies to an automated robot in an industrial warehouse. The robot is equipped with a hemispherical LiDAR (approximately 180-degree field of view) and a wheel speed encoder. The warehouse contains workers and static structures such as walls and shelves. The robot needs to perform localization, obstacle avoidance, and path planning in a dynamic environment.

[0109] The system runs as shown in Figure 2, and the process is executed cyclically as shown in Figure 1. The key points are as follows: 1. Point cloud preprocessing.

[0110] The point cloud is denoised, NaN removed, distance limited, and self-point removed to improve the quality of dynamic detection and registration.

[0111] 2. Dynamic detection and tracking.

[0112] Detect and track dynamic targets such as staff; for targets with low and stable movement speed, the dynamic probability can be reduced to avoid misclassifying static facilities as dynamic targets.

[0113] 3. Point-level weights and weighted NDT.

[0114] Dynamic probabilities are mapped to point-level weights. The weights of dynamic points are reduced while the weights of static structural points remain high, so that the robot can still maintain registration constraints by relying on static structures such as walls and shelves when there are many people.

[0115] 4. Initial wheel speed and adaptive degradation.

[0116] Wheel speed estimation provides initial pose to improve convergence speed; when the warehouse passage is narrow, the field of view is limited, or dynamic occlusion is severe and causes degradation, the prior weight is increased and the dynamic suppression intensity is weakened through the mechanism shown in Figure 5. If necessary, wheel speed prediction is output to reduce pose and ensure navigation continuity.

[0117] 5. Filtered prediction output.

[0118] The registered pose is filtered and smoothed before being output for use by the navigation and control module.

[0119] Obviously, the above embodiments are merely illustrative examples for clear explanation and are not intended to limit the implementation. Those skilled in the art will recognize that other variations or modifications can be made based on the above description. It is neither necessary nor possible to exhaustively list all possible implementations here. However, obvious variations or modifications derived therefrom are still within the scope of protection of this invention.

Claims

1. A hemispherical field-of-view lidar weighted NDT positioning method, characterized in that, Includes the following steps: S1. Dynamic Target Perception and Point Cloud Dynamic Probability Calculation: Real-time dynamic target detection and continuous tracking of each dynamic target, identifying dynamic obstacles in the environment; S2. Point Cloud Weighting and NDT Objective Function Construction: Calculate the point-level dynamic probability for each point cloud point, convert the point-level dynamic probability into point-level weights, and introduce the point-level weights into the optimization objective function of NDT matching; S3. Wheel Speed ​​Data Fusion and Pose Inference: Utilize the wheel speed data of the vehicle chassis as prior information for vehicle motion to infer the initial relative pose of the vehicle; S4. NDT Point Cloud Matching and Optimization: The vehicle's prior motion information is incorporated into the optimization objective function of NDT matching. The objective function is then solved during the optimization process to obtain a more accurate vehicle pose. S5. Degradation Detection and Adaptive Adjustment Mechanism: The quality indicators of point cloud matching are monitored in real time. When matching degradation is detected, the system can adaptively adjust the weight of the vehicle's prior motion information and the strength of dynamic target suppression. S6. Output Localization Results: The current localization information is output based on the optimized vehicle pose.

2. The hemispherical field-of-view lidar weighted NDT positioning method according to claim 1, characterized in that: Step S1 above includes processing the original LiDAR point cloud using a deep learning-based target detection model to detect dynamic targets in the environment in real time, and then using a multi-target tracking algorithm to continuously track each detected dynamic target.

3. The hemispherical field-of-view lidar weighted NDT positioning method according to claim 1, characterized in that: Step S2 above includes, S2-1, processing the current frame point cloud set. any point in the middle For each target trajectory, calculate the attribution strength of that point to the target. ； When the point is within the tracking target bounding box, the outward expansion... Radar distance can vary from point to point Adaptive change, when point season ;in, The distance from the point to the center of the box is a function. For attenuation parameters, To track the target bounding box; When the point is not within the tracking target bounding box, take 。 4. The hemispherical field-of-view lidar weighted NDT positioning method according to claim 3, characterized in that: The above step S2 includes, S2-2, for each target The dynamic confidence level of the target is obtained by fusing its detection confidence level, tracking confidence level, velocity information, and tracking status. ,Right now: ;in, To test the confidence level, To track confidence levels, For the target velocity modulus, For reference speed, For state-added items, Limit the results to Within the range.

5. The hemispherical field-of-view lidar weighted NDT positioning method according to claim 4, characterized in that: Step S2 above includes S2-3, fusing the influence of multiple targets on the same point to obtain the point-level dynamic probability of that point. Maximum fusion is employed. 。 6. The hemispherical field-of-view lidar weighted NDT positioning method according to claim 5, characterized in that: Step S2 above includes S2-4, introducing point-level weights into the optimization objective function of NDT point cloud matching, assuming... For the current frame number A point, via pose parameters After transformation, it falls into the map voxel. The corresponding residual vector is The covariance of voxels is The weighted NDT objective function can then be expressed as: By minimizing The pose estimate of the current frame can be obtained, where the weights By directly adjusting the contribution of each point to the objective function, the interference of dynamic points on optimization is effectively suppressed, while static structural points still play a dominant role in the registration process.

7. The hemispherical field-of-view lidar weighted NDT positioning method according to claim 1, characterized in that: Step S3 above includes: combining the wheel speed data of the chassis to calculate the relative pose of the vehicle. The wheel speed data provides prior information about the vehicle's motion. By calculating the pose change at the previous moment, an initial pose estimate of the second frame point cloud is generated. This initial pose estimate can be obtained in the following way: initial pose ;in, This represents the amount of pose change estimated based on wheel speed data.

8. The hemispherical field-of-view lidar weighted NDT positioning method according to any one of claims 1-7, characterized in that: Step S4 above includes: adding prior wheel speed information to the objective function through a regularization term to form an optimization objective that includes dynamic point weighting and motion priors; that is: 。 9. The hemispherical field-of-view lidar weighted NDT positioning method according to any one of claims 1-7, characterized in that: The adaptive adjustment mechanism in step S5 above includes: when point cloud matching is detected to be degraded, the system will automatically adjust the optimization strategy: increase the weight of wheel speed prior to ensure that the optimization process relies more on wheel speed prior information; reduce the suppression strength of dynamic points to ensure that static points are not mistakenly deleted when dynamic interference is strong, which would lead to a decrease in positioning performance.

Images