Fusion positioning method and system, storage medium and program product

Abstract

The invention discloses a fusion positioning method and system, a storage medium and a program product, and the method comprises the steps: collecting original point cloud data of target equipment through a laser radar, carrying out the preprocessing of the collected original point cloud data, and obtaining effective point cloud data; analyzing the preprocessed effective point cloud data fused with inertial measurement data collected by an inertial measurement unit by using a laser radar mileage calculation method to obtain a first estimated pose of the target equipment; performing preset part feature extraction on the effective point cloud data, and matching the extracted preset part point cloud features with the known preset part distribution map of the current site by using an adaptive Monte Carlo positioning method to obtain a second estimated pose; and fusing the first estimated pose and the second estimated pose by using an extended Kalman filtering algorithm, and performing real-time output to obtain a final pose of the target equipment.

Description

Technical Field

[0001] The present application relates to the field of robot intelligent navigation technology, and in particular to a fusion positioning method, system, storage medium and program product. Background Art

[0002] With the rapid development of robotics, the demand for robot positioning and navigation is increasing, especially in complex indoor and outdoor environments, where accurate positioning technology is particularly important. Existing positioning methods combine multiple sensors such as vision, inertial measurement unit (IMU), light detection and ranging (LiDAR), and global positioning system (GPS) to achieve high-precision pose estimation through filtering algorithms and deep learning algorithms. Although significant progress has been made in the development of existing technologies, it also faces many challenges. For example, in dynamic environments, moving objects such as pedestrians and vehicles will interfere with pose estimation; synchronization and calibration issues of multi-sensor data will also affect navigation accuracy. In addition, the limitations of computing resources and real-time requirements require algorithms to make a trade-off between accuracy and efficiency. Summary of the Invention

[0003] To address the above technical issues, this application discloses a fusion positioning method, system, storage medium, and program product. By extracting the point cloud features of pillars from LiDAR scanning data, a high-precision and robust fusion positioning system is constructed using a small number of sensors combined with an adaptive Monte Carlo positioning method and LiDAR odometry. Specifically, the technical solutions of this application are as follows:

[0004] In a first aspect, the present application discloses a fusion positioning method, comprising the following steps:

[0005] Using a laser radar to collect raw point cloud data of a target device, and preprocessing the collected raw point cloud data to obtain valid point cloud data;

[0006] Analyzing the pre-processed effective point cloud data fused with inertial measurement data collected by an inertial measurement unit using a laser radar mileage calculation method to obtain a first estimated position and posture of the target device;

[0007] Extracting features of preset components from the valid point cloud data, and matching the extracted point cloud features of the preset components with a known distribution map of the preset components in the current site using an adaptive Monte Carlo positioning method to obtain a second estimated pose;

[0008] The first estimated pose and the second estimated pose are fused using an extended Kalman filter algorithm, and a final pose of the target device is obtained by outputting the result in real time.

[0009] In some embodiments, the preprocessing of the collected raw point cloud data to obtain valid point cloud data specifically includes:

[0010] A height threshold is set, valid point clouds are screened from the original point cloud data, and valid information of the valid point clouds is generated, where the valid information includes the position of the point cloud and the distance between the point cloud and adjacent point clouds.

[0011] In some embodiments, the method of using a laser radar odometry method to analyze the pre-processed effective point cloud data and the inertial measurement data collected by an inertial measurement unit to obtain a first estimated position and posture of the target device specifically includes:

[0012] Pre-integrating the inertial measurement data to calculate a first position change of the target device;

[0013] Using a closest point iterative algorithm to register the valid point clouds between two adjacent frames to obtain a second posture change of the target device;

[0014] A nonlinear optimization method is used to combine the first posture change and the second posture change to obtain the first estimated posture.

[0015] In some embodiments, the registering of the valid point clouds between two adjacent frames using a closest point iterative algorithm to obtain a second posture change of the target device specifically includes:

[0016] Input adjacent first frame point cloud data and second frame point cloud data; for each source point cloud in the first frame point cloud data, find the point cloud closest to it in the second frame point cloud data as the target point cloud;

[0017] Calculate the rotation matrix and translation vector between each source point cloud and its corresponding target point cloud;

[0018] Applying the rotation matrix and the translation vector to the source point cloud to obtain a new transformed point cloud;

[0019] The error between the transformed point cloud and the target point cloud is determined, and the iteration is stopped if the error is smaller than the target error.

[0020] In some embodiments, extracting preset component features from the valid point cloud data specifically includes:

[0021] Projecting the valid point cloud onto a two-dimensional plane parallel to the ground of the photovoltaic power station, and performing horizontal clustering: grouping the valid point clouds whose first distance in the horizontal direction is less than a first threshold into one category;

[0022] Vertically merging the valid point clouds classified into one category: grouping the valid point clouds whose second distance in the vertical direction is less than a second threshold into one cluster;

[0023] Filtering the preset component cluster from the clustered point cloud clusters according to the geometric features of the preset component;

[0024] For each of the preset component clusters, its geometric data is calculated; and the extracted preset component clusters are visualized and saved.

[0025] In some embodiments, the method of using the adaptive Monte Carlo positioning method to match the extracted point cloud features of the preset components with the known distribution map of the preset components in the current site to obtain a second estimated pose specifically includes:

[0026] generating a group of randomly distributed particles according to an initial position estimate of the target device, and initializing the particles;

[0027] Matching the point cloud features of the preset components with the known distribution map of the preset components in the current site based on the Monte Carlo positioning method; calculating the likelihood of the particle at the current position to obtain a matching weight for each particle;

[0028] According to the matching weight of each particle, the particles with high matching weight are preferentially selected, and re-sampled to generate a new particle set;

[0029] According to the poses and matching weights of all the particles, the pose of the particle with the largest matching weight is taken as the second estimated pose of the target device.

[0030] In some embodiments, the method of using an extended Kalman filter algorithm to fuse the first estimated pose and the second estimated pose and outputting the final pose of the target device in real time specifically includes:

[0031] Inputting the first estimated pose and the second estimated pose as observation values ​​into an extended Kalman filter algorithm;

[0032] Establishing an observation model and a process model; using the process model to predict the state estimation and covariance matrix of the target device at the current moment;

[0033] For a plurality of the observation values, sequentially calculating their Kalman gains, updating the state estimation and updating the covariance matrix;

[0034] By calculating a weighted average of the covariance matrices of the plurality of observation values, a fused covariance matrix is ​​obtained, and the first estimated pose and the second estimated pose are fused, thereby obtaining a final pose of the target device.

[0035] In a second aspect, the present application also includes a fusion positioning system, comprising a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the method described in any one of the above embodiments.

[0036] In a third aspect, the present application further discloses a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the method described in any one of the above embodiments.

[0037] In a fourth aspect, the present application further discloses a computer program product, comprising a computer program, which implements the steps of the method described in any one of the above embodiments when executed by a processor.

[0038] Compared with the prior art, this application has at least one of the following beneficial effects:

[0039] 1. This application combines the Adaptive Monte Carlo Localization method (AMCL) and the Fast LiDAR Odometry method (FAST-LIO), which can fully leverage the complementary advantages of the two and significantly improve the robustness and accuracy of pose estimation, making it particularly suitable for positioning in known environments. AMCL relies on prior maps and excels at global positioning and dynamic environment adaptation, but it relies on the initial pose and is limited to 2D scenes. FAST-LIO, based on LiDAR-IMU fusion, provides high-precision 3D odometry. It does not rely on maps and is insensitive to the initial pose, but has problems with cumulative errors and interference from dynamic objects. By fusing these two algorithms, FAST-LIO can provide AMCL with a reliable initial pose and achieve high-frequency local updates, while AMCL can correct FAST-LIO's long-term drift and enhance global consistency, ultimately achieving 2D / 3D collaborative positioning, improved robustness in dynamic environments, and cumulative error suppression. It is suitable for indoor robots, drones, and precise navigation in GPS-free environments.

[0040] 2. The technical solution of this application is applicable to the accurate posture estimation of photovoltaic power stations and photovoltaic cleaning robots. Photovoltaic power stations usually have a large number of photovoltaic brackets, and each bracket has regularly arranged preset components, such as the feature extraction of columns, which provides a wealth of reference points for positioning. By accurately extracting the geometric features of the columns, such as position, height, diameter, etc., combined with the environmental map, a stable reference object is provided for the positioning system. This positioning method based on column features can effectively avoid the problem of positioning loss in complex environments, and even in areas with more occlusion or interference, it can restore and maintain positioning accuracy by matching column features. By utilizing the existing column structure for feature extraction, there is no need for additional expensive markers or infrastructure, which reduces the construction and maintenance costs of the system. At the same time, the combination of lidar and IMU can reduce dependence on other sensors, such as GPS, while ensuring positioning accuracy, further improving the independence and autonomy of the system.

[0041] 3. This application integrates data from lidar and inertial measurement units. Lidar provides high-precision environmental geometry information and can generate detailed point cloud maps to help robots or vehicles perceive surrounding obstacles and terrain. The inertial measurement unit measures acceleration and angular velocity in real time, providing high-frequency motion information and maintaining position continuity even during lidar scan intervals. By fusing these two types of data using nonlinear optimization methods, the error impact of a single sensor can be effectively reduced. This not only improves positioning accuracy, but also enhances the adaptability and reliability of the system, providing a more reliable positioning solution for applications such as autonomous driving and robot navigation.

[0042] 4. This application is highly robust. LiDAR works effectively under various lighting and weather conditions, provides high-precision three-dimensional point cloud data, directly reflects the geometric structure of the environment, is not affected by lighting changes and weather interference, and is suitable for the outdoor environment where photovoltaic power stations are located. This ensures the stability and reliability of the positioning system. At the same time, the addition of IMU further enhances the performance of the positioning system. IMU can measure acceleration and angular velocity in real time, and provide information on posture changes in a short period of time. During the LiDAR data update interval, IMU can be used to estimate the robot's motion trajectory to ensure the continuity of positioning. This multi-sensor fusion solution that combines LiDAR and IMU not only improves positioning accuracy, but also enhances the system's anti-interference ability and adaptability. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] The preferred implementation scheme will be described below in a clear and understandable manner with reference to the accompanying drawings to further illustrate the above-mentioned characteristics, technical features, advantages and implementation methods of the present application.

[0044] Figure 1 This is a flowchart of one embodiment of a fusion positioning method of the present application;

[0045] Figure 2 This is a flowchart of the sub-steps of step S200 in another embodiment of a fusion positioning method of the present application;

[0046] Figure 3 This is a flowchart of the sub-steps of step S310 in another embodiment of a fusion positioning method of the present application;

[0047] Figure 4 This is a flowchart of the sub-steps of step S320 in another embodiment of a fusion positioning method of the present application. DETAILED DESCRIPTION

[0048] In the following description, specific details such as specific system structures and technologies are provided for illustration rather than limitation to facilitate a thorough understanding of the embodiments of the present application. However, it should be clear to those skilled in the art that the present application may be implemented in other embodiments without these specific details. In other cases, detailed descriptions of well-known systems, devices, circuits, and methods are omitted to avoid obstructing the description of the present application with unnecessary details.

[0049] It will be understood that when used in this specification and the appended claims, the term "comprising" indicates the presence of the described features, integers, steps, operations, elements and / or components, but does not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or collections.

[0050] To simplify the drawings, only portions relevant to the invention are schematically depicted in each figure; they do not represent the actual structure of the product. Furthermore, to simplify the drawings and facilitate understanding, in some figures, only one component with the same structure or function is schematically depicted or labeled. In this document, "one" not only means "only one" but also "more than one."

[0051] It should be further understood that the term "and / or" used in this specification and the appended claims refers to and includes any and all possible combinations of one or more of the associated listed items.

[0052] In addition, in the description of the present application, the terms "first", "second", etc. are only used to distinguish the description and cannot be understood as indicating or implying relative importance.

[0053] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the specific implementation methods of the present application will be described below with reference to the accompanying drawings. Obviously, the drawings described below are only some embodiments of the present application. For those skilled in the art, other drawings and other implementation methods can be obtained based on these drawings without inventive work.

[0054] In the field of intelligent robotic navigation, robot posture determination technology is a key enabler for autonomous movement and precise positioning of intelligent agents, such as robots, self-driving vehicles, and drones. By determining the position and posture of intelligent agents in real time, posture determination technology provides the fundamental spatial perception capabilities for navigation systems, enabling them to plan paths, avoid obstacles, and complete tasks in complex environments.

[0055] The key to achieving pose determination is environmental perception and recognition. Commonly used technical solutions in existing technologies primarily involve capturing images of the surrounding environment with a camera, then performing image recognition to determine position based on image similarity. Alternatively, multiple sensors, such as visual measurement units, IMUs, LiDAR, and GPS, are combined to achieve high-precision pose estimation through filtering algorithms and deep learning. For example, deep learning-based pose estimation methods, such as convolutional neural networks (CNNs) and graph neural networks (GNNs), can improve the real-time performance and accuracy of pose determination when processing environments and dynamic scenes.

[0056] However, despite significant progress in existing technologies, pose estimation technology still faces several shortcomings and challenges that need to be addressed. First, while sensor fusion improves pose estimation accuracy, the synchronization and calibration of multi-sensor data remain complex. Sensor drift and error accumulation, especially over long periods of time, can lead to decreased accuracy. Second, visual SLAM technology is unstable in dynamic environments or those with drastic lighting changes. It is susceptible to occlusion, texture loss, or motion blur, resulting in localization failures or inaccurate mapping. Furthermore, while deep learning-based methods perform well in certain scenarios, they rely on large amounts of annotated data, resulting in high training costs and potentially insufficient generalization in practical applications, especially in novel, unseen environments. Computational resource limitations are also a significant issue. While edge computing and dedicated hardware have improved computational efficiency, balancing real-time performance with power consumption remains challenging on resource-constrained mobile robots or small devices. Finally, existing technologies lack robustness in highly dynamic environments, such as those with high-speed motion or drastically changing scenes, making them difficult to address rapidly changing pose requirements.

[0057] In order to improve the accuracy of posture measurement, this application extracts the point cloud features of the pillars in the lidar scanning data, and uses a small number of sensors combined with the adaptive Monte Carlo positioning method and the lidar mileage calculation method to construct a high-precision and robust fusion positioning system.

[0058] Reference Manual Figure 1 As shown, an embodiment of a fusion positioning method of the present application specifically includes the following steps:

[0059] S100: Use a laser radar to collect original point cloud data of the target device, and pre-process the collected original point cloud data to obtain valid point cloud data.

[0060] Specifically, the raw point cloud data is filtered to remove noise and unnecessary points and extract important features from the environment. This process includes downsampling, such as voxel grid filtering, ground separation, and outlier removal. The filtered point cloud data provides clearer data for subsequent pose estimation.

[0061] S200: Using a laser radar mileage calculation method, the pre-processed valid point cloud data is fused with the inertial measurement data collected by the inertial measurement unit to analyze the inertial measurement data to obtain a first estimated position and posture of the target device.

[0062] Specifically, the filtered point cloud data is passed to Fast LiDAR Odometry (FAST-LIO), an efficient LiDAR odometry algorithm used to estimate the sweeper's pose (position and attitude) in real time. By analyzing the point cloud data, FAST-LIO infers the sweeper's relative pose changes based on point cloud matching and motion.

[0063] S300: extracting preset component features from the valid point cloud data, and matching the extracted preset component point cloud features with a known preset component distribution map of the current site using an adaptive Monte Carlo positioning method to obtain a second estimated pose.

[0064] Specifically, point cloud features of pre-defined components of the PV plant, such as columns, are extracted from the point cloud data. The Adaptive Monte Carlo Localization (AMCL) method uses these features for matching. Columns that conform to a specific geometric model are identified from the point cloud. The extracted column features are then matched against a known column map of the PV plant, further optimizing and verifying the sweeper's pose estimation.

[0065] S400: Using an extended Kalman filter algorithm, the first estimated pose and the second estimated pose are fused, and a final pose of the target device is obtained by outputting the result in real time.

[0066] Specifically, the pose information obtained from FAST-LIO and AMCL is fused using the Extended Kalman Filter (EKF) algorithm. The EKF performs nonlinear optimization on the poses of AMCL and FAST-LIO, thereby improving the accuracy and stability of pose estimation. The fused pose information from the EKF is output as the final pose of the sweeper, which is then used by the sweeper control system for path planning and motion control.

[0067] In the field of robot navigation and positioning, AMCL and FAST-LIO are two widely used algorithms with different applicable scenarios. AMCL, a positioning method based on particle filtering, is mainly used for low-speed robot navigation in known environments, such as warehousing logistics and service robots. It is mature, stable and easy to implement, but its adaptability and computational efficiency in dynamic environments are limited. FAST-LIO is a tightly coupled mileage calculation method based on lidar and IMU. It is suitable for high-precision positioning and mapping in unknown environments. It performs particularly well in autonomous driving, drones and complex outdoor environments. It has high precision, high real-time performance and strong adaptability, but it has high requirements for sensor quality and computing resources.

[0068] The AMCL algorithm based on LiDAR and the FAST-LIO algorithm based on vision have their own advantages and disadvantages. The AMCL algorithm uses particle filtering for positioning and relies on prior maps. It is suitable for static environments, but will be affected in dynamic environments. FAST-LIO combines LiDAR and inertial sensors to provide high-frequency and high-precision positioning, but due to the errors of inertial sensors, it is sensitive to cumulative errors during long-term operation. This application combines the AMCL algorithm and the FAST-LIO algorithm to give full play to the complementary advantages of the two, significantly improve the robustness and accuracy of pose estimation, and is particularly suitable for positioning in known environments. AMCL relies on prior maps and is good at global positioning and dynamic environment adaptation, but it relies on initial poses and is limited to 2D scenes; while FAST-LIO is based on LiDAR-IMU fusion, provides high-precision 3D odometer, does not rely on maps and is not sensitive to initial poses, but has problems with cumulative errors and interference from dynamic objects. By fusing these two algorithms, FAST-LIO can provide AMCL with a reliable initial pose and achieve high-frequency local updates, while AMCL can correct the long-term drift of FAST-LIO and enhance global consistency, ultimately achieving 2D / 3D collaborative positioning, improved robustness in dynamic environments, and cumulative error suppression. It is suitable for precise navigation of indoor robots, drones, and in GPS-free environments.

[0069] Based on the above embodiment, this application discloses another embodiment of a fusion positioning method. In step S100, a laser radar is used to collect raw point cloud data of a target device, and the collected raw point cloud data is preprocessed to obtain valid point cloud data. Specifically, the method includes the following sub-steps:

[0070] S110 , setting a height threshold to filter valid point clouds from the original point cloud data.

[0071] S120 , generating valid information of the valid point cloud, where the valid information includes the position of the point cloud and the distance between the point cloud and adjacent point clouds.

[0072] Specifically, the point cloud is preprocessed, removing invalid points such as ground point clouds using a height threshold. The point cloud is then projected onto a two-dimensional plane, retaining the valid information of each point cloud, including its location and distance. Preprocessing the point cloud can significantly improve the performance and efficiency of subsequent algorithms. Denoising and motion distortion correction improve data quality and enhance algorithm robustness. Downsampling and feature extraction reduce computational complexity and accelerate real-time processing. Normal estimation and intensity normalization optimize feature representation and improve matching accuracy. Multi-sensor fusion and occlusion compensation adapt to different hardware and environmental requirements. Ultimately, high-quality input data is provided for subsequent tasks, thereby improving the accuracy, speed, and stability of positioning tasks.

[0073] Based on the above embodiment, this application discloses another embodiment of a fusion positioning method. Figure 2 In step S200, the pre-processed valid point cloud data is fused with the inertial measurement data collected by the inertial measurement unit using a laser radar mileage calculation method to analyze the inertial measurement data to obtain a first estimated position and posture of the target device. This specifically includes the following sub-steps:

[0074] Since the map scene used in this application is a two-dimensional map, only the position and direction on the two-dimensional plane are extracted. FAST-LIO fuses IMU and radar data to estimate the robot's trajectory and position as follows:

[0075] S210 , pre-integrate the inertial measurement data to calculate the first position change of the target device between adjacent moments.

[0076] Specifically, FAST-LIO uses IMU data for pre-integration, calculating the pose change between adjacent moments using the IMU's acceleration and angular velocity. Because IMU data is much more frequent than LiDAR data, frequent updates can be performed within the time interval between LiDAR data.

[0077] The role of pre-integration: Using IMU data, motion is inferred between two frames of radar data. This means that the update of the point cloud position and attitude between the two frames is inferred, reducing the computational burden of relying solely on radar point clouds.

[0078] S220: Use a closest point iterative algorithm to register the valid point clouds between two adjacent frames to obtain a second posture change of the target device.

[0079] Specifically, FAST-LIO uses point cloud registration to calculate the pose change between LiDAR data. It uses the Iterative Closest Point (ICP) algorithm to match and align two frames of LiDAR point cloud data, thereby calculating the relative pose change between the two frames.

[0080] In another implementation of this embodiment, step S220 includes:

[0081] S221 , inputting adjacent first frame point cloud data and second frame point cloud data; for each source point cloud in the first frame point cloud data, searching for the point cloud closest to it in the second frame point cloud data as a target point cloud.

[0082] S222, calculating the rotation matrix and translation vector between each source point cloud and its corresponding target point cloud.

[0083] S223, applying the rotation matrix and the translation vector to the source point cloud to obtain a new transformed point cloud.

[0084] S224, determining the error between the transformed point cloud and the target point cloud, and stopping the iteration if the error is less than the target error.

[0085] Specifically, the simplified process of the ICP algorithm is as follows:

[0086] 1. Initialization: Input source point cloud and target point cloud.

[0087] 2. Find the nearest point: For each point in the source point cloud, find the nearest point in the target point cloud.

[0088] 3. Calculate the transformation: Based on the matched corresponding point pairs, use the least squares method or singular value decomposition (SVD) method to calculate the optimal rotation matrix and translation vector to minimize the distance between the source point cloud and the target point cloud.

[0089] 4. Apply transformation: Apply the calculated transformation to the source point cloud. Apply the transformation matrix and translation vector of the current iteration to the source point cloud to obtain a new transformed point cloud.

[0090] 5. Convergence Check: Checks whether the error is less than a threshold or the maximum number of iterations has been reached. If so, the iteration stops; otherwise, it continues. When convergence is achieved, the final transformation matrix and translation vector are output, representing the change in the target device's pose. The final transformation matrix can be applied to the source point cloud to obtain a registered point cloud aligned with the target point cloud.

[0091] S230, using a nonlinear optimization method to combine the first pose change and the second pose change to obtain a first estimated pose, so as to minimize the error.

[0092] Specifically, in FAST-LIO, the fusion of IMU and LiDAR data is primarily performed through optimization. FAST-LIO uses a nonlinear optimization method called an extended Kalman filter to fuse the IMU pre-integration results and the pose updates from point cloud registration to minimize the error.

[0093] In this embodiment, FAST-LIO achieves high-precision and high-frequency positioning and mapping by tightly coupling LiDAR and IMU data. This significantly improves computational efficiency and provides reliable pose estimation for robot navigation in highly dynamic scenarios.

[0094] Based on the above embodiment, this application discloses another embodiment of a fusion positioning method. In step S300, features of preset components are extracted from valid point cloud data. The extracted point cloud features of the preset components are matched with the known distribution map of preset components in the current site using an adaptive Monte Carlo positioning method to obtain a second estimated pose. This method specifically includes the following sub-steps:

[0095] S310 , selecting a preset component cluster from the clustered point cloud clusters according to the geometric features of the preset component.

[0096] Specifically, cluster screening is performed based on the Euclidean clustering method: according to the spatial distribution of the point cloud after clustering, including the size and shape of the cluster; for example, based on the geometric features that pillars usually have higher heights and smaller diameters, it is judged whether these clusters may be pillars.

[0097] In one implementation of this embodiment, refer to the attached specification. Figure 3 , the step S310 further includes the following sub-steps:

[0098] S311 , projecting the valid point cloud onto a two-dimensional plane parallel to the ground of the photovoltaic power station, and performing horizontal clustering: valid point clouds with a first distance in the horizontal direction less than a first threshold are classified into one category.

[0099] S312 , vertically merging the valid point clouds classified into one category: grouping the valid point clouds whose second distance in the vertical direction is less than the second threshold into one cluster.

[0100] S313 , selecting a preset component cluster from the clustered point cloud clusters according to the geometric features of the preset component.

[0101] Specifically, horizontal clustering is first performed, grouping point clouds with distances and depths less than a certain threshold into one category. Vertical merging is then performed. If vertically adjacent points belong to the same category and their coordinate and depth differences meet the requirements, they are merged into a new cluster. Finally, each cluster is screened, and the spatial distribution of the clusters, including their size and shape, is used to determine whether they are likely to be pillars. The clustering method used is Euclidean clustering, and the simple process is as follows:

[0102] 1. Neighborhood judgment: For each point, use Euclidean distance to calculate the distance between the point and other points. If the distance is less than the threshold, the point belongs to the neighborhood of the point.

[0103] 2. Extension judgment: If the neighborhood size of a point is sufficient, that is, greater than or equal to the threshold parameter (min_pts), a cluster is formed and other points are gradually added to the cluster by expanding the neighborhood.

[0104] 3. Noise point judgment: If the neighborhood of a point is smaller than the threshold parameter (min_pts), the point will not be added to any cluster and is usually regarded as a noise point.

[0105] In another implementation of this embodiment, step S310 further includes the following sub-steps:

[0106] S314 , for each preset component cluster, calculating its geometric data; visualizing and saving the extracted preset component cluster.

[0107] Specifically, for each preset component cluster, such as a column cluster, its geometric data is calculated, and the extracted column cluster is visualized to facilitate checking the accuracy of the extraction results. The extracted column features are saved as a file for subsequent processing. The geometric data of each cluster, for example: Height: Calculate the distance between the highest point and the lowest point of each cluster. Diameter: Estimate the diameter by fitting a cylindrical model or calculating the width of the point cloud. Center position: Calculate the center of mass of each cluster. Direction: Use Principal Component Analysis (PCA) to calculate the main direction vector of each cluster.

[0108] Based on the above embodiments, the present application discloses another embodiment of a fusion positioning method, step S300, which specifically also includes the following sub-steps: S320: Use the adaptive Monte Carlo positioning method to match the extracted preset component point cloud features with the preset component distribution map known in the current site to obtain a second estimated pose.

[0109] In one implementation of this embodiment, refer to the attached specification. Figure 4 Step S320 specifically includes:

[0110] S321 , generating a group of randomly distributed particles based on the initial position estimation of the target device, and initializing the particles.

[0111] Input: Photovoltaic power station pole diagram, radar point cloud data after pole extraction and processing, number of particles and other parameters. In AMCL, a set of particles must be initialized first. Each particle represents a possible position and posture. The initial position of the particle can be set according to the last position of the on-board cleaning robot, known environmental information or random distribution. Each particle has a weight, which represents the possibility of the state corresponding to the particle. Since this application uses a two-dimensional map, the particles contain position (x, y) and direction (θ). These particles can be randomly generated at the initial position, or generated through the current motion model of the robot.

[0112] AMCL adaptive Monte Carlo localization generates a set of particles to represent possible hypotheses about the robot's position and continuously updates the weights of these particles using sensor data, gradually approximating the robot's true position. AMCL's core advantage lies in its adaptive mechanism, which dynamically adjusts the number of particles to improve computational efficiency while maintaining strong robustness to sensor noise and environmental variations. It is particularly stable and easy to implement for global localization tasks in known environments.

[0113] S322, matching the preset component point cloud features with the known preset component distribution map of the current site based on the Monte Carlo positioning method; calculating the likelihood of the particle at the current position to obtain the matching weight of each particle.

[0114] Specifically, the sensor input is the radar point cloud data after pillar extraction. The extracted point cloud pillar features are converted from the robot coordinate system to the map coordinate system.

[0115] Calculate the weight of each particle: For each particle, calculate the degree of match between its corresponding pillar feature and the known pillar pattern in the map. A Gaussian model is typically used to calculate the probability of a match. For example, the distance from the pillar feature point to the nearest obstacle in the map is calculated and a weight is calculated based on this distance. The smaller the distance, the higher the weight, indicating that the particle's corresponding pose is more likely to be the robot's actual location.

[0116] Each particle compares the current radar point cloud data with the known PV power station pole map to calculate the likelihood of the particle at that location. Usually, the sensor model is used to calculate the weight of each particle, that is, the degree of match between the sensor observation value that the particle can generate at its current location and the actual observation value. Bayesian estimation is usually used to update the particle weight: ωt =P(z t |x t ).

[0117] Among them, ω t is the particle weight, z t is the current sensor observation data, x t is the position and posture of the particle, P(z t |x t ) is the match between the particle position and the sensor observation.

[0118] Preferably, this application uses the first estimated pose as the initial pose input for the Monte Carlo localization method. Specifically, the sweeper pose calculated using FAST-LIO is used as the initial pose input for AMCL. This results in a particle distribution that is closer to the actual pose, significantly reducing the time it takes to converge to the correct position and improving the real-time performance of AMCL pose correction.

[0119] S323: Based on the weight of each particle, particles with high matching weight are preferentially selected and resampled to generate a new particle set.

[0120] Specifically, resampling by weight: resampling generates a new set of particles based on the weight of each particle. Particles with higher weights are more likely to be selected, thus retaining more likely pose estimates.

[0121] Normalize weights: Normalize the weights of all particles to ensure their sum is 1. The basic idea behind resampling is to randomly select particles from the current particle set based on their weights, repeatedly selecting particles with high weights and eliminating particles with low weights. This allows particles to be concentrated in more likely locations, thereby improving positioning accuracy.

[0122] S324 , based on the positions and matching weights of all particles, the position of the particle with the largest matching weight is taken as the latest estimated position of the target device.

[0123] Specifically, the optimal estimate of the sweeper's current position can be calculated based on the particle state distribution. AMCL selects the state of the particle with the largest weight as the estimate of the robot's current position, while still retaining the randomness of particle sampling.

[0124] In another implementation of this embodiment, the above steps are repeated each time new sensor data arrives until the algorithm converges, meaning the robot's position is stable and deterministic. Specifically, when the weight distribution is relatively concentrated and the variance of the particles is small, the algorithm has converged and the robot's current position can be output.

[0125] In this embodiment, the known column map of the current site comes from the CAD drawing of the photovoltaic power station, which contains the position of the photovoltaic module columns in the photovoltaic power station. The technical solution in this embodiment is applicable to the accurate posture estimation of photovoltaic power stations and photovoltaic cleaning robots. Photovoltaic power stations usually have a large number of photovoltaic brackets, and the feature extraction of the columns in each bracket provides a wealth of reference points for positioning. By accurately extracting the geometric features of the columns, such as position, height, diameter, etc., combined with the environmental map, a stable reference object is provided for the positioning system. AMCL can use the column point cloud extracted by the laser sensor to match the known point cloud to estimate the position of the robot in a known map, and can dynamically adjust the position distribution of particles according to the sensor data for precise positioning.

[0126] Based on the above embodiment, this application discloses another embodiment of a fusion positioning method. In step S400, the first estimated pose and the second estimated pose are fused using an extended Kalman filter algorithm, and the final pose of the target device is output in real time. The method specifically includes the following sub-steps:

[0127] S410: Input the first estimated pose and the second estimated pose as observation values ​​into an extended Kalman filter algorithm.

[0128] S420, establishing an observation model and a process model; using the process model to predict the state estimation and covariance matrix of the target device at the current moment.

[0129] S430, sequentially calculating the Kalman gain for the multiple observation values, updating the state estimation and updating the covariance matrix.

[0130] S440 , calculating a weighted average of the covariance matrices of the multiple observations to obtain a fused covariance matrix, thereby fusing the first estimated pose with the second estimated pose, and obtaining a final pose of the target device.

[0131] Specifically, the Extended Kalman Filter (EKF) algorithm linearizes the nonlinear system at each time step, enabling it to handle nonlinear motion and observation models. In the prediction step, the EKF predicts the state and covariance of the next moment based on the motion model. In the update step, it corrects the prediction based on the observed data and ultimately outputs a fused optimal state estimate.

[0132] In one implementation of this embodiment, an EKF algorithm is used to fuse the first estimated pose with the second estimated pose to output a final pose of the target device. The main steps are as follows:

[0133] System modeling: Define the state vector. Establish the state transition equation. Establish the observation equation.

[0134] The state vector usually includes the target device's pose information, namely the first estimated pose and the second estimated pose, which includes position, velocity, attitude, etc.

[0135] Assume that the first estimated pose output of AMCL is: y amcl ,θ amcl and the covariance P amcl , where x amcl 、y amcl The x and y coordinates of the pose estimate output by AMCL, θ amcl is the estimated yaw angle.

[0136] The second estimated pose of FAST-LIO is: y fastlio ,θ fastlio and covariance P fastlio , where x fastlio 、y fastlio The x and y coordinates of the pose estimate output by FAST-LIO, θ fastlio is the estimated yaw angle.

[0137] The pose is input into EKF as the observation value. The final estimated pose is obtained by calculating the weighted average through EKF:

[0138] in: is the first estimated pose; is the second estimated pose; is the covariance matrix corresponding to the first estimated pose; is the covariance matrix corresponding to the second estimated pose; P fused is the covariance matrix after fusion.

[0139] Update the covariance matrix:

[0140] The updated covariance matrix can be used to evaluate the contribution of different sensors to the final estimation result. If the covariance of AMCL is small and the covariance of FAST-LIO is large, the fused covariance matrix P fused It will rely more on the results of AMCL, and vice versa, it will rely more on the results of FAST-LIO. Finally, the positioning results of AMCL and FAST-LIO are fused through EKF to achieve fusion positioning.

[0141] Based on the same concept, this application also discloses a fusion positioning system. The system is used to implement the steps of any of the above-mentioned method embodiments. Specifically, this application discloses an embodiment of a fusion positioning system, which includes: a target device body, a laser radar, an inertial measurement unit, a processor, a memory, and a computer program stored in the memory. The processor executes the computer program to implement the steps of any of the above-mentioned method embodiments.

[0142] LiDAR is used to collect raw point cloud data of the target device.

[0143] An inertial measurement unit (IMU) is used to measure the linear acceleration and angular velocity changes of the target device using an accelerometer and gyroscope.

[0144] The processor includes a preprocessing module for preprocessing the collected original point cloud data to obtain valid point cloud data.

[0145] The first algorithm module is used to use the laser radar mileage calculation method to analyze the pre-processed valid point cloud data fused with the inertial measurement data collected by the inertial measurement unit to obtain a first estimated position and posture of the target device.

[0146] The second algorithm module is used to extract the preset component features from the valid point cloud data, and use the adaptive Monte Carlo positioning method to match the extracted preset component point cloud features with the preset component distribution map known in the current site to obtain a second estimated pose.

[0147] The fusion algorithm module is used to fuse the first estimated pose with the second estimated pose using the extended Kalman filter algorithm, and output the final pose of the target device in real time.

[0148] Based on the above embodiments, the present application discloses another embodiment of a fusion positioning system, a preprocessing module, which is specifically used to: set a height threshold, filter valid point clouds from the original point cloud data, and generate valid information of the valid point clouds, the valid information including the position of the point cloud and the distance between it and adjacent point clouds.

[0149] The first algorithm module specifically includes:

[0150] The pre-integration unit is used to pre-integrate the inertial measurement data and calculate the first position change of the target device.

[0151] The point cloud registration unit is used to register the valid point clouds between two adjacent frames using the nearest point iterative algorithm to obtain the second pose change of the target device.

[0152] The filtering optimization unit is used to use a nonlinear optimization method to combine the first pose change and the second pose change to obtain a first estimated pose.

[0153] The point cloud registration unit is specifically used to: input adjacent first frame point cloud data and second frame point cloud data; for each source point cloud in the first frame point cloud data, find the point cloud closest to it in the second frame point cloud as the target point cloud; calculate the rotation matrix and translation vector between each source point cloud and its corresponding target point cloud; apply the rotation matrix and translation vector to the source point cloud to obtain a new transformed point cloud; determine the error between the transformed point cloud and the target point cloud, and stop iteration if the error is less than the target error.

[0154] The second algorithm module specifically includes: a feature extraction unit, which is used to extract preset component features from valid point cloud data; specifically used to: project the valid point cloud onto a two-dimensional plane parallel to the ground of the photovoltaic power station, and perform horizontal clustering: classify the valid point clouds with a first distance in the horizontal direction less than a first threshold into one category; vertically merge the valid point clouds classified into one category: classify the valid point clouds with a second distance in the vertical direction less than a second threshold into one cluster; filter out preset component clusters from the clustered point cloud clusters based on the geometric features of the preset components; calculate the geometric data of each preset component cluster; and visualize and save the extracted preset component clusters.

[0155] The algorithm matching unit is specifically used to: generate a set of randomly distributed particles based on the initial position estimate of the target device and initialize the particles; match the preset component point cloud features with the preset component distribution map known in the current site based on the Monte Carlo positioning method; calculate the likelihood of the particles at the current position to obtain the matching weight of each particle; based on the matching weight of each particle, give priority to particles with high matching weights, and resample to generate a new particle set; based on the poses and matching weights of all particles, take the pose of the particle with the largest matching weight as the second estimated pose of the target device.

[0156] The fusion algorithm module is specifically used to input the first estimated pose and the second estimated pose as observation values ​​into the extended Kalman filter algorithm; establish an observation model and a process model; use the process model to predict the state estimate and covariance matrix of the target device at the current moment; calculate the Kalman gain for multiple observation values ​​in sequence, update the state estimate and update the covariance matrix; obtain the fused covariance matrix by calculating the weighted average of the covariance matrices of multiple observation values, realize the fusion of the first estimated pose and the second estimated pose, and thus obtain the final pose of the target device.

[0157] Based on the same concept, the present application also includes a fusion positioning system, including a memory, a processor and a computer program stored in the memory, and the processor executes the computer program to implement the steps of the method in any of the above embodiments.

[0158] In a third aspect, the present application further discloses a computer-readable storage medium having a computer program stored thereon, which implements the steps of the method in any of the above-mentioned embodiments when the computer program is executed by a processor.

[0159] In a fourth aspect, the present application further discloses a computer program product, comprising a computer program, which implements the steps of the method in any of the above-mentioned embodiments when executed by a processor.

[0160] The present application discloses a fusion positioning method, system, storage medium, and program product with the same technical concept. The technical details of the four embodiments are applicable to each other and will not be described in detail here to reduce repetition.

[0161] Those skilled in the art will clearly understand that, for the sake of convenience and brevity of description, only the division of the above-mentioned program modules is used as an example for illustration. In actual applications, the above-mentioned functions can be assigned to different program modules as needed, that is, the internal structure of the device can be divided into different program units or modules to complete all or part of the functions described above. The program modules in the embodiment can be integrated into one processing unit, or each unit can exist physically alone, or two or more units can be integrated into one processing unit. The above-mentioned integrated unit can be implemented in the form of hardware or in the form of a software program unit. In addition, the specific names of the program modules are only for the purpose of distinguishing each other and are not used to limit the scope of protection of this application.

[0162] Although the preferred embodiments of the present application have been described, those skilled in the art may make additional changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present application.

Claims

1. A fusion positioning method, characterized in that: The steps include: Using a laser radar to collect raw point cloud data of a target device, and preprocessing the collected raw point cloud data to obtain valid point cloud data; Analyzing the pre-processed effective point cloud data fused with inertial measurement data collected by an inertial measurement unit using a laser radar mileage calculation method to obtain a first estimated position and posture of the target device; Extracting features of preset components from the valid point cloud data, and matching the extracted point cloud features of the preset components with a known distribution map of the preset components in the current site using an adaptive Monte Carlo positioning method to obtain a second estimated pose; The first estimated pose and the second estimated pose are fused using an extended Kalman filter algorithm, and a final pose of the target device is obtained by outputting the result in real time.

2. A fusion positioning method according to claim 1, characterized in that: The preprocessing of the collected original point cloud data to obtain valid point cloud data specifically includes: A height threshold is set, valid point clouds are screened from the original point cloud data, and valid information of the valid point clouds is generated, where the valid information includes the position of the valid point cloud and the distance between the valid point cloud and adjacent point clouds.

3. A fusion positioning method according to claim 1, characterized in that: The method of using the laser radar mileage calculation method to analyze the pre-processed effective point cloud data and the inertial measurement data collected by the inertial measurement unit to obtain the first estimated position and posture of the target device specifically includes: Pre-integrating the inertial measurement data to calculate a first position change of the target device; Using a closest point iterative algorithm to register the valid point clouds between two adjacent frames to obtain a second posture change of the target device; A nonlinear optimization method is used to combine the first posture change and the second posture change to obtain the first estimated posture.

4. A fusion positioning method according to claim 3, characterized in that: The method of registering the valid point clouds between two adjacent frames using the closest point iterative algorithm to obtain the second posture change of the target device specifically includes: Input adjacent first frame point cloud data and second frame point cloud data; for each source point cloud in the first frame point cloud data, find the point cloud closest to it in the second frame point cloud data as the target point cloud; Calculate the rotation matrix and translation vector between each source point cloud and its corresponding target point cloud; Applying the rotation matrix and the translation vector to the source point cloud to obtain a new transformed point cloud; The error between the transformed point cloud and the target point cloud is determined, and the iteration is stopped if the error is smaller than the target error.

5. A fusion positioning method according to claim 2, characterized in that: The step of extracting preset component features from the valid point cloud data specifically includes: Projecting the valid point cloud onto a two-dimensional plane parallel to the ground of the photovoltaic power station, and performing horizontal clustering: grouping the valid point clouds whose first distance in the horizontal direction is less than a first threshold into one category; Vertically merging the valid point clouds classified into one category: grouping the valid point clouds whose second distance in the vertical direction is less than a second threshold into one cluster; Filtering the preset component cluster from the clustered point cloud clusters according to the geometric features of the preset component; For each of the preset component clusters, its geometric data is calculated; and the extracted preset component clusters are visualized and saved.

6. A fusion positioning method according to claim 1, characterized in that: The method of using the adaptive Monte Carlo positioning method to match the extracted point cloud features of the preset components with the known distribution map of the preset components in the current site to obtain a second estimated pose specifically includes: generating a group of randomly distributed particles according to an initial position estimate of the target device, and initializing the particles; Matching the point cloud features of the preset components with the known distribution map of the preset components in the current site based on the Monte Carlo positioning method; calculating the likelihood of the particle at the current position to obtain a matching weight for each particle; According to the matching weight of each particle, the particles with high matching weight are preferentially selected, and a new particle set is generated by resampling; According to the poses and matching weights of all the particles, the pose of the particle with the largest matching weight is taken as the second estimated pose of the target device.

7. The fusion positioning method according to claim 1, wherein: The method of using the extended Kalman filter algorithm to fuse the first estimated pose and the second estimated pose, and outputting the final pose of the target device in real time, specifically includes: Inputting the first estimated pose and the second estimated pose as observation values ​​into an extended Kalman filter algorithm; Establishing an observation model and a process model; using the process model to predict the state estimation and covariance matrix of the target device at the current moment; For a plurality of the observation values, sequentially calculating their Kalman gains, updating the state estimation and updating the covariance matrix; By calculating a weighted average of the covariance matrices of the plurality of observation values, a fused covariance matrix is ​​obtained, and the first estimated pose and the second estimated pose are fused, thereby obtaining a final pose of the target device.

8. A fusion positioning system comprising a memory, a processor, and a computer program stored in the memory, characterized in that: The processor executes the computer program to implement the steps of the method according to any one of claims 1 to 7.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that When the computer program is executed by a processor, the steps of the method described in any one of claims 1 to 7 are implemented.

10. A computer program product comprising a computer program, characterized in that When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

Images