A 3D LiDAR-based AGV Natural Scene Localization Method and System

Abstract

The present application provides a method and system for natural scene positioning of an AGV based on a 3D lidar, which relates to the technical field of automatic guided vehicle control. The method includes: constructing matching submap information to provide a key reference for subsequent positioning; using a vision sensor to capture multiple image data, and at the same time, real-time collecting IMU data and odometer data of the current pose of the AGV, and accurately analyzing the motion state of the AGV based on this. The 3D lidar obtains scene point cloud data to construct a local map, registers it with the matching submap to obtain an initial pose estimate, and finally integrates the initial pose, motion state, and image data, and determines the final pose information of the AGV through complex calculations. Multiple sources of data work together to comprehensively improve the accuracy and reliability of the AGV's positioning in the natural scene.

Description

Technical Field

[0001] The present application relates to the field of automatic guided vehicle control technology, and in particular to a 3D laser radar-based AGV natural scene positioning method and system. Background Art

[0002] In AGV (Automated Guided Vehicle) warehousing and handling projects, autonomous positioning is one of the core basic technologies. Motion planning and control, and traffic control and dispatching are inseparable from accurate and stable positioning functions. However, due to the large differences in different industrial scenarios, AGVs still rely heavily on manually deployed feature-marked objects to achieve accurate positioning. The resulting problems are high deployment costs and workload, as well as specific environments and whether users are willing to support it. With the rise of 3D lasers, pure natural scene positioning technology has gradually become possible.

[0003] In the existing methods, the precise positioning of the lidar is obtained by calculating the particle score. This method samples a large number of particles in the possible pose space, and then calculates the particle score according to the degree of match between each particle and the reference map, and finally selects the particle with the highest score as the current pose estimate of the AGV. In the specific implementation, the system generates hundreds to thousands of particles representing possible poses, each particle representing the possible position and orientation of the AGV. The weight of each particle is then calculated by comparing the similarity between the laser scanning results predicted by the particle and the actually observed laser scanning data. The weight calculation usually uses the laser endpoint model method to evaluate the difference between the observed value and the predicted value. Finally, the system estimates the actual pose of the AGV based on the particle weight distribution, and retains the high-weight particles and eliminates the low-weight particles by resampling.

[0004] However, in some degraded scenarios such as narrow channels, open areas and uneven ground conditions, the positioning function still poses challenges, such as poor positioning accuracy, reduced stability and even positioning failure. Therefore, traditional methods do not take into account the universal applicability of open and narrow scenarios. Summary of the invention

[0005] The present application provides an AGV natural scene positioning method and system based on 3D laser radar, which is used to solve the problem of difficulty in accurately positioning an automatic guided vehicle (AGV) in complex and changeable natural scenes.

[0006] In a first aspect, the present application provides an AGV natural scene positioning method based on a 3D lidar, which is applied to a scene positioning system. The method includes: constructing matching submap information; acquiring multiple image data through a vision sensor; acquiring IMU data and odometer data of the current pose of the AGV in real time; determining the motion state information of the AGV according to the IMU data and the odometer data; acquiring point cloud data of the current scene through the 3D lidar; constructing local map information according to the point cloud data; registering the local map information with the matching submap information to determine the initial pose estimation information of the AGV; and determining the final pose information of the AGV by combining the initial pose estimation information, the motion state information, and the image data.

[0007] By adopting the above technical solution, the image data of the vision sensor, the motion state data of the IMU and the odometer, and the point cloud data of the 3D lidar complement each other. A local map is constructed and registered with the matching submap to obtain the initial pose, and then optimized by combining multi-source data. This process makes full use of the advantages of each sensor. Whether it is the wide-area recognition of an open scene or the fine feature capture of a narrow scene, it can accurately perform initial positioning. The comprehensive optimization overcomes the limitations of a single sensor, ensuring that the positioning method has high adaptability and accuracy in different natural scenes.

[0008] Combined with some embodiments of the first aspect, in some embodiments, the step of determining the final pose information of the AGV by combining the initial pose estimation information, the motion state information, and the image data specifically includes: combining the initial pose estimation information, the motion state information, and the image data to determine pose optimization parameters through a pose optimization model, which is constructed in advance through deep learning according to multiple initial pose estimation information, motion state information, and image data sets with pose optimization parameter annotations; and correcting the initial pose estimation information by combining the pose optimization parameters to determine the final pose information.

[0009] By adopting the above technical solution, a pose optimization model constructed through deep learning is used to determine pose optimization parameters by combining the initial pose estimation information, the motion state information, and the image data. The deep learning model can learn the complex relationship between multi-source data and pose optimization parameters, improving the accuracy of the parameters. Correcting the initial pose estimation information according to these parameters can further optimize the positioning accuracy of the AGV, reduce the positioning error, make the positioning of the AGV more accurate in different scenes, and better meet the actual application requirements.

[0010] In some embodiments in combination with some embodiments of the first aspect, in the step of constructing the matching subgraph information, it specifically includes: obtaining global map data, dividing the global map data into grid cells, and each of the grid cells contains FAST corner point quantity information; calculating the environmental complexity through the information entropy formula E = -Σ(pi×log(pi)) based on the FAST corner point quantity information, where pi represents the proportion of FAST corner points in the i-th grid cell, and the environmental complexity is used to characterize the distribution of scene features; dynamically adjusting the point cloud sampling density according to the environmental complexity, and the dynamic adjustment includes performing encrypted sampling when the environmental complexity is greater than the first threshold T1, and performing sparse sampling when the environmental complexity is less than the second threshold T2, where T1 and T2 are adaptively adjusted according to the historical sampling effect; obtaining point cloud data according to the point cloud sampling density, performing an ordered analysis on the point cloud data to obtain a key frame set; calculating the distance between the current key frame and other key frames in the key frame set, and determining the key frames with a distance less than the preset distance threshold as adjacent key frames; determining a key frame convex hull by combining the adjacent key frames through a preset algorithm; analyzing the key frame convex hull through a preset algorithm to obtain a key frame concave hull; constructing a matching subgraph by combining the adjacent key frames, the key frame convex hull and the key frame concave hull.

[0011] By adopting the above technical solution, when constructing the matching subgraph, the global map is divided into grid cells, the environmental complexity is calculated according to the FAST corner point quantity, and the point cloud sampling density is dynamically adjusted according to the complexity, so that the point cloud data can be obtained more reasonably. After obtaining the key frame set, the adjacent key frames, the key frame convex hull and the concave hull are determined, and these steps comprehensively consider the local and global features of the scene. The matching subgraph constructed in this way can more accurately reflect the scene features, and in the subsequent registration with the local map, it can improve the accuracy of registration, thereby improving the positioning accuracy of the AGV.

[0012] In combination with some embodiments of the first aspect, in some embodiments, before the step of constructing a matching subgraph by combining the adjacent key frame, the convex hull of the key frame, and the concave hull of the key frame, it further includes: establishing a spatio-temporal scoring mechanism for the feature points in the sampled key frames. The spatio-temporal scoring mechanism includes: setting a historical tracking window W for the feature points, calculating the time dimension score St based on the tracking stability of the feature points within the window period, where St = (number of successfully tracked frames / W) × (1 + feature point position variance compensation term); determining the descriptor cosine similarity between the feature points and their corresponding adjacent feature points, and combining the distance weights to determine the spatial dimension discrimination score Sd; comprehensively calculating the final score S of the feature points as S = St × Sd × (1 + C), where C is a context reward factor used to balance the spatial distribution of the feature points; calculating the point group quality measure Q = Σ(Si × Wi) based on the final score S of the feature points and the preset feature point spatial distribution weight Wi, where Wi is used to prevent the feature points from being overly concentrated; determining a feature point pool of a set size according to the point group quality measure Q, and dynamically updating the feature points in the key frame when the final score of a new feature point exceeds the lowest score in the feature point pool.

[0013] By adopting the above technical solution, a spatio-temporal scoring mechanism is established for the feature points in the sampled key frames, and the final score of the feature points is obtained by comprehensively considering the time dimension score, the spatial dimension discrimination score, and the context reward factor. Then, the feature point pool is determined according to the point group quality measure, and the feature points are dynamically updated. This makes the feature points in the key frame more representative and stable, and avoids the over-concentration of feature points. When constructing the matching subgraph, it can provide higher-quality feature information, improve the quality of the matching subgraph, and further enhance the accuracy and reliability of AGV positioning.

[0014] In combination with some embodiments of the first aspect, in some embodiments, the step of obtaining the point cloud data of the current scene by using a 3D lidar specifically includes: establishing a multi-layer reflection intensity discrimination model, dividing the point cloud obtained by the 3D lidar into three levels of high, medium, and low according to the reflection intensity, and determining the feature extraction strategy for objects with different reflection characteristics; processing the echo signal of the laser pulse by using the wavefront reconstruction technology according to the feature extraction strategy, and extracting the multi-echo data by analyzing the waveform characteristics of the echo signal; dynamically adjusting the scanning frequency and resolution of the 3D lidar according to the environmental complexity, the echo data, and the AGV motion state.

[0015] By adopting the above technical solution, a multi-layer reflection intensity discrimination model is established, the point cloud is stratified according to the reflection intensity, and the feature extraction strategy is determined for objects with different reflection characteristics, so that features can be extracted more accurately. The echo signal is processed by the wavefront reconstruction technology to extract multi-echo data, enriching the point cloud information. The scanning frequency and resolution are dynamically adjusted according to the environmental complexity, echo data and AGV motion state, enabling the 3D lidar to better adapt to different scenarios, obtain more accurate and comprehensive point cloud data, and provide a more reliable basis for AGV positioning.

[0016] Combined with some embodiments of the first aspect, in some embodiments, after the step of obtaining the point cloud data of the current scene by the 3D lidar, it further includes: determining the density distribution of the point cloud data in the horizontal and vertical directions; when the point cloud density in the horizontal direction is lower than the lower limit of the set threshold, determining that the current scene is an open scene; when the point cloud density in the horizontal direction is higher than the upper limit of the set threshold and the distribution density is greater than the set density threshold, determining that the current scene is a narrow channel scene; adopting corresponding optimization strategies for different scene types.

[0017] By adopting the above technical solution, the density distribution of the point cloud data in the horizontal and vertical directions is determined to judge the scene type. The point cloud density characteristics of different scene types are different, and the scene can be accurately identified through density threshold judgment. After identifying different types such as open scenes and narrow channel scenes, it provides a basis for subsequent adoption of corresponding optimization strategies, enabling the AGV positioning system to be adjusted according to the characteristics of different scenes, and improving the adaptability and accuracy of positioning.

[0018] Combined with some embodiments of the first aspect, in some embodiments, the step of adopting corresponding optimization strategies for different scene types specifically includes: for an open scene, dynamically expanding the scanning radius of the 3D lidar scanning point cloud and increasing the weight of the ground point cloud feature points at the same time; for a narrow channel scene, using the corridor characteristic analysis algorithm to determine the channel direction vector V and improving the confidence of the pose estimation in the direction parallel to the channel direction vector V; for the ground uneven scene, detecting abnormal height changes by analyzing the height gradient distribution of the point cloud data in the local area; when the detected abnormal height change exceeds the preset height change threshold, starting the adaptive attitude compensation algorithm to eliminate the point cloud distortion caused by the uneven ground; for a dynamic environment, using the background separation technology to identify and filter the point cloud data generated by moving objects to reduce the interference of dynamic obstacles on positioning.

[0019] By adopting the above technical solutions, corresponding optimization strategies are taken for different scenario types. In an open scenario, the scanning radius is enlarged and the weight of the feature points of the ground point cloud is increased, which can increase the effective point cloud information; in a narrow channel scenario, the confidence of the pose estimation parallel to the channel direction is improved to make the positioning more in line with the characteristics of the scenario; in a scenario with uneven ground, the point cloud distortion is eliminated to ensure the accuracy of the point cloud data; in a dynamic environment, the point cloud data of moving objects is filtered to reduce interference. These strategies combined can enable the AGV to achieve accurate positioning in various complex scenarios and improve the stability and reliability of the positioning.

[0020] In a second aspect, the present application provides a scenario positioning system, which includes: one or more processors and a memory; the memory is coupled to the one or more processors, and the memory is used to store computer program code, and the computer program code includes computer instructions, and the one or more processors call the computer instructions to enable the scenario positioning system to execute the method described in the first aspect and any possible implementation manner in the first aspect.

[0021] In a third aspect, the present application provides a computer-readable storage medium, including instructions, which when running on a scenario positioning system, enable the scenario positioning system to execute the method described in the first aspect and any possible implementation manner in the first aspect.

[0022] In a fourth aspect, the present application provides a computer program product, which when running on a scenario positioning system, enables the scenario positioning system to execute the method described in the first aspect and any possible implementation manner in the first aspect.

[0023] One or more technical solutions provided in the embodiments of the present application have at least the following technical effects or advantages:

[0024] 1. Due to the adoption of the technical means of multi-sensor fusion positioning, that is, image data is obtained through a vision sensor, the motion state information is provided by an IMU and an odometer, and the point cloud data is collected by a 3D lidar, and the local map information constructed by the 3D lidar is registered with the matching sub-map information obtained by processing the global map data, therefore, the technical problems of inaccurate positioning of a single sensor and difficulty in adapting to different natural scenarios in the prior art are effectively solved, and further, the technical effect of accurate and stable positioning of the AGV in diverse natural scenarios such as open and narrow scenarios is achieved.

[0025] 2. Due to the adoption of a technical means of dynamically constructing matching subgraphs based on environmental complexity, specifically dividing the global map into grid units, calculating the environmental complexity based on the number of FAST corner points in the grid, and dynamically adjusting the point cloud sampling density to obtain a set of key frames, and then constructing a matching subgraph in combination with adjacent key frames, key frame convex hull and concave hull. Therefore, the technical problem of the lack of specificity in the construction of matching subgraphs and the inability to adapt to changes in complex scenes in the existing technology is effectively solved, thereby achieving the technical effect of matching subgraphs accurately reflecting the characteristics of different natural scenes and improving the accuracy and universality of AGV positioning.

[0026] 3. Due to the adoption of technical means for intelligently distinguishing scene types based on point cloud density, by determining the density distribution of point cloud data acquired by 3D lidar in the horizontal and vertical directions, the scene types such as open and narrow are accurately determined according to the set threshold. Therefore, the technical problems of the existing technology that it is difficult to adjust the positioning strategy in real time according to the scene and the poor adaptability are effectively solved, thereby achieving the technical effect of providing accurate basis for subsequent targeted optimization strategies and improving the overall applicability of the positioning system. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] Figure 1 It is a flow chart of a natural scene positioning method of AGV based on 3D laser radar in an embodiment of the present application;

[0028] Figure 2 It is another flowchart of the AGV natural scene positioning method based on 3D laser radar in an embodiment of the present application;

[0029] Figure 3 It is a schematic diagram of the structure of a physical device of the scene positioning system in an embodiment of the present application. DETAILED DESCRIPTION

[0030] The terms used in the following embodiments of the present application are only for the purpose of describing specific embodiments, and are not intended to be used as limitations to the present application. As used in the specification and appended claims of the present application, the singular expressions "one", "a kind of", "said", "above", "the" and "this" are intended to also include plural expressions, unless there is a clear indication to the contrary in the context. It should also be understood that the term "and / or" used in the present application refers to and includes any or all possible combinations of one or more listed items.

[0031] In the following, the terms "first" and "second" are used for descriptive purposes only and are not to be understood as suggesting or implying relative importance or implicitly indicating the number of the indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of the features, and in the description of the embodiments of the present application, unless otherwise specified, "plurality" means two or more.

[0032] For ease of understanding, the method provided in this embodiment will be described in terms of its process below. Please refer to Figure 1 , which is a schematic flowchart of the AGV natural scene positioning method based on 3D lidar in the embodiments of this application.

[0033] S101. Construct matching sub-map information;

[0034] The matching sub-map information is a set of refined map data that is pre-constructed and highly summarizes the scene features, and is used to provide an accurate global scene reference benchmark for subsequent AGV positioning. It integrates important elements such as key geometric features, spatial layout information, and the distribution law of feature points in the scene. When the AGV is running, the local map information constructed from the current scene point cloud data obtained by the 3D lidar needs to be registered with the matching sub-map information. Through this registration, the system can quickly determine the approximate position and pose of the AGV in the global scene, that is, the initial pose estimation information, laying a solid foundation for subsequent accurate pose calculation by combining other sensor data. This step will be described in detail in steps S201 - S208 and will not be elaborated here.

[0035] S102. Obtain multiple image data through visual sensors;

[0036] The scene positioning system is equipped with multiple visual sensors with high resolution and wide viewing angle, such as industrial-grade cameras, to capture the surrounding environment information of the AGV in all directions. These visual sensors are reasonably installed at different positions of the AGV to ensure that the field of view covers the AGV's driving path and the surrounding area, reducing visual blind spots.

[0037] During the operation of the AGV, the visual sensors continuously collect images at a set frame rate. The setting of the frame rate needs to comprehensively consider the speed of scene change and the system's data processing ability. In a scene with frequent dynamic changes, such as an area in a logistics warehouse where people and goods move frequently, a higher frame rate (such as 60 frames per second) can capture instantaneous changes in a timely manner; while in a relatively static scene, such as a fixed operation area in a specific production workshop, a frame rate of 30 frames per second can not only meet the positioning requirements but also reduce the system's data processing burden.

[0038] The collected image data will be preprocessed first. First is the denoising process, using an advanced non-local means filtering algorithm. This algorithm estimates the true value of the current pixel by finding similar pixel blocks in the image, effectively removing noise while maximizing the retention of image details and avoiding the edge blurring problem that may be caused by traditional filtering algorithms. Then, grayscale processing is performed to convert the color image into a grayscale image, simplifying the subsequent data processing flow and improving processing efficiency. Finally, the histogram equalization algorithm is used to enhance the image contrast, making the object edges and textures in the image clearer to obtain the final image data.

[0039] S103. Obtain the IMU data and odometer data of the AGV's current pose in real time;

[0040] The IMU (Inertial Measurement Unit) in the scene positioning system consists of a high-precision accelerometer and a gyroscope, which work closely together to monitor the motion state of the AGV in real time. The accelerometer is responsible for measuring the acceleration information of the AGV in three axes (x, y, z axes), and the gyroscope measures the angular velocity information of the AGV. The IMU collects data at an extremely high frequency, usually hundreds or even thousands of times per second, ensuring that the minute motion changes of the AGV are captured in a timely manner and providing real-time data support for accurate pose estimation.

[0041] After the system obtains the IMU data, it first performs data calibration. Due to the inherent problems of the IMU sensor such as zero bias and scale factor error, the collected data needs to be calibrated. A calibration algorithm based on Kalman filtering is adopted. This algorithm dynamically estimates and compensates for the errors according to the historical data and current measurement values of the IMU, effectively improving the data accuracy. During the calibration process, the system monitors the working state of the IMU in real time. Once abnormal data is detected, the fault diagnosis and repair mechanism is immediately activated to ensure the reliability of the IMU data. For example, when a jump in the accelerometer data is detected, the system combines the gyroscope data and historical acceleration data for judgment. If it is confirmed as abnormal, the accelerometer is automatically recalibrated or switched to a backup sensor.

[0042] The odometer is installed on the drive wheels of the AGV. By measuring the number of rotations and the diameter of the drive wheels, the driving distance and direction change of the AGV are accurately calculated. The data acquisition frequency of the odometer is related to the motion speed of the AGV. The faster the speed, the higher the acquisition frequency to ensure the real-time nature of the data. To improve the accuracy of the odometer data, the system adopts a dual-encoder design, with encoders installed on both sides of the drive wheels. By comparing the data of the two encoders, the measurement errors caused by factors such as wheel slippage and wear can be effectively eliminated. For example, when slight slippage occurs on one side of the wheel, the data of the other encoder can be used as a reference and compensated through an algorithm to ensure the accuracy of the odometer data.

[0043] S104. Determine the motion state information of the AGV based on the IMU data and the odometer data;

[0044] After the scene positioning system obtains IMU data and odometer data, it will first perform in-depth fusion processing on these two types of data. The system uses the Extended Kalman Filter (EKF) algorithm, which fully combines the acceleration and angular velocity information measured by the IMU and the driving distance and direction change data measured by the odometer. Since the IMU data has a high update frequency but has cumulative errors, and the odometer data is relatively stable but affected by factors such as wheel slippage, the EKF algorithm can find a balance between the two, and accurately determine the motion state information of the AGV by continuously iteratively updating the state estimate.

[0045] The system will accurately calculate the speed and acceleration of the AGV based on the fused data. For speed calculation, not only the integration result of the IMU acceleration data is considered, but also the driving distance change of the odometer within a certain time interval is combined, and a more accurate speed value is obtained through weighted averaging. For example, when calculating the horizontal speed, if the IMU integrated speed is v1 and the odometer calculated speed is v2, weights w1 and w2 (w1 + w2 = 1) are assigned according to their reliability under different road conditions, and the final speed v = w1×v1 + w2×v2. The calculation of acceleration is based on the real-time change of the IMU acceleration data and is calibrated using the odometer data to remove abnormal fluctuations caused by factors such as vibration, so that the acceleration information can better reflect the true motion trend of the AGV.

[0046] The system will also determine the attitude and heading angle of the AGV based on the gyroscope data of the IMU and the direction change data of the odometer. The angular velocity measured by the gyroscope is converted into attitude angle changes through quaternion transformation, and the attitude angle is optimized by combining the turning information recorded by the odometer. For example, when the odometer detects that the AGV is turning, the system will adjust the heading angle calculated by the gyroscope according to the turning radius and driving distance to ensure the accuracy of the AGV attitude and heading angle, providing reliable direction information for subsequent positioning.

[0047] S105. Obtain the point cloud data of the current scene through a 3D lidar;

[0048] In the scene positioning system of an Automated Guided Vehicle (AGV), the 3D lidar scans the surrounding environment comprehensively in a specific scanning mode. During the scanning process, the 3D lidar continuously emits laser beams, and these laser beams will reflect back after encountering objects in the surrounding environment. The lidar calculates the distance between each measurement point and the AGV by precisely measuring the flight time of the laser beam, that is, the time interval from emission to reception. According to the principle of the constancy of the speed of light, for example, if the laser flight time is t and the speed of light is c, then the distance d between the measurement point and the AGV is d = c×t / 2 (because the laser travels back and forth). Through a large number of such measurement points, the 3D lidar can obtain a vast amount of point cloud data. These data are like countless tiny coordinate points, densely distributed in space, accurately depicting the contours and position information of objects in the surrounding environment, providing a basis for subsequent positioning and environmental perception.

[0049] To understand and utilize these point cloud data more deeply, the scene positioning system establishes a multi-layer reflection intensity discrimination model. The point cloud obtained by the 3D lidar has different reflection intensities, which are caused by factors such as the material, surface roughness, and color of the object. The system divides the point cloud into three levels: high, medium, and low according to the reflection intensity. For objects with high reflection intensity, such as metal shelves and glass curtain walls, they have a strong ability to reflect laser, and the reflection signal is clear and strong. For such objects, the system adopts a feature extraction strategy of edge extraction and contour fitting. Using techniques such as edge detection algorithms, it accurately extracts the edge information of the object, and then through fitting algorithms such as the least squares method, it obtains the accurate contour of the object, thereby accurately identifying and positioning these objects.

[0050] For objects with medium reflection intensity, such as ordinary wooden shelves and plastic cargo boxes, their reflection intensity is moderate. The system will focus on their surface texture and shape features. It uses a feature point-based matching algorithm to extract unique feature points on the surface of the object, and identifies and matches the object through these feature points to obtain its shape and position information.

[0051] For objects with low reflection intensity, such as goods covered with dark fabrics and black rubber products, they have weak reflection of laser, and it is relatively difficult to obtain their features. The system adopts algorithms to enhance the reflection signal, such as increasing the laser emission power and optimizing the sensitivity of the receiving circuit, and at the same time combines filtering and signal enhancement technologies to extract effective feature information from the weak reflection signal.

[0052] During the process of acquiring point cloud data, the 3D lidar also uses wavefront reconstruction technology to process the echo signals of laser pulses. When the laser propagates, when it encounters objects with different properties, the waveform of the echo signal will change. By analyzing the waveform characteristics of the echo signal, the system can extract multi-echo data. For example, when the laser beam encounters a transparent or semi-transparent object, multiple reflections will occur. Traditional methods may only be able to capture one reflection signal, while wavefront reconstruction technology can separate and extract multi-echoes, so as to more accurately determine information such as the position, thickness, and shape of the object. This is very important in practical applications. For example, in a warehouse, there may be some goods wrapped in transparent plastic films. Through multi-echo data, the AGV can more accurately perceive the presence and position of these goods and avoid collisions.

[0053] In addition, to adapt to different environments and the motion states of AGVs, the system will dynamically adjust the scanning frequency and resolution of the 3D lidar according to the environmental complexity, echo data, and AGV motion states. In a complex environment, such as a warehouse area with dense shelves and messy cargo placement, the environmental complexity is high, and more detailed information needs to be obtained to accurately identify and locate objects. At this time, the system will increase the scanning frequency, increase the number of laser beams emitted and received per unit time, and at the same time increase the resolution to make the measurement points denser, so as to obtain more accurate point cloud data. On the contrary, in a relatively empty area, the environmental complexity is low. Reducing the scanning frequency and resolution can reduce the data volume, improve the data processing efficiency, and also reduce the energy consumption of the lidar.

[0054] When the AGV moves at a relatively high speed, in order to ensure that the information of the surrounding environment can be obtained in time and avoid errors caused by motion, the system will correspondingly increase the scanning frequency to ensure that the point cloud data can be quickly updated during the movement of the AGV. When the AGV is close to stationary or moving slowly, the scanning frequency can be appropriately reduced. According to the quality and stability of the echo data, the system will also dynamically adjust the scanning parameters. If the echo signal is weak or there is more interference, the scanning frequency and transmission power will be increased to obtain more reliable point cloud data.

[0055] Through the above series of technical means, the 3D lidar can acquire high-quality, comprehensive and scene-adaptive point cloud data, providing solid data support for the accurate positioning and environmental perception of AGVs in complex natural scenes.

[0056] In some embodiments, in order to improve the accuracy and adaptability of positioning, targeted optimization strategies are adopted according to the characteristics of different scenarios. Specifically, for the acquired point cloud data, the density distribution in the horizontal and vertical directions is calculated respectively. When the point cloud density in the horizontal direction is lower than the lower limit of the preset threshold, it means that there are fewer reference objects in the scene. At this time, the current scene is determined to be an open scene. For example, in a large open-air square, the point cloud density in the horizontal direction will be very low. If the point cloud density in the horizontal direction is higher than the upper limit of the set threshold and the distribution density is also greater than the set density threshold, it indicates that the objects in the scene are densely distributed in the horizontal direction and show a specific distribution pattern, and it can be determined as a narrow passage scene. The narrow passages between the shelves in a warehouse belong to this type of scene.

[0057] For an open scene, the scanning radius of the 3D lidar scanning point cloud can be dynamically increased, so as to obtain point cloud data in a wider area, increase the reference information in the scene, and contribute to more accurate positioning. In an open scene, the ground is usually a relatively stable reference element. By increasing the weight of the feature points of the ground point cloud, the positioning algorithm can pay more attention to the ground information, thereby improving the stability of positioning. For a narrow passage scene, the corridor characteristic analysis algorithm is adopted to determine the direction vector V of the passage according to the point cloud data. This vector represents the orientation of the passage and is an important reference information in the narrow passage scene, which can improve the confidence of the pose estimation in the direction parallel to the passage direction vector V. Because in a narrow passage, the AGV (Automated Guided Vehicle) mainly moves along the passage direction, accurate pose estimation is crucial for avoiding collisions and efficient navigation. For a scene with uneven ground, the height gradient distribution of the point cloud data in a local area can be analyzed to detect whether there are abnormal height changes on the ground. The height gradient distribution reflects the undulation of the ground. When the detected abnormal height change exceeds the preset height change threshold, it indicates that the uneven ground has a greater impact on positioning. At this time, the adaptive attitude compensation algorithm is started to adjust the attitude of the AGV, eliminate the point cloud distortion caused by the uneven ground, and ensure the accuracy of positioning. For a dynamic environment, the background separation technology is used to identify the point cloud data generated by moving objects. The background separation technology distinguishes the static background and dynamic objects by analyzing information such as the motion characteristics and time series of the point cloud data. Then the point cloud data generated by moving objects is filtered out to reduce the interference of dynamic obstacles on positioning and ensure the stability of positioning.

[0058] S106. Construct local map information according to the point cloud data;

[0059] After the scene positioning system obtains the point cloud data collected by the 3D lidar, it first preprocesses the data. Using the statistical filtering algorithm, according to the spatial distribution characteristics of the point cloud data, the outlier points are removed to avoid abnormal data interfering with subsequent map construction. For example, in a warehouse scenario, occasionally occurring reflection abnormal points may be misjudged as object features. Statistical filtering removes the points that deviate from the normal distribution by calculating the neighborhood statistical information of the points. At the same time, the voxel downsampling algorithm is adopted to reduce the amount of point cloud data and improve the processing efficiency without losing key features. This algorithm divides the space into small voxels, and the point clouds within each voxel are merged into a representative point to reduce data redundancy.

[0060] After completing the preprocessing, the system uses the point cloud clustering algorithm to perform clustering analysis on the data. DBSCAN (Density-Based Spatial Clustering of Applications with Noise) is a commonly used method, which identifies clusters based on the density distribution of the point cloud. In a warehouse environment, the point clouds of different objects such as shelves and goods have different densities, and DBSCAN can distinguish these point clouds to form different clusters, and each cluster represents an independent object or a part of an object.

[0061] For each cluster, the system extracts its geometric features. For regular object clusters, such as shelves, the fitting algorithm is used to calculate size parameters such as length, width, and height, as well as position and orientation information; for irregular object clusters, such as goods with various shapes, the contour extraction algorithm is used to obtain their external contour features and calculate the centroid position. These features are used to build object models. For example, a polygon mesh model is used to represent the object shape, and the object model is integrated into the local map to endow the local map with semantic information, enabling the system to distinguish different objects.

[0062] To ensure the real-time and accuracy of the local map, the system adopts an incremental map update strategy. When the AGV moves, the newly obtained point cloud data is matched and fused with the already constructed local map. The Iterative Closest Point (ICP) algorithm is used to find the best matching relationship between the new point cloud data and the existing data in the map. During the matching process, through continuous iteration and optimization, the distance error between the two groups of point clouds is minimized, so as to integrate the new object model and feature information into the local map, realizing the real-time update of the map and ensuring that it can accurately reflect the changes in the environment around the AGV.

[0063] S107. Register the local map information with the matching sub-map information to determine the initial pose estimation information of the AGV;

[0064] When the scene positioning system performs the registration of the local map and the matching sub-map, it will first preprocess the local map and the matching sub-map. For the local map, the system will use a point cloud filtering algorithm to remove noise points. For example, it will use a statistical filtering method to remove outliers that deviate from the normal distribution range according to the statistical characteristics of the spatial distribution of the point cloud data, ensuring that the point cloud data in the local map can accurately represent the surrounding environmental objects. For the matching sub-map, similar preprocessing operations will be performed, and feature extraction will also be carried out on it to facilitate more efficient registration in the follow-up.

[0065] In the selection of the registration algorithm, the system preferentially adopts a feature-based registration method. For example, it uses the Fast Point Feature Histogram (FPFH) algorithm to extract feature descriptors in the local map and the matching sub-map. The FPFH algorithm generates unique and stable feature vectors by calculating the geometric features of points and their neighborhoods, and these feature vectors can effectively describe the local geometric structure of the point cloud. Through the data structure, the corresponding relationship of the feature vectors in the local map and the matching sub-map can be quickly found, and the initial corresponding point pairs can be found. The system will also combine the Iterative Closest Point (ICP) algorithm for further optimization. The ICP algorithm iteratively finds the optimal rigid body transformation (including translation and rotation) between two sets of point clouds, minimizing the distance error between the point cloud in the local map and the corresponding points in the matching sub-map.

[0066] To cope with problems such as occlusion and interference from similar structures that may occur in complex environments, the system introduces a registration assistance strategy based on semantic information. Using the semantic information (such as object category, position relationship, etc.) obtained when constructing the matching sub-map and the local map before, the registration process is constrained. For example, if it is known that a certain area in the matching sub-map represents a shelf, and the point cloud features in the corresponding area of the local map also conform to the features of the shelf, then the matching weight of the point cloud in this part of the area can be increased during the registration process to improve the accuracy and reliability of the registration.

[0067] After the above series of registration operations, the system calculates the initial pose estimation information of the AGV in the global coordinate system according to the finally determined rigid body transformation parameters. This initial pose estimation information includes the position coordinates (x, y, z) of the AGV and the attitude angles (such as heading angle, pitch angle, roll angle), providing an important basis for more accurate positioning calculations in the follow-up.

[0068] S108. Combine the initial pose estimation information, motion state information, and image data to determine the final pose information of the AGV.

[0069] The scene positioning system takes the initial pose estimation information, motion state information, and image data as inputs and transmits them into the pose optimization model. During the training phase, the pose optimization model uses a large number of datasets annotated with pose optimization parameters for deep learning. These datasets contain rich initial pose estimation information, motion state information, and image data in different scenarios. Through the backpropagation algorithm, the model continuously adjusts its parameters to enable it to learn the complex mapping relationship between multi-source data and pose optimization parameters.

[0070] In practical applications, when new multi-source data is input, the pose optimization model first extracts features from the data. For image data, a convolutional neural network is used for feature extraction to capture visual features such as textures and shapes in the image; for motion state information, a recurrent neural network is used to process its time series features and analyze the motion trends and change rules of the AGV; for the initial pose estimation information, it is directly used as part of the input to the model. Then, the model fuses these features and performs analysis and calculations through a multi-layer neural network to output pose optimization parameters.

[0071] The system corrects the initial pose estimation information according to the pose optimization parameters output by the pose optimization model. For example, if the pose optimization parameters indicate that the AGV needs to be translated a certain distance in the x direction and rotated a certain angle around the z axis in terms of pose, the system will accordingly adjust the position and pose parameters in the initial pose estimation information. During the correction process, the system will consider the reliability of different data. If the quality of the visual image data is high and the features are obvious, the weight corresponding to the image data will be appropriately increased during the correction process; if the motion state information is stable over a period of time, its weight will also be increased to ensure the accuracy of the final pose information.

[0072] To further improve the accuracy and stability of the final pose information, the system also introduces an adaptive filtering mechanism. According to the motion state of the AGV and environmental changes, the filtering parameters are dynamically adjusted. For example, when the AGV is moving at high speed, the smoothness of the filtering is appropriately increased to reduce the impact of noise and jitter generated by the motion on the final pose; when the AGV is approaching the target position or in a complex environment, the filtering smoothness is reduced to improve the response speed to environmental changes, so that the final pose can more timely reflect the actual situation.

[0073] In the embodiments of the present application, through a series of technical means such as multi-sensor data fusion, constructing a matching subgraph based on environmental complexity, discriminating scene types according to point cloud density, and optimizing the positioning strategy, accurate and stable positioning of AGV in complex natural scenes is achieved. It not only effectively solves the problems in the prior art such as inaccurate positioning of a single sensor, lack of pertinence in constructing a matching subgraph, and difficulty in adjusting the positioning strategy in real time according to the scene, but also improves the accuracy, universality of AGV positioning and the overall applicability of the positioning system.

[0074] After combining the above content, the following further and more specific process description of the method provided in this embodiment will be given. Please refer to Figure 2 , which is another process schematic diagram of the AGV natural scene positioning method based on 3D lidar in the embodiments of the present application.

[0075] S201. Obtain global map data, and divide the global map data into grid cells, each grid cell containing FAST corner point quantity information;

[0076] The scene positioning system will first obtain global map data from a storage device or a remote server. These global map data may be obtained through previous environmental surveying, modeling, etc., covering the overall layout, terrain, object distribution, etc. of the AGV operation area. After obtaining the data, the system divides the global map into multiple grid cells according to certain rules. When dividing, factors such as the size of the map, the motion accuracy requirements of the AGV, and the limitation of computing resources will be comprehensively considered to select an appropriate grid size. For example, in a large warehouse environment, to ensure positioning accuracy, the map may be divided into square grid cells with a side length of 0.5 meters.

[0077] For each grid cell, the system extracts the FAST corner point quantity information. FAST corner points (Features from Accelerated Segment Test) are corner points identified by an algorithm for quickly detecting image corner points. In map data processing, these corner points can effectively characterize the feature richness of the scene. The system will use the FAST corner point detection algorithm to traverse the map data within each grid cell and count the number of corner points.

[0078] S202. Calculate the environmental complexity based on the FAST corner point quantity information through the information entropy formula E = -Σ(pi × log(pi)), where pi represents the proportion of FAST corner points in the i-th grid cell, and the environmental complexity is used to characterize the scene feature distribution;

[0079] After the scene positioning system obtains the FAST corner point quantity information of each grid cell, it calculates the proportion pi of the FAST corner points in each grid cell to the total number of corner points. The total number of corner points refers to the sum of the FAST corner point quantities of all grid cells in the global map. By traversing all grid cells, the total number of corner points N is counted. For the i-th grid cell, its FAST corner point quantity is ni, then Next, the system uses the information entropy formula E = -Σ(pi × log(pi)) to calculate the environmental complexity. Information entropy is an index used to measure the uncertainty of information. In this scenario, the higher the environmental complexity, the more uneven the distribution of features in the scene and the more complex the changes; the lower the environmental complexity, the more single the scene features and the more uniform the distribution.

[0080] The system will compare and analyze the calculated environmental complexity with a preset reference value. By analyzing a large amount of map data of different scenarios, the corresponding relationship between different environmental complexity intervals and scene types is established. For example, when the environmental complexity is in a relatively high interval, it may correspond to a storage area with dense shelves and messy goods placement; while when the environmental complexity is in a lower interval, it may correspond to an empty passage or an open area. This corresponding relationship provides an important basis for adjusting the positioning strategy according to the environmental complexity later.

[0081] S203. Dynamically adjust the point cloud sampling density according to the environmental complexity. The dynamic adjustment includes performing dense sampling when the environmental complexity is greater than the first threshold T1, and performing sparse sampling when the environmental complexity is less than the second threshold T2, where T1 and T2 are adaptively adjusted according to the historical sampling effect;

[0082] After calculating the environmental complexity, the scene positioning system dynamically adjusts the point cloud sampling density according to the preset first threshold T1 and second threshold T2 ((T1 > T2)). When the environmental complexity is greater than T1, it indicates that the scene features are rich and complex. At this time, the system will execute the dense sampling strategy. During the dense sampling process, the 3D lidar will increase the emission frequency of the laser beam and the scanning angle range. For example, under normal circumstances, the lidar emits 1000 laser beams per second, and at this time it may be increased to 2000 laser beams per second; the scanning angle range may also be expanded from the original 300 degrees to 360 degrees to obtain more point cloud data and more comprehensively capture the scene details. At the same time, the sampling interval will be reduced, making the obtained point cloud data denser and able to more accurately depict the object contours and position information in the scene.

[0083] When the environmental complexity is less than T2, it means that the scene features are relatively simple, and the system will execute a sparse sampling strategy. During sparse sampling, the laser beam emission frequency of the lidar is reduced, such as from 1000 laser beams per second to 500 laser beams per second. At the same time, the scanning angle range is narrowed to reduce unnecessary data acquisition and improve data processing efficiency. The sampling interval is increased to reduce the amount of point cloud data, but still ensure that key scene feature information can be obtained.

[0084] The first threshold T1 and the second threshold T2 are not fixed. The system will adaptively adjust according to the historical sampling effect. The system will record data such as the environmental complexity, sampling density, and final positioning accuracy during each sampling. If within a certain environmental complexity range, the improvement in positioning accuracy after encrypted sampling is not obvious, or sparse sampling does not result in a significant decrease in positioning accuracy, then the system will appropriately adjust the values of T1 and T2. For example, if it is found that when the environmental complexity is within a certain range, the positioning error only decreases by 1% after encrypted sampling, while the data processing time increases by 50%, then the value of T1 can be appropriately reduced so that a more reasonable sampling strategy can be adopted under this environmental complexity.

[0085] S204. Obtain point cloud data according to the point cloud sampling density, and conduct an orderly analysis of the point cloud data to obtain a set of key frames;

[0086] The scene positioning system controls the 3D lidar to obtain corresponding point cloud data according to the determined point cloud sampling density. After obtaining the point cloud data, the system conducts an orderly analysis on it. First, a filtering algorithm is used to remove noise points. For example, bilateral filtering is adopted, which can not only effectively remove noise but also better retain the edge features of the point cloud. On the basis of removing noise, the system uses a feature extraction algorithm to extract key features in the point cloud data. For example, a curvature-based feature extraction method is used to identify features such as the edges and corners of objects by calculating the curvature of the point cloud.

[0087] To obtain the set of key frames, the system uses a sliding window method to screen the processed point cloud data. A time window or a space window is set, and the changes in the point cloud data are analyzed within each window. If the point cloud data within the window shows significant changes in features compared to the data in the previous window, such as the appearance of new objects or large changes in the positions of objects, the point cloud data corresponding to this window is taken as a key frame. For example, in a warehousing scenario, when an AGV passes through the shelf area, the point cloud data of the shelves changes significantly, and these changes meet the key frame screening criteria, and the corresponding data will be recorded. When screening key frames, the system also makes judgments by combining the motion state information of the AGV. If the AGV is in a stationary state but the point cloud data changes significantly, it may indicate the presence of dynamic objects in the surrounding environment. In this case, the relevant point cloud data will also be taken as a key frame. In this way, it is ensured that the key frames can accurately reflect the important changes in the scene and provide valuable data for subsequent construction of matching subgraphs.

[0088] S205. Calculate the distances between the current key frame and other key frames in the key frame set, and determine the neighboring key frames whose distances are less than the preset distance threshold;

[0089] After obtaining the key frame set, in order to accurately find the neighboring key frames closely related to the current key frame, the scene positioning system will use a specific algorithm to calculate the distances. The system will select one or more appropriate distance metrics, such as Euclidean distance, cosine distance, or distance algorithms based on feature vectors, etc. Taking Euclidean distance as an example, it measures the distance by calculating the differences in the point cloud coordinates of two key frames in three-dimensional space. For the entire key frames, the Euclidean distances of all corresponding points will be calculated, and a comprehensive distance metric value between the two key frames will be obtained.

[0090] When determining the preset distance threshold, the system will comprehensively consider the complexity of the scene and the positioning accuracy requirements. In a complex scene, due to the rich changes in key frames, in order to ensure that the selected neighboring key frames have a high degree of correlation, the threshold will be appropriately reduced; while in a simple scene, the threshold can be increased to reduce the number of neighboring key frames and reduce the subsequent calculation amount. The threshold is not fixed, and the system will dynamically adjust it according to the actual positioning effect and historical data. For example, in a specific warehousing area, through multiple experiments, it is found that when the threshold is set to a certain specific value, the positioning accuracy is the highest and the calculation efficiency can also meet the requirements, and the system will take this value as the preset distance threshold for this area. When the distances between other key frames in the set and the current key frame are less than the preset distance threshold, the other key frames within the preset distance threshold are determined as the neighboring key frames of the current key frame.

[0091] S206. Determine the key frame convex hull by combining the preset algorithm with the neighboring key frames;

[0092] After the scene positioning system determines the adjacent key frames, it will use a preset algorithm to determine the convex hull of the key frames. Commonly used algorithms such as the Gift Wrapping algorithm, whose principle is to start from a starting point and gradually "wrap" along the edge of the point cloud to find the points that make up the convex hull.

[0093] The system first selects a point in the lower leftmost position among the adjacent key frames as the starting point, and this point must be a vertex of the convex hull. Then, based on the starting point, it calculates the angles of the lines connecting other adjacent key frames to the starting point, and selects the point with the smallest angle as the next vertex. In this way, it traverses all adjacent key frames in turn, continuously updating the vertices of the convex hull until it returns to the starting point, thereby constructing the convex hull of the key frames. When calculating the angles, the system will consider the three-dimensional spatial information of the point cloud to ensure that the convex hull can accurately reflect the distribution range of the adjacent key frames in space.

[0094] To improve the algorithm efficiency, the system will preprocess the adjacent key frames, such as removing outliers and redundant points. Outliers may cause abnormal convex hull shapes, and redundant points will increase the computational amount. Through methods such as statistical filtering and clustering analysis, remove the points that deviate from the normal distribution range and the repeated points to optimize the data quality of the adjacent key frames.

[0095] During the process of constructing the convex hull, the system will record the geometric properties of the convex hull, such as information about area, perimeter, vertex coordinates, etc. These properties not only help with subsequent analysis of the convex hull, but also provide a more detailed description of the scene features for the construction of the matching subgraph. For example, the area and perimeter of the convex hull can reflect the size and shape complexity of the area covered by the adjacent key frames, and the vertex coordinates clearly define the specific position of the convex hull in space.

[0096] S207. Analyze the convex hull of the key frames through a preset algorithm to obtain the concave hull of the key frames;

[0097] After the scene positioning system obtains the convex hull of the key frames, it will use a preset algorithm to analyze it to obtain the concave hull of the key frames. A common method is to process based on the boundary points of the convex hull. The system will first sort the boundary points of the convex hull and traverse these points in clockwise or counterclockwise order. During the traversal, it checks the geometric relationship between three adjacent points to determine whether there is a concave area.

[0098] Specifically, for three consecutive points A, B, C on the boundary of the convex hull, by calculating the cross product of the vectors and to determine their relative position relationship. If the result of the cross product indicates that point C is on the vector On the left side (judged according to the set direction), and this deviation exceeds a certain threshold, then it is considered that there may be a concave area between these three points. The system will further analyze the point cloud data in this area, and determine the boundary of the concave area by fitting curves or planes, etc., so as to construct the concave hull of the key frame.

[0099] S208. Construct a matching subgraph by combining adjacent key frames, the convex hull of key frames, and the concave hull of key frames.

[0100] After the scene localization system obtains adjacent key frames, the convex hull of key frames, and the concave hull of key frames, it starts to construct a matching subgraph. The system will organically integrate these elements to form a matching subgraph that can accurately reflect the scene characteristics. First, the point cloud data of adjacent key frames is fused to form a basic point cloud set. During the fusion process, weighted processing is performed according to the time sequence and spatial relationship of key frames, so that key frames that are closer to the current key frame and have smaller changes have higher weights to highlight the local characteristics of the scene.

[0101] Then, the information of the convex hull and concave hull of key frames is incorporated into this basic point cloud set. The vertex coordinates, geometric properties, etc. of the convex hull and concave hull are added to the point cloud set and marked as special feature points. These feature points can more accurately describe the boundary and local shape of the scene, providing richer feature information for subsequent registration with the local map.

[0102] In some embodiments, in order to optimize the construction process of the matching subgraph, before constructing the matching subgraph, a spatio-temporal scoring mechanism is established for the feature points in the sampled key frames. This mechanism evaluates the quality of feature points from multiple dimensions and realizes the dynamic update of feature points in key frames, so as to improve the quality of the matching subgraph and the accuracy of localization. Specifically as follows: Set the historical tracking window W of feature points, which is a time interval used to count the tracking situation of feature points within a period of time. The tracking stability of feature points within the window period is the key basis for calculating the time dimension score St. St = (number of successfully tracked frames / W) × (1 + variance compensation term of feature point position), where the number of successfully tracked frames refers to the number of frames in which the system can stably track this feature point within the window W. If a feature point can be stably tracked all the time within the window, the number of successfully tracked frames is equal to W, and at this time the St value is larger, indicating that the feature point is stable in the time dimension, and the variance compensation term of feature point position is used to adjust the influence brought by the change of feature point position. If the change of feature point position is small, the value of the variance compensation term is small, and the influence on St is also small; on the contrary, if the position changes greatly, the variance compensation term will increase, appropriately reducing the value of St to avoid unstable feature points from obtaining too high a score.

[0103] Calculate the spatial dimension discrimination score Sd by determining the cosine similarity of the descriptors between a feature point and its corresponding adjacent feature points and combining the distance weights. The descriptor cosine similarity is used to measure the similarity degree of the feature point and its adjacent feature points in feature description. The higher the similarity, the more similar they are in spatial features. The distance weights take into account the spatial distance between feature points. Feature points with a closer distance have a greater impact on Sd. By combining these two factors, the discrimination of feature points in the spatial dimension can be evaluated more accurately. For example, in a shelf scenario, for two adjacent and feature-similar points, if they are close to each other, their Sd scores will be relatively high; conversely, if they are far apart, even if they are feature-similar, the Sd scores will be reduced due to the influence of distance weights.

[0104] Combine the time dimension score St and the spatial dimension discrimination score Sd, and then combine the context reward factor C to calculate the final score of the feature point S = St × Sd × (1 + C). The context reward factor C is used to balance the spatial distribution of feature points and can be adjusted according to the specific situation of the scenario. In some complex scenarios where the distribution of feature points is relatively sparse, the value of C can be appropriately increased to encourage retaining more feature points to describe the scenario more comprehensively; while in scenarios where feature points are relatively dense, the value of C is reduced to avoid the influence of too many similar feature points. In this way, a final score S that can more comprehensively reflect the quality of feature points can be obtained. Calculate the point cloud quality measure Q = Σ(Si × Wi) based on the final score S of the feature points and the preset spatial distribution weight Wi of the feature points. Here, Wi is used to prevent feature points from being overly concentrated and is set according to the distribution of feature points in space. If the feature points in a certain area are too dense, the Wi value of the feature points in that area will be relatively small to reduce their impact on the overall point cloud quality measure; while for feature points with a relatively uniform distribution, the Wi value will be relatively large. Through this weighted calculation, an index Q that can reflect the overall quality of the point cloud can be obtained.

[0105] Determine a feature point pool of a set size according to the point cloud quality measure Q. The feature point pool is a set for storing feature points, and its size is set according to actual needs. When the final score of a new feature point exceeds the lowest score in the feature point pool, the feature points in the key frame are dynamically updated, which means adding the new high-quality feature points to the feature point pool and removing the feature points with the lowest score in the original pool at the same time. Through this dynamic update mechanism, it can be ensured that the feature points in the feature point pool always have high quality and representativeness, providing higher-quality feature information for subsequent construction of the matching subgraph, thereby improving the accuracy and reliability of AGV positioning.

[0106] In the embodiments of the present application, the environmental complexity is calculated by obtaining global map data and dividing grid cells, and the point cloud sampling density is dynamically adjusted accordingly to obtain a set of key frames. Then, adjacent key frames, the convex hull and concave hull of the key frames are determined to construct a matching subgraph, realizing the provision of an accurate and scene-adaptive global reference benchmark for AGV positioning.

[0107] The scene positioning system in the embodiments of the present invention will be described from the perspective of hardware processing. Please refer to Figure 3 , which is a schematic structural diagram of an entity device of the scene positioning system in the embodiments of the present application.

[0108] It should be noted that Figure 3 the structure of the scene positioning system shown is only an example, and should not bring any limitations to the functions and usage scope of the embodiments of the present invention.

[0109] As Figure 3 shown, the scene positioning system includes a central processing unit (CPU) 301, which can perform various appropriate actions and processes according to the program stored in the read-only memory (ROM) 302 or the program loaded from the storage section 308 into the random access memory (RAM) 303, such as executing the method described in the above embodiments. In the RAM 303, various programs and data required for system operation are also stored. The CPU 301, ROM 302, and RAM 303 are connected to each other through a bus 304. The input / output (I / O) interface 305 is also connected to the bus 304.

[0110] The following components are connected to the I / O interface 305: an input section 306 including an audio input device, a button switch, etc.; an output section 307 including a liquid crystal display (LCD), an audio output device, an indicator light, etc.; a storage section 308 including a hard disk, etc.; and a communication section 309 including a network interface card such as a LAN (Local Area Network) card, a modem, etc. The communication section 309 performs communication processing via a network such as the Internet. The drive 310 is also connected to the I / O interface 305 as needed. A removable medium 311, such as a magnetic disk, an optical disk, a magneto-optical disk, a semiconductor memory, etc., is installed on the drive 310 as needed, so that the computer program read from it can be installed into the storage section 308 as needed.

[0111] In particular, according to an embodiment of the present invention, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, an embodiment of the present invention includes a computer program product that includes a computer program carried on a computer-readable medium, and the computer program includes a computer program for performing the method shown in the flowchart. In such an embodiment, the computer program can be downloaded and installed from a network through the communication part 309, and / or installed from the removable medium 311. When the computer program is executed by the central processing unit (CPU) 301, various functions defined in the present invention are executed.

[0112] It should be noted that specific examples of the computer-readable storage medium may include, but are not limited to: an electrical connection having one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM), a flash memory, an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above. In the present invention, the computer-readable storage medium can be any tangible medium that contains or stores a program, and the program can be used by or in combination with an instruction execution system, apparatus, or device.

[0113] The flowcharts and block diagrams in the accompanying drawings illustrate the possible architectures, functions, and operations of systems, methods, and computer program products according to various embodiments of the present invention. Among them, each block in the flowchart or block diagram may represent a module, a program segment, or a part of code, and the above module, program segment, or part of code includes one or more executable instructions for implementing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the blocks may occur in a different order than marked in the accompanying drawings.

[0114] Specifically, the scene positioning system of this embodiment includes a processor and a memory, and a computer program is stored on the memory. When the computer program is executed by the processor, the AGV natural scene positioning method based on 3D lidar provided in the above embodiment is implemented.

[0115] As another aspect, the present invention also provides a computer-readable storage medium, which may be included in the scene positioning system described in the above embodiments; or may exist alone without being assembled into the scene positioning system. The above storage medium carries one or more computer programs, and when the one or more computer programs are executed by a processor of the scene positioning system, the scene positioning system implements the AGV natural scene positioning method based on 3D lidar provided in the above embodiments.

[0116] As described above, the above embodiments are only used to illustrate the technical solutions of the present application, rather than to limit them; although the present application has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements on some of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the present application in each embodiment.

[0117] As used in the above embodiments, according to the context, the term "when..." can be interpreted to mean "if...", "after...", "in response to determining...", or "in response to detecting...". Similarly, according to the context, the phrase "when determining..." or "if detecting (the stated condition or event)" can be interpreted to mean "if determining...", "in response to determining...", "when detecting (the stated condition or event)", or "in response to detecting (the stated condition or event)".

[0118] Those of ordinary skill in the art can understand all or part of the processes in the above embodiments of the method. These processes can be completed by relevant hardware instructed by a computer program. The program can be stored in a computer-readable storage medium. When the program is executed, it can include the processes of the above method embodiments. The foregoing storage medium includes: various media such as ROM or random access memory RAM, magnetic disk, or optical disc that can store program codes.

Claims

1. A natural scene positioning method for AGV based on 3D lidar, applied to a scene positioning system, characterized in that The method includes: Constructing matching subgraph information; Obtaining multiple image data through a visual sensor; Obtaining IMU data and odometer data of the current pose of the AGV in real time; Determining the motion state information of the AGV according to the IMU data and the odometer data; Obtaining point cloud data of the current scene through a 3D lidar; Constructing local map information according to the point cloud data; Registering the local map information with the matching subgraph information to determine the initial pose estimation information of the AGV; Combining the initial pose estimation information, motion state information, and image data to determine the final pose information of the AGV; After the step of obtaining point cloud data of the current scene through a 3D lidar, it further includes: Determining the density distribution of the point cloud data in the horizontal and vertical directions; When the point cloud density in the horizontal direction is lower than the lower limit of the set threshold, determining that the current scene is an empty scene; When the point cloud density in the horizontal direction is higher than the upper limit of the set threshold and the distribution density is greater than the set density threshold, determining that the current scene is a narrow channel scene; Adopting corresponding optimization strategies for different scene types; The step of adopting corresponding optimization strategies for different scene types specifically includes: For an empty scene, dynamically expanding the scanning radius of the 3D lidar scanning point cloud and increasing the weight of the ground point cloud feature points at the same time; For a narrow channel scene, using a corridor characteristic analysis algorithm to determine the channel direction vector V and improving the confidence of the pose estimation in the direction parallel to the channel direction vector V; For an uneven ground scene, detecting abnormal height changes by analyzing the height gradient distribution of the point cloud data in a local area; When the detected abnormal height change exceeds the preset height change threshold, starting an adaptive attitude compensation algorithm to eliminate the point cloud distortion caused by uneven ground; For a dynamic environment, using background separation technology to identify and filter the point cloud data generated by moving objects to reduce the interference of dynamic obstacles on positioning.

2. The method according to claim 1, wherein The step of combining the initial pose estimation information, motion state information, and image data to determine the final pose information of the AGV specifically includes: Combining the initial pose estimation information, motion state information, and image data to determine pose optimization parameters through a pose optimization model, and the pose optimization model is constructed in advance through deep learning according to multiple initial pose estimation information, motion state information, and image data sets with pose optimization parameter annotations; Combining the pose optimization parameters to correct the initial pose estimation information to determine the final pose information.

3. The method according to claim 1, wherein In the step of constructing matching subgraph information, it specifically includes: Obtaining global map data and dividing the global map data into grid cells, and each grid cell contains FAST corner point quantity information; Calculating the environmental complexity based on the FAST corner point quantity information through the information entropy formula E = -Σ(pi × log(pi)), where pi represents the proportion of FAST corner points in the i-th grid cell, and the environmental complexity is used to characterize the scene feature distribution. Dynamically adjust the point cloud sampling density according to the environmental complexity. The dynamic adjustment includes performing encrypted sampling when the environmental complexity is greater than the first threshold T1, and performing sparse sampling when the environmental complexity is less than the second threshold T2, where T1 and T2 are adaptively adjusted according to the historical sampling effect; Obtain point cloud data according to the point cloud sampling density, perform an ordered analysis on the point cloud data, and obtain a set of key frames; Calculate the distance between the current key frame and other key frames in the key frame set, and determine the adjacent key frames whose distances are less than the preset distance threshold; Determine the key frame convex hull by combining with the adjacent key frames through a preset algorithm; Analyze the key frame convex hull through a preset algorithm to obtain the key frame concave hull; Construct a matching subgraph by combining the adjacent key frames, the key frame convex hull and the key frame concave hull.

4. The method according to claim 3, wherein Before the step of constructing a matching subgraph by combining the adjacent key frames, the key frame convex hull and the key frame concave hull, it further includes: Establish a spatio-temporal scoring mechanism for the feature points in the sampled key frames. The spatio-temporal scoring mechanism includes: Set the historical tracking window W of the feature points, and calculate the time dimension score St based on the tracking stability of the feature points within the window period, where St = (number of successfully tracked frames / W) × (1 + feature point position variance compensation term); Determine the descriptor cosine similarity between the feature points and the corresponding adjacent feature points, and determine the spatial dimension discrimination score Sd by combining the distance weights; Comprehensively calculate the final score S of the feature points as S = St × Sd × (1 + C), where C is the context reward factor for balancing the spatial distribution of the feature points; Calculate the point group quality measure Q = Σ(Si × Wi) based on the final score S of the feature points and the preset feature point spatial distribution weight Wi, where Wi is used to prevent the over-concentration of feature points; Determine a feature point pool of a set size according to the point group quality measure Q, and dynamically update the feature points in the key frame when the final score of the new feature points exceeds the lowest score in the feature point pool.

5. The method according to claim 1, characterized in that In the step of obtaining the point cloud data of the current scene by the 3D lidar, it specifically includes: Establish a multi-layer reflection intensity discrimination model, divide the point cloud obtained by the 3D lidar into three levels of high, medium and low according to the reflection intensity, and determine the feature extraction strategy for objects with different reflection characteristics; Process the echo signal of the laser pulse by using the wavefront reconstruction technology according to the feature extraction strategy, and extract the multi-echo data by analyzing the waveform characteristics of the echo signal; Dynamically adjust the scanning frequency and resolution of the 3D lidar according to the environmental complexity, the echo data and the AGV motion state.

6. A scene positioning system, characterized in that, The scene positioning system includes: one or more processors and a memory; the memory is coupled to the one or more processors, the memory is used to store computer program code, the computer program code includes computer instructions, and the one or more processors call the computer instructions to enable the scene positioning system to execute the method according to any one of claims 1-5.

7. A computer-readable storage medium, comprising instructions, characterized in that, When the instruction runs on the scene positioning system, it enables the scene positioning system to execute the method according to any one of claims 1-5.

8. A computer program product, characterized in that, When the computer program product runs on the scene positioning system, the scene positioning system is caused to execute the method according to any one of claims 1-5.

Images