Fusion positioning method and system based on laser radar ICP confidence factor

Abstract

The invention discloses a fusion positioning method and system based on a laser radar ICP confidence factor. The method comprises the steps of obtaining point cloud data output by an AGV vehicle-mounted laser radar and map information of a scene map where the AGV vehicle-mounted laser radar is located; based on the point cloud data and the map information, point cloud registration is completed through an ICP algorithm to determine an ICP pose, and an ICP confidence factor is calculated according to a matching error, convergence characteristics, a characteristic overlapping degree and point cloud quality; adjusting an observation covariance matrix based on the ICP confidence factor; based on the AGV pose, the ICP pose and the observation covariance matrix, constructing an observation noise model, and outputting fused pose information; the AGV poses comprise the AGV pose of the wheel type odometer and the AGV pose of the inertial measurement unit. According to the method, a dynamic mapping mechanism from the ICP confidence factor to the observation covariance is introduced into the EKF local fusion framework, mismatching and trajectory jump are inhibited under the working conditions of low overlapping / shielding / noise fluctuation and the like, and the convergence speed and precision are improved under the scene of full geometric constraint.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, and more specifically to a fusion positioning method and system based on LiDAR ICP confidence factor. Background Technology

[0002] Currently, with the continuous deepening of automation and intelligent logistics systems in modern factories, traditional manual transportation and semi-automated operation modes can no longer meet the demands of efficient, precise, and flexible industrial scenarios. Against the backdrop of intelligent manufacturing and flexible production gradually becoming mainstream, the widespread application of automated control equipment has not only significantly improved production efficiency and logistics response speed but also injected strong momentum into the high-quality development of the social economy. As the core equipment of intelligent logistics systems, Automated Guided Vehicles (AGVs), with their autonomous navigation capabilities, flexible scheduling, and high reliability, are gradually replacing traditional transportation methods and are widely used in various aspects such as warehouse management, production line material distribution, and finished product transfer. Currently, the mainstream guidance methods for AGVs mainly include magnetic strip guidance, QR code guidance, and laser guidance. Among them, laser-guided AGVs, equipped with lidar, can scan the surrounding environment in real time and build a map, achieving precise positioning, navigation, and motion control.

[0003] Compared to traditional guidance methods, laser guidance exhibits significant advantages: First, it eliminates the need for physical markers such as magnetic strips or QR codes, greatly reducing site modification and maintenance costs. Second, laser radar's high resolution and rapid response capabilities enable AGVs to achieve stable and high-precision positioning in dynamic and complex industrial environments, demonstrating stronger environmental adaptability. These characteristics provide crucial technological support for the intelligent transformation and flexible manufacturing of factories, further propelling modern industry towards greater efficiency and flexibility.

[0004] However, traditional ICP algorithms heavily rely on the structural features of the scene. In practical applications, when the laser point cloud is affected by reflections, occlusions, or moving objects, matching degradation can easily occur, leading to significant deviations or even divergence in pose estimation, severely impacting the system's positioning accuracy and stability. To improve the system's robustness in complex environments, a common approach is to fuse multiple sensors, such as inertial measurement units (IMUs) and wheeled odometry. However, IMUs themselves suffer from zero-bias drift, and odometry also has accumulated errors. Simply using them as the primary positioning source makes it difficult to maintain accuracy over the long term.

[0005] Therefore, how to avoid the matching degradation problem of the lidar ICP algorithm in complex industrial scenarios while effectively compensating for the influence of IMU zero bias drift and wheel odometer cumulative error, and how to achieve long-term, high-precision, and highly robust positioning of AGV by dynamically optimizing the fusion weight of multi-source sensor data, is a problem that urgently needs to be solved by those skilled in the art. Summary of the Invention

[0006] In view of the above problems, the present invention proposes a fusion positioning method and system based on lidar ICP confidence factor to overcome or at least partially solve the above problems.

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

[0008] In a first aspect, embodiments of the present invention provide a fusion localization method based on lidar ICP confidence factor; specifically including the following steps: S1. Obtain the point cloud data output by the AGV's onboard LiDAR and the map information of the scene. S2. Based on the point cloud data and map information, the point cloud registration is completed using the ICP algorithm to determine the ICP pose, and the ICP confidence factor is calculated based on the matching error, convergence characteristics, feature overlap, and point cloud quality. S3. Adjust the observation covariance matrix based on the ICP confidence factor; S4. Based on the AGV pose, ICP pose, and observation covariance matrix, construct an observation noise model and output the fused pose information; the AGV pose includes: the AGV pose of the wheeled odometer and the AGV attitude of the inertial measurement unit.

[0009] Furthermore, it also includes: S5. Generate a global navigation path based on the fused pose information and the pre-built scene map, optimize the local path by combining real-time sensor data, and output speed commands.

[0010] Furthermore, in step S2, the ICP confidence factor is formed by weightedly fusing the results of the matching error function, convergence function, overlap function, and point cloud quality function to create a comprehensive confidence index, the function of which is expressed as:

[0011] In the formula, For ICP confidence factor, For the matching error function, For convergence function, For overlap function, For point cloud quality function, , , , These are the weighting coefficients of the corresponding functions; The ICP confidence factor is smoothed over time using an exponential moving average method.

[0012] In the formula, The confidence factor after smoothing at the current time. This is the smoothed result from the previous time step. For smoothing parameters.

[0013] In step S3, the observed covariance matrix is ​​adjusted based on the ICP confidence factor. The adjustment method is expressed as follows:

[0014] In the formula, The observed covariance matrix is ​​based on the confidence factor. For its maximum covariance, It represents the minimum covariance.

[0015] Furthermore, the calculation process of the matching error function is as follows: ,

[0016] In the formula, For rotation matrix, It is a translation matrix. For source point cloud, For the target point cloud, For vector difference, This represents the length difference.

[0017] Add a robust weight To prevent individual defects from affecting the overall effect, For control parameters;

[0018] Calculate the weighted root mean square error :

[0019] Calculate the matching error function :

[0020] EMSEmax This represents the maximum acceptable error.

[0021] Furthermore, the convergence function is composed of a weighted sum of the iteration number function, the error reduction ratio function, and the error curve stationarity function; Wherein: the iteration number function is used to reflect the relationship between the actual number of iterations and the maximum number of iterations, and its formula is:

[0022] In the formula, Let be the function of the number of iterations. For the number of iterations, This represents the maximum number of iterations. The error reduction ratio function reflects the ratio of the initial residual to the final residual, and its formula is:

[0023] In the formula, Let the error decrease proportional function be the function of the error reduction ratio. This is the initial iteration error. This represents the final iteration error; The error curve stationarity function is used to reflect the stability of the error curve during the iteration process, and its formula is:

[0024]

[0025] In the formula, Let be the stationarity function of the error curve. for Error metric for each iteration As a measure of the stability of the error sequence, This is a stability penalty coefficient. To avoid division by zero; The convergence confidence score is obtained by weighting and synthesizing the iteration number function, the error reduction ratio function, and the error curve stationarity function, and its formula is expressed as:

[0026] In the formula, C s For convergence function, , , These are the weights of the three indicators mentioned above.

[0027] The expression for the overlap function is:

[0028] In the formula, For overlap function, The number of valid points participating in the matching. This represents the number of points where the residual is less than the threshold.

[0029] The expression for the point cloud quality function is:

[0030] In the formula, For point cloud quality function, For noise weights, For point cloud noise, Weight loss due to point cloud This represents the point cloud loss rate.

[0031] Furthermore, the specific process of determining the ICP pose through point cloud registration using the ICP algorithm is as follows: The polar coordinate data output by the lidar is converted into rectangular coordinate data and then mapped to the global coordinate system; The point cloud data to be matched is normalized. Based on the normalized point cloud data, the corresponding point pairs between point clouds are determined, and an error function based on the point-to-point model is constructed. Solve for the optimal rigid body transformation; Repeat the corresponding point pair and solve the optimal rigid body transformation to reduce the error function until the convergence condition is met; Output the rotation matrix and translation vector, and analyze them to obtain the ICP pose.

[0032] Furthermore, the specific process of constructing the observation noise model and outputting the fused pose information is as follows: Define the system state vector and set the observation noise; Receive the AGV pose output from the inertial measurement unit and the AGV pose output from the wheel odometer, and recursively calculate the prior state according to the discrete kinematics model; The prior covariance matrix is ​​calculated using the Jacobian matrix of the state through the state transition function. Receive the ICP pose and the observation covariance matrix dynamically adjusted by the ICP confidence factor; Construct the observation vector and observation matrix; Solving for the deviation between observed values ​​and prior predictions reflects the difference between prior predictions and actual observations; Calculate the residual covariance matrix, and then solve for the Kalman gain using the residual covariance matrix; By combining Kalman gain and observation bias, the prior state is corrected to obtain the fused posterior state, and the covariance matrix is ​​updated. The fused pose information and posterior covariance matrix are output to the navigation stack.

[0033] Secondly, embodiments of the present invention provide a fusion positioning system based on lidar ICP confidence factor, comprising: The ICP point cloud matching module receives point cloud data output from the AGV's LiDAR and map information from the scene map. It completes point cloud registration and determines the ICP pose through the ICP algorithm, and calculates the ICP confidence factor based on matching error, convergence characteristics, feature overlap, and point cloud quality. The observation covariance matrix is ​​then adjusted using the ICP confidence factor. An extended Kalman filter module is used to receive the AGV pose from the wheeled odometer, the AGV attitude from the inertial measurement unit, and the ICP pose and observation covariance matrix output by the ICP point cloud matching module to construct an observation noise model; by recursively updating the prediction equation and the observation equation, dynamic fusion of multi-source data is achieved, and the fused pose information is output.

[0034] Furthermore, the system also includes: The navigation stack receives the fused pose information output by the extended Kalman filter module, generates a global navigation path based on the map information of the pre-built scene map, optimizes the local path by combining real-time sensor data, and outputs speed commands.

[0035] As can be seen from the above technical solution, compared with the prior art, the present invention discloses a fusion positioning method based on lidar ICP confidence factor, which has the following beneficial effects: This invention relates to a fusion localization method based on lidar ICP confidence factor. It introduces a dynamic mapping mechanism from ICP confidence factor to observation covariance into the EKF local fusion framework. This mechanism integrates matching error, convergence characteristics, overlap, point cloud quality, and other indicators, and smooths them over time. It adaptively adjusts the ICP measurement weights, enabling the system to suppress mismatches and trajectory jumps under conditions of low overlap, occlusion, and noise fluctuations, and improve convergence speed and accuracy in scenarios with sufficient geometric constraints. Simultaneously, the navigation stack performs path planning and local obstacle avoidance based on the fused pose information, and outputs speed commands. Attached Figure Description

[0036] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0037] Figure 1 This is a flowchart of the fusion localization method provided in the embodiments of the present invention; Figure 2 This is a structural diagram of the fusion positioning system provided in an embodiment of the present invention. Detailed Implementation

[0038] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0039] This invention discloses a fusion localization method based on lidar ICP confidence factor, such as... Figure 1 As shown, the specific steps include: S1. Obtain the point cloud data output by the AGV's onboard LiDAR and the map information of the scene. S2. Based on the point cloud data and map information, the point cloud registration is completed using the ICP algorithm to determine the ICP pose, and the ICP confidence factor is calculated based on the matching error, convergence characteristics, feature overlap, and point cloud quality. S3. Adjust the observation covariance matrix based on the ICP confidence factor; S4. Based on the AGV pose, ICP pose, and observation covariance matrix, construct an observation noise model and output the fused pose information; the AGV pose includes: the AGV pose of the wheeled odometer and the AGV attitude of the inertial measurement unit. S5. Generate a global navigation path based on the fused pose information and the pre-built scene map, optimize the local path by combining real-time sensor data, and output speed commands.

[0040] This method introduces a dynamic mapping mechanism from ICP confidence factor to observation covariance into the EKF local fusion framework. It integrates indicators such as matching error, convergence characteristics, overlap, and point cloud quality, and performs time smoothing to adaptively adjust the ICP measurement weight. This suppresses mismatches and trajectory jumps under conditions of low overlap, occlusion, and noise fluctuations, and improves convergence speed and accuracy in scenarios with sufficient geometric constraints. At the same time, the navigation stack performs path planning and local obstacle avoidance based on the fused pose information and outputs speed commands.

[0041] The following is a detailed description of each of the above steps; In step S1, the point cloud data output by the AGV's onboard LiDAR and the map information of the surrounding scene are acquired. The map information is a pre-built or online-updated environmental map used to describe the spatial structure and passable areas within the AGV's operating area, including environmental boundaries, obstacle locations, travel paths, and grid occupancy information. The specific process is as follows: Acquire point cloud data from the onboard LiDAR of the Automated Guided Vehicle (AGV) at the automated guided vehicle (AGV) and convert the acquired point cloud data into polar coordinates. x n =r n cos n , yn=rnsin nThe polar coordinate data of the LiDAR is converted into Cartesian coordinate data. Coordinate transformation is then used to map the LiDAR Cartesian point cloud data to the global coordinate system, achieving uniformity in point cloud positions and ensuring consistent obstacle locations.

[0042] In step S2, based on the point cloud data and map information, the ICP algorithm is used to complete point cloud registration and determine the ICP pose. The ICP confidence factor is then calculated based on the matching error, convergence characteristics, feature overlap, and point cloud quality. The specific process is as follows: S2.1. Determine the ICP pose by performing point cloud registration using the ICP algorithm: The point cloud data to be matched is normalized to ensure the stability and accuracy of the matching calculation. By eliminating the overall translation of the point cloud, a set of feature points that are only related to the internal geometric distribution of the point cloud is obtained, which provides a basis for solving the rotation matrix and rigid body transformation in the future. Target point cloud A and source point cloud B respectively contain n There are discrete points, where target point cloud A represents point cloud data of scene features such as obstacles in the scene map, provided by the SLAM map, and source point cloud B represents point cloud data actually scanned by the LiDAR, denoted as:

[0043] Calculate the geometric center A of the target point cloud A and the source point cloud B respectively. c and B c :

[0044] Subtracting the geometric center of the corresponding point cloud from each point yields the normalized point set:

[0045] In the above formula, the set and These are the centering results for the target point cloud A and the source point cloud B, respectively. This result eliminates the overall translation of the point clouds, normalizing their geometric centers to the origin.

[0046] The normalized point set can be used to calculate the rotation matrix and the optimal rigid body transformation, thereby achieving matching and alignment between point clouds; while the original point cloud and its geometric center can be used to restore the matching results in the global coordinate system.

[0047] After completing the point cloud normalization process, this invention further determines the corresponding point pairs between point clouds. A corresponding point pair refers to the pair of points that are closest to each other in two sets of point clouds, and this correspondence is the core of the ICP algorithm.

[0048] To improve registration stability, a nearest neighbor correspondence method is adopted, using the AGV pose from the wheeled odometer and the AGV attitude from the IMU as initial values. After transforming the current frame point cloud to map coordinates according to the predicted pose, the nearest neighbor corresponding point is found in the index structure, and distance thresholds and normal angle / intensity thresholds are set to exclude outliers. Subsequently, a robustly weighted point-to-point objective function is used to iteratively solve the rigid body transformation. When the convergence threshold or iteration upper limit is reached, the pose observation and statistics such as RMSE and inlier ratio are output for subsequent confidence assessment and gating. The frequency of the 2D LiDAR is 10-20Hz, the IMU frequency is 80Hz, and the odometer frequency is 40Hz.

[0049] For the normalized point set and ,in For the target point cloud One point, For the current point cloud A two-dimensional point. For any point... Its corresponding point The selection of satisfies the following optimization criteria:

[0050] That is, select the points that are closest in the Euclidean distance as the corresponding point pair.

[0051] After establishing the corresponding point pairs in the point cloud, an error function based on the point-to-point model is further constructed:

[0052] In the above formula, The overall error function, It is a two-dimensional rotation matrix. It is a translation vector.

[0053] By constructing the correlation matrix:

[0054] In the above formula, The correlation matrix between the source point cloud and the target point cloud. for The transpose of a point.

[0055] For matrix H Perform singular value decomposition:

[0056]

[0057] In the above formula, for The left singular vector matrix; It is a singular value matrix that describes the degree of dispersion of point pairs along the principal direction; for The right singular vector matrix; The rotation matrix obtained during construction ensures that the squared error is minimized during point cloud registration.

[0058] in, The specific form is:

[0059] In the above formula, The rotation angle; Translation vector The calculation formula is:

[0060] In the above formula, For target point cloud The geometric center; Source Point Cloud The geometric center.

[0061] The calculated rotation matrix With translation vector Applying this to the source point cloud collection yields an updated source point cloud. :

[0062] After updating the point cloud, the process of determining corresponding point pairs and solving for the optimal rigid body transformation is repeated to gradually reduce the error function. The convergence criteria are set as follows: (12) In the above formula, For the first The error function value of the next iteration. This is the error value from the previous iteration. For the error threshold, For the number of iterations, This represents the maximum allowed number of iterations.

[0063] The above formula has a mean error threshold. for Maximum number of iterations For 100 times.

[0064] When the convergence condition is met, the final rotation matrix is ​​output. With translation vector This achieves optimal registration between the target point cloud and the source point cloud.

[0065] The ICP pose of the AGV in the map coordinate system can be represented by the ICP algorithm. This is derived from the translation vector. Get position coordinates , by rotation matrix Decomposition yields orientation angle This will output the location result. .

[0066] S2.2 Calculate the ICP confidence factor based on matching error, convergence characteristics, feature overlap, and point cloud quality: S2.2.1 Matching error function; The matching error function is used to evaluate the geometric consistency of point cloud registration. Specifically, it calculates the average deviation between the source point cloud and corresponding points in the target point cloud after applying rotation and translation transformations. To reduce the impact of outlier matching points on the overall result, robustness weights are introduced into the error calculation for correction. During the ICP algorithm, the source point cloud is mapped to the target point cloud coordinate system using rotation and translation vectors, and the registration error is measured based on the difference between the transformed points and the target points.

[0067] , (13) In the above formula, For rotation matrix, It is a translation matrix. For source point cloud, For the target point cloud, For vector difference, This represents the length difference.

[0068] Add a robust weight To prevent individual defects from affecting the overall effect, For control parameters;

[0069] Calculate the weighted root mean square error :

[0070] Find the matching error function , The maximum acceptable error:

[0071] S2.2.2, Convergence function; The convergence function is used to measure the convergence performance of the ICP iteration process. It is mainly evaluated from three aspects: the iteration number function, the error reduction ratio function, and the error curve stationarity function. The iteration count function reflects the relationship between the actual number of iterations and the maximum number of iterations. Its formula is:

[0072] In the above formula, Let be the function of the number of iterations. For the number of iterations, This represents the maximum number of iterations. The error reduction ratio function reflects the ratio of the initial residual to the final residual, and its formula is:

[0073] In the above formula, Let the error decrease proportional function be the function of the error reduction ratio. This is the initial iteration error. This represents the final iteration error; The error curve stationarity function is used to reflect the stability of the error curve during the iteration process, and its formula is:

[0074]

[0075] In the above formula, Let be the stationarity function of the error curve. for Error metric for each iteration As a measure of the stability of the error sequence, This is a stability penalty coefficient. To avoid division by zero.

[0076] The convergence confidence score is obtained by weighting and synthesizing the iteration number function, the error reduction ratio function, and the error curve stationarity function. Its formula can be expressed as:

[0077] In the above formula, For a convergent iterative function, , , These are the weights of the iteration number function, the error reduction ratio function, and the error curve stationarity function, respectively. The weight parameters in the above formula , , The values ​​are 0.25, 0.40, and 0.35, respectively.

[0078] S2.2.3, Overlap function; The overlap function measures the effective overlap between the source and target point clouds in geometric space. ICP registration relies on effective constraint information between the two sets of point clouds; therefore, the larger the overlap area between the point clouds, the higher the reliability of the matching result. Conversely, when the point cloud overlap is low, ICP is prone to misregistration or convergence to a local optimum. Its functional expression is:

[0079] In the above formula, For overlap function, The number of valid points participating in the matching. This represents the number of points where the residual is less than the threshold.

[0080] S2.2.4, Point Cloud Quality Function; The point cloud quality function is used to evaluate the observation quality of the input point cloud. Point cloud quality is affected by various factors, including sensor noise, occlusion, and environmental interference. When the point cloud noise is too high or the proportion of missing points is too high, the stability and accuracy of ICP matching results will decrease significantly. Therefore, it is necessary to constrain it using the point cloud quality function. The function can be expressed as:

[0081] In the above formula, For point cloud quality function, For noise weights, For point cloud noise, Weight loss due to point cloud Point cloud loss rate; In the above formula, noise weight The value is 0.4, indicating that the point cloud has lost weights. It is 0.6.

[0082] S2.2.5, ICP confidence factor; The ICP confidence factor is used to quantitatively evaluate the overall reliability of ICP registration results. This confidence factor is formed by weighted fusion of the results from the matching error function, convergence function, overlap function, and point cloud quality function, resulting in a comprehensive reliability index. Its function expression is as follows:

[0083] In the above formula, For ICP confidence factor, For the matching error function, For convergence characteristic function, For overlap function, For point cloud quality function, , , , These are the weighting coefficients; In the above formula, the weight coefficients of each function , , , The values ​​are 0.35, 0.25, 0.25, and 0.15, respectively, and their sum is 1.

[0084] S2.2.6 Mapping of confidence factors to observed covariance matrix: To suppress the impact of single-frame noise and occasional anomalies on confidence assessment, Perform an exponential moving average to obtain the smoothing confidence factor. :

[0085] In the above formula, The confidence factor after smoothing at the current time. This is the smoothed result from the previous time step. For smoothing parameters; In the above formula, the smoothing parameter The value is 0.8.

[0086] In step S3, the observation covariance matrix is ​​adjusted based on the ICP confidence factor; The smoothed ICP confidence factor is used to dynamically adjust the observation covariance matrix of the output ICP pose. The adjustment method can be expressed as follows:

[0087] In the above formula, The observed covariance matrix is ​​based on the confidence factor. For its maximum covariance, Minimum covariance; In the above formula, the maximum covariance minimum covariance .

[0088] In step S4, an observation noise model is constructed based on the AGV pose, ICP pose, and observation covariance matrix, and the fused pose information is output; the AGV pose includes: the AGV pose of the wheeled odometer and the AGV attitude of the inertial measurement unit. A fusion positioning method based on extended Kalman filter (EKF) is adopted, in which the inertial measurement unit (IMU) and the wheeled odometer output the AGV attitude and AGV pose, respectively, for prior prediction of the system state, and the ICP pose obtained by the lidar based on ICP registration is used as the observation for posterior update.

[0089] State definition and noise setting: In a planar motion scenario, the system state vector is defined as follows: Where [x,y] represents the position. For heading angle, Linear velocity, ω is the angular velocity.

[0090] The AGV attitude from the IMU and the AGV pose from the wheeled odometer are used as predicted values, according to the discrete kinematics model:

[0091]

[0092]

[0093] In the above formula, , , They are respectively Time prediction and estimation Let it be denoted as the discrete time index. For the previous moment, The sampling interval; In the above formula, the sampling interval Timelines from IMU and odometer.

[0094] Obtain the prior state ,make Characterizing the state transition function For the Jacobian matrix of the state, the prior covariance matrix is ​​expressed as:

[0095] In the above formula, To predict the covariance matrix, the error of the predicted state is measured; For process noise covariance; For nonlinear functions Regarding state variables Taking the partial derivative yields a matrix.

[0096] The ICP pose output by the ICP point cloud matching module is used as the observation value to construct the observation vector, observation model, and covariance matrix:

[0097]

[0098]

[0099]

[0100] In the above formula, For observation vectors; The observation matrix; For observation purposes, noise is usually assumed to be zero-mean Gaussian noise; To observe the covariance matrix, This represents the observation variance.

[0101] Calculate the deviation between observed and predicted values :

[0102] Creating covariance Matrix and calculate Kalman gain :

[0103]

[0104] Calculate the final output localization and covariance matrix:

[0105] In the above formula, It is the identity matrix. For the fused pose information, Let be the covariance matrix.

[0106] In step S5, a global navigation path is generated based on the fused pose information and the map information of the pre-built scene map. Local path optimization is performed by combining real-time sensor data, and a speed command is output. After receiving the fused positioning information output by EKF, the AGV pose is first published at a unified time coordinate and the cost map is updated. Then, the global path to the target is verified or generated, and the trajectory is optimized in combination with the local environment. The linear velocity and angular velocity control commands that meet the kinematic and safety constraints are output. If the path or optimization is not feasible, the recovery action is triggered and replanning is performed.

[0107] This invention relates to a fusion positioning method based on lidar ICP confidence factors. First, the ICP module calculates the ICP confidence factor based on matching error, convergence characteristics, overlap, and point cloud quality. Then, it monotonically maps this confidence factor to an observation covariance matrix, driving an EKF to adaptively weight and fuse the IMU, wheeled odometer, and ICP pose, outputting a stable positioning result with uncertainty. The navigation stack then performs path planning and local obstacle avoidance, generates control commands, and replans when infeasibility occurs. This scheme effectively suppresses mismatches and trajectory jumps under conditions of low overlap, occlusion, and noise fluctuations. In scenarios with sufficient geometric constraints, it improves convergence efficiency and positioning accuracy. It has a simple structure, adjustable parameters, and is easily embedded into existing AGV systems, making it suitable for autonomous navigation in complex dynamic environments.

[0108] Based on the same inventive concept, embodiments of the present invention also provide a fusion positioning system based on lidar ICP confidence factor; such as Figure 2 As shown, it includes: The ICP point cloud matching module receives point cloud data output from the AGV's LiDAR and map information from the scene map. It completes point cloud registration and determines the ICP pose using the ICP algorithm, and calculates the ICP confidence factor based on matching error, convergence characteristics, feature overlap, and point cloud quality. The observation covariance matrix is ​​then adjusted using the ICP confidence factor.

[0109] An extended Kalman filter module is used to receive the AGV pose from the wheeled odometer, the AGV attitude from the inertial measurement unit, and the ICP pose and observation covariance matrix output by the ICP point cloud matching module to construct an observation noise model; by recursively updating the prediction equation and the observation equation, dynamic fusion of multi-source data is achieved, and the fused pose information is output.

[0110] The navigation stack receives the fused pose information output by the extended Kalman filter module, generates a global navigation path based on the map information of the pre-built scene map, optimizes the local path by combining real-time sensor data, and outputs speed commands to achieve autonomous navigation control.

[0111] This system introduces a dynamic mapping mechanism from ICP confidence factor to observation covariance into the EKF local fusion framework. It integrates indicators such as matching error, convergence characteristics, overlap, and point cloud quality, and performs time smoothing to adaptively adjust the ICP measurement weights. This enables the system to suppress mismatches and trajectory jumps under conditions of low overlap, occlusion, and noise fluctuations, and improve convergence speed and accuracy in scenarios with sufficient geometric constraints. At the same time, the navigation stack performs path planning and local obstacle avoidance based on the fused pose information and outputs speed commands.

[0112] Example: Taking Automated Guided Vehicles (AGVs) in a warehousing and logistics scenario as an example; The AGV is equipped with a two-dimensional lidar, a wheeled odometer, and an inertial measurement unit (IMU), and a two-dimensional grid map of the corresponding operating environment is pre-built.

[0113] During system operation, the vehicle-mounted LiDAR collects environmental point cloud data in real time. The ICP point cloud matching module receives the point cloud data and scene map information. By matching the current LiDAR point cloud with the point cloud or feature model of the corresponding area in the map, the ICP pose estimate of the AGV relative to the global map is obtained.

[0114] While completing point cloud registration, the ICP point cloud matching module performs a quality assessment of the matching process. It calculates the ICP confidence factor by comprehensively considering indicators such as matching error, iterative convergence characteristics, point cloud overlap, point cloud density, and noise level. Based on the ICP confidence factor, it adaptively adjusts the observation covariance matrix corresponding to the ICP pose and outputs the ICP pose and its observation covariance.

[0115] The extended Kalman filter module receives AGV pose information from the wheeled odometer and inertial measurement unit, as well as the ICP pose and dynamically adjusted observation covariance. It constructs a prediction equation based on the system motion model and constructs an observation equation using the ICP pose as the observation input. It achieves the fusion of multi-source sensor information through recursive updates and outputs the fused AGV pose information.

[0116] During AGV operation, when entering narrow passages, corner areas, or areas with significant obstructions, the ICP confidence factor decreases due to reduced point cloud overlap. Consequently, the extended Kalman filter module reduces the weight of ICP observations in the filter update to suppress pose abrupt changes caused by mismatches. Conversely, when the AGV enters geometrically stable and feature-rich areas, the ICP confidence factor increases, and the filter module increases the weight of ICP observations, thereby improving pose estimation accuracy and convergence speed. The navigation stack receives the fused pose information, combines it with a pre-built scene map to generate a global navigation path, and performs local path optimization and dynamic obstacle avoidance based on real-time sensor data. It outputs linear and angular velocity commands to achieve autonomous navigation control of the AGV.

[0117] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the apparatus disclosed in the embodiments, since it corresponds to the method disclosed in the embodiments, the description is relatively simple; relevant parts can be referred to in the method section.

[0118] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A fusion positioning method based on lidar ICP confidence factor, characterized in that, Includes the following steps: S1. Obtain the point cloud data output by the AGV's onboard LiDAR and the map information of the scene. S2. Based on the point cloud data and map information, the point cloud registration is completed using the ICP algorithm to determine the ICP pose, and the ICP confidence factor is calculated based on the matching error, convergence characteristics, feature overlap, and point cloud quality. S3. Adjust the observation covariance matrix based on the ICP confidence factor; S4. Based on the AGV pose, ICP pose, and observation covariance matrix, construct an observation noise model and output the fused pose information. The AGV pose includes: the AGV pose of the wheeled odometer and the AGV attitude of the inertial measurement unit.

2. The fusion positioning method based on lidar ICP confidence factor as described in claim 1, characterized in that, Also includes: S5. Generate a global navigation path based on the fused pose information and the pre-built scene map, optimize the local path by combining real-time sensor data, and output speed commands.

3. The fusion positioning method based on lidar ICP confidence factor as described in claim 1, characterized in that, In step S2, the ICP confidence factor is formed by weightedly fusing the results of the matching error function, convergence function, overlap function, and point cloud quality function to create a comprehensive confidence index, the function of which is expressed as: In the formula, For ICP confidence factor, For the matching error function, For convergence function, For overlap function, For point cloud quality function, , , , These are the weighting coefficients of the corresponding functions; The ICP confidence factor is smoothed over time using an exponential moving average method. In the formula, The confidence factor after smoothing at the current time. This is the smoothed result from the previous time step. For smoothing parameters.

4. The fusion positioning method based on lidar ICP confidence factor as described in claim 3, characterized in that, In step S3, the observed covariance matrix is ​​adjusted based on the ICP confidence factor, and the adjustment method is expressed as follows: In the formula, For the observation covariance matrix based on confidence factors, For its maximum covariance, It represents the minimum covariance.

5. The fusion positioning method based on lidar ICP confidence factor as described in claim 3, characterized in that, The calculation process of the matching error function is as follows: ， In the formula, Let be a rotation matrix. It is a translation matrix. For source point cloud, For the target point cloud, For vector difference, This represents the length difference. Add a robust weight To prevent individual defects from affecting the overall effect, For control parameters; Calculate the weighted root mean square error : Calculate the matching error function : EMSEmax This represents the maximum acceptable error.

6. The fusion positioning method based on lidar ICP confidence factor as described in claim 3, characterized in that, The convergence function is composed of a weighted sum of the iteration number function, the error reduction ratio function, and the error curve stationarity function; Wherein: the iteration number function is used to reflect the relationship between the actual number of iterations and the maximum number of iterations, and its formula is: In the formula, Let be the function of the number of iterations. For the number of iterations, This represents the maximum number of iterations. The error reduction ratio function reflects the ratio of the initial residual to the final residual, and its formula is: In the formula, Let the error decrease proportional function be the function of the error reduction. This is the initial iteration error. This represents the final iteration error; The error curve stationarity function is used to reflect the stability of the error curve during the iteration process, and its formula is: In the formula, Let be the stationarity function of the error curve. for Error metric for each iteration As a measure of the stability of the error sequence, This is a stability penalty coefficient. To avoid division by zero; The convergence confidence score is obtained by weighting and synthesizing the iteration number function, the error reduction ratio function, and the error curve stationarity function, and its formula is expressed as: In the formula, C s For convergence function, , , These are the weights of the three indicators mentioned above.

7. The fusion positioning method based on lidar ICP confidence factor as described in claim 3, characterized in that, The expression for the overlap function is: In the formula, For overlap function, The number of valid points for matching. This represents the number of points where the residual is less than the threshold.

8. The fusion positioning method based on lidar ICP confidence factor as described in claim 3, characterized in that, The expression for the point cloud quality function is: In the formula, For point cloud quality function, For noise weights, For point cloud noise, Weight loss due to point cloud This represents the point cloud loss rate.

9. A fusion positioning system based on lidar ICP confidence factor, characterized in that, include: The ICP point cloud matching module receives point cloud data output from the AGV's LiDAR and map information from the scene map. It completes point cloud registration and determines the ICP pose through the ICP algorithm, and calculates the ICP confidence factor based on matching error, convergence characteristics, feature overlap, and point cloud quality. The observation covariance matrix is ​​then adjusted using the ICP confidence factor. An extended Kalman filter module is used to receive the AGV pose from the wheeled odometer, the AGV attitude from the inertial measurement unit, and the ICP pose and observation covariance matrix output by the ICP point cloud matching module to construct an observation noise model; by recursively updating the prediction equation and the observation equation, dynamic fusion of multi-source data is achieved, and the fused pose information is output.

10. A fusion positioning system based on lidar ICP confidence factor as described in claim 9, characterized in that, Also includes: The navigation stack receives the fused pose information output by the extended Kalman filter module, generates a global navigation path based on the map information of the pre-built scene map, optimizes the local path by combining real-time sensor data, and outputs speed commands.

Images