Autonomous navigation method and system for overloaded agv based on multi-source fusion and map reconstruction

Abstract

The application discloses a heavy-load AGV autonomous navigation method and system based on multi-source fusion and map reconstruction, relates to the technical field of monitoring and analysis, and comprises the following steps: calculating the spatial distance difference value of the polar coordinate data of the remaining candidate targets in a high-reflective target set and the theoretical coordinate data, and marking the remaining candidate targets with a spatial distance difference value greater than a preset distance threshold as ghost candidate points; extracting the vertical bisector surface data of the connecting line of the corresponding points of the seed true value point set and the ghost candidate points, and comparing the spatial coincidence degree of the vertical bisector surface data and the environment contour point cloud data; if the spatial coincidence degree is greater than a preset coincidence threshold, the ghost candidate points are determined to be reflection ghosts and are removed from the high-reflective target set, and the points that are not removed continue to participate in the ghost removal cycle until the spatial distance difference values are all not greater than the preset distance threshold, and then a purified true value reflective column point set is output; and based on the purified true value reflective column point set, global pose data is calculated to perform autonomous navigation. The application has the effect of improving navigation efficiency.

Description

Technical Field

[0001] This application relates to the field of monitoring and analysis technology, and in particular to a method and system for autonomous navigation of heavy-duty AGVs based on multi-source fusion and map reconstruction. Background Technology

[0002] With the rapid evolution of modern industrial logistics, heavy-duty automated guided vehicles (AGVs) are increasingly used in warehousing and manufacturing environments, and autonomous navigation based on laser point cloud matching has become the mainstream positioning method.

[0003] However, in actual industrial settings, especially in complex work environments with dense metal shelves, stainless steel equipment, and smooth walls, traditional environmental perception technologies often face extremely severe challenges from optical multipath reflection interference. Existing positioning systems, when extracting reflective reference features, mostly rely too heavily on a single echo intensity threshold for blind data filtering. This static and superficial feature extraction mechanism struggles to distinguish between genuine, inherent reflective targets and optical ghosting derived from specular refraction at a physical level, leading to the system frequently absorbing falsified geometric feature data. The influx of a large amount of false observation information severely disrupts the topology of the local point cloud network, causing irreversible severe distortion and systematic coordinate drift in map matching and pose calculation processes. This makes it extremely easy for heavily loaded automated guided vehicles (AGVs) to lose their global reference base under full-load, high-speed operation, leading to dangerous route deviations, system deadlocks, or emergency shutdowns. It completely fails to balance long-term positioning robustness and absolute mapping accuracy under harsh industrial light fields. Summary of the Invention

[0004] To address the shortcomings of existing technologies, this application provides a method and system for autonomous navigation of heavy-duty AGVs based on multi-source fusion and map reconstruction.

[0005] Firstly, this application provides a method for autonomous navigation of heavy-duty AGVs based on multi-source fusion and map reconstruction, including the following steps: Prior pose data is generated by integrating odometer data from heavy-duty AGV chassis with inertial navigation data. Extract the set of highly reflective targets and their corresponding polar coordinate data from the current frame of LiDAR point cloud data, and call the global reflective column topology map data and environmental contour point cloud data; Based on prior pose data, the global reflector topology map data is projected onto the current radar coordinate system to generate a theoretical reference point set. The local observation topology map is constructed using polar coordinate data, and the local observation topology map is matched with the theoretical reference point set using graph isomorphism matching. Extract the actual angular velocity values ​​of successfully matched nodes in continuous frame polar coordinate data, and compare them with the theoretical angular velocity values. When the difference between the two is less than a preset angular velocity threshold, mark the corresponding node combination as a seed ground value point set. Trigger the ghost removal loop and calculate the theoretical coordinates of the global reflector topology map data in the current radar coordinate system based on the seed ground value point set; Calculate the spatial distance difference between the polar coordinate data and the theoretical coordinate data of the remaining candidate targets in the set of highly reflective targets, and mark the remaining candidate targets whose spatial distance difference is greater than a preset distance threshold as ghost candidate points; Extract the perpendicular bisector data of the line connecting the corresponding points of the seed ground value point set and the ghost candidate points, and compare the spatial overlap between the perpendicular bisector data and the environmental contour point cloud data. If the spatial overlap is greater than the preset overlap threshold, the ghost candidate point is determined to be a reflective ghost and is removed from the set of highly reflective targets. The unremoved points continue to participate in the ghost removal loop until the spatial distance difference is no greater than the preset distance threshold, at which point the purified true value reflective column point set is output. Autonomous navigation is performed based on global pose data obtained by purifying the true value reflective column point set. Vertical bisecting plane data with spatial overlap greater than a preset overlap threshold are converted into highly reflective solid surface data and updated to global reflective column topology map data to generate dynamic semantic map data.

[0006] Secondly, this application provides a heavy-duty AGV autonomous navigation system based on multi-source fusion and map reconstruction, including: The fusion module is used to fuse heavy-duty AGV chassis odometer data and inertial navigation data to generate prior pose data; The extraction module is used to extract the set of highly reflective targets and their corresponding polar coordinate data from the current frame of lidar point cloud data, and to call the global reflective column topology map data and environmental contour point cloud data; The module is used to project global reflector topology map data onto the current radar coordinate system based on prior pose data to generate a theoretical reference point set, and to construct a local observation topology map using polar coordinate data, and to perform graph isomorphism matching between the local observation topology map and the theoretical reference point set. The comparison module is used to extract the actual angular velocity values ​​of successfully matched nodes in continuous frame polar coordinate data and compare them with the theoretical angular velocity values. When the difference between the two is less than a preset angular velocity threshold, the corresponding node combination is marked as a seed ground value point set. The loop module is used to trigger the ghost removal loop and calculate the theoretical coordinate data of the global reflector topology map data in the current radar coordinate system based on the seed truth point set; The marking module is used to calculate the spatial distance difference between the polar coordinate data and the theoretical coordinate data of the remaining candidate targets in the set of highly reflective targets, and to mark the remaining candidate targets whose spatial distance difference is greater than a preset distance threshold as ghost candidate points. The calculation module is used to extract the perpendicular bisector data of the line connecting the corresponding points of the seed ground value point set and the ghost candidate points, and compare the spatial overlap between the perpendicular bisector data and the environmental contour point cloud data. The output module is used to determine that if the spatial overlap is greater than a preset overlap threshold, the ghost candidate point is a reflective ghost and is removed from the set of highly reflective targets. The unremoved points continue to participate in the ghost removal loop until the spatial distance difference is no greater than the preset distance threshold, at which point the purified true value reflective column point set is output. The update module is used to calculate the global pose data based on the purified true value reflective column point set to perform autonomous navigation, and convert the vertical bisecting plane data with spatial overlap greater than the preset overlap threshold into highly reflective solid surface data to update the global reflective column topology map data to generate dynamic semantic map data.

[0007] In summary, this application includes at least one of the following beneficial technical effects: 1. This application provides an autonomous navigation method for heavy-duty AGVs based on multi-source fusion and map reconstruction. By constructing a seed ground value point set and triggering a ghost removal loop, and fusing spatial distance difference judgment with spatial overlap comparison of vertical bisecting plane-environment contour point cloud, it can accurately identify and remove reflection ghosts generated by mirror refraction from the physical geometry level rather than the single intensity threshold level. This effectively avoids the pollution of local point cloud topology by false observation features and ensures that the reflective column point set participating in pose calculation is a high-confidence ground value. This greatly improves the perception reliability of heavy-duty AGVs in high-reflectivity scenarios such as metal shelves, stainless steel equipment, and smooth walls. 2. This application combines graph isomorphic matching, continuous frame angular velocity comparison, spatial distance residual verification, and perpendicular bisector coplanarity analysis to form a multi-level, closed-loop ghost removal mechanism. In each frame of LiDAR data, only points that simultaneously satisfy the constraints of consistent topology, consistent kinematic parameters, reasonable spatial geometric position, and coplanarity with the real environment contour can be retained. This mechanism fundamentally blocks the possibility of ghost features participating in global pose calculation, thereby avoiding the problems of drastic pose jumps, cumulative coordinate drift, and loss of global reference caused by ghost matching in traditional methods. This enables heavy-duty AGVs to maintain centimeter-level positioning accuracy even under full-load and high-speed operation. 3. This application introduces a forced exit mechanism. When the ghost removal loop exceeds the preset maximum number of loops and there are still abnormal candidate points, the system automatically blocks the pose calculation process of the current frame and rolls back to the prior pose data to perform navigation. This avoids system deadlock or emergency shutdown caused by abnormal observation in a single frame. At the same time, the successfully identified highly reflective solid surfaces are dynamically updated to the global reflective column topology map to generate map data with semantic information. This enables the map to be reconstructed and self-learned online according to the optical changes of the working environment, which significantly improves the adaptability of heavy-duty AGV to dynamic changes in lighting and reflection conditions during long-term continuous operation. Attached Figure Description

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

[0009] Figure 1 This is a flowchart of a method for autonomous navigation of a heavy-duty AGV based on multi-source fusion and map reconstruction, according to an embodiment of this application.

[0010] Figure 2 This is a schematic diagram of a heavy-duty AGV autonomous navigation system based on multi-source fusion and map reconstruction, according to an embodiment of this application. Detailed Implementation

[0011] The following description, in conjunction with the implementation of this invention, is merely an example and illustration of the concept of this invention. Those skilled in the art can make various modifications or additions to the specific embodiments described, or use similar methods to replace them, as long as they do not deviate from the inventive concept or exceed the scope defined in these claims, all of which should fall within the protection scope of this invention.

[0012] Example 1 This application discloses a method for autonomous navigation of heavy-duty AGVs based on multi-source fusion and map reconstruction.

[0013] Reference Figure 1 The autonomous navigation method for heavy-duty AGVs based on multi-source fusion and map reconstruction includes the following steps: Prior pose data is generated by integrating odometer data from heavy-duty AGV chassis with inertial navigation data. Extract the set of highly reflective targets and their corresponding polar coordinate data from the current frame of LiDAR point cloud data, and call the global reflective column topology map data and environmental contour point cloud data; Based on prior pose data, the global reflector topology map data is projected onto the current radar coordinate system to generate a theoretical reference point set. The local observation topology map is constructed using polar coordinate data, and the local observation topology map is matched with the theoretical reference point set using graph isomorphism matching. Extract the actual angular velocity values ​​of successfully matched nodes in continuous frame polar coordinate data, and compare them with the theoretical angular velocity values. When the difference between the two is less than a preset angular velocity threshold, mark the corresponding node combination as a seed ground value point set. Trigger the ghost removal loop and calculate the theoretical coordinates of the global reflector topology map data in the current radar coordinate system based on the seed ground value point set; Calculate the spatial distance difference between the polar coordinate data and the theoretical coordinate data of the remaining candidate targets in the set of highly reflective targets, and mark the remaining candidate targets whose spatial distance difference is greater than a preset distance threshold as ghost candidate points; Extract the perpendicular bisector data of the line connecting the corresponding points of the seed ground value point set and the ghost candidate points, and compare the spatial overlap between the perpendicular bisector data and the environmental contour point cloud data. If the spatial overlap is greater than the preset overlap threshold, the ghost candidate point is determined to be a reflective ghost and is removed from the set of highly reflective targets. The unremoved points continue to participate in the ghost removal loop until the spatial distance difference is no greater than the preset distance threshold, at which point the purified true value reflective column point set is output. Autonomous navigation is performed based on global pose data obtained by purifying the true value reflective column point set. Vertical bisecting plane data with spatial overlap greater than a preset overlap threshold are converted into highly reflective solid surface data and updated to global reflective column topology map data to generate dynamic semantic map data.

[0014] Specifically, the process of converting perpendicular bisector data into highly reflective solid surface data involves the system detecting when the spatial overlap of perpendicular bisectors generated by different observation frames or different pairs of reflective pillars in the global coordinate system continuously exceeds a preset overlap threshold. Statistically, this confirms that the plane is not a random noise point or a computational byproduct of single multipath reflection, but rather an objectively existing continuous specular reflector. In the specific conversion execution, the system extracts the core spatial parameters of this high-confidence perpendicular bisector, forcibly changes its attribute label from a virtual symmetric auxiliary surface derived from ghosting to a physical highly reflective solid surface in the underlying data structure, and assigns it corresponding spatial thickness and semantic blocking attributes. Finally, this instantiated and visualized solid surface feature matrix is ​​used as a new static environmental constraint node, fused and registered in the global reflective pillar topology map, thereby giving the map the ability to dynamically identify and actively suppress subsequent specular distortion ghosting semantic immunity.

[0015] In a specific embodiment, the prior pose data refers to the initial position and heading estimation information of the heavy-duty AGV in the global coordinate system obtained through multi-source sensor fusion. Specifically, it can be implemented by fusing chassis wheel speed odometer and inertial measurement unit data using an extended Kalman filter algorithm or an error state Kalman filter algorithm. This provides a high-frequency kinematic baseline for subsequent point cloud map projection. The highly reflective target set refers to the spatial discrete point set with significant optical reflection characteristics selected from the current frame of the LiDAR point cloud. This can be implemented by setting a dynamic echo intensity threshold and a spatial clustering algorithm to extract potential reflective pillars and other reflective features in the environment. Radial reflection interference sources and local observation topology maps refer to geometrical networks constructed using polar coordinate data that reflect the relative spatial arrangement of targets within the field of view. Specifically, this can be achieved by extracting the radial Euclidean distance and polar angle between adjacent targets as edge and node attributes, which is used to establish local feature constraints with translation and rotation scale invariance. Graph isomorphism matching refers to the mathematical process of comparing the structural equivalence of local observation features with theoretical map projection features. Specifically, this can be achieved by calculating the distance residual and angle residual matrices and generating a weighted topological similarity score, which is used to quickly locate the real map matching nodes without requiring precise initial pose.

[0016] Among them, theoretical coordinate data refers to the expected physical position of the global reflective column under the current radar field of view, which is derived from the back-inference of high-confidence nodes. Specifically, it can be realized by aligning the observation matrix and the global matrix with the singular value decomposition algorithm, solving the closed-form solution of rigid body transformation, and then reprojecting. It is used to provide a mathematical benchmark for identifying abnormal false targets outside the theoretical position. Perpendicular plane data refers to the hypothetical physical reflection plane reconstructed inversely according to the optical mirror reflection symmetry law. Specifically, it can be realized by extracting the midpoint of the three-dimensional space connecting the real reflective point and the ghost candidate point and constructing the plane equation with the normalized normal vector. It is used to determine the exact spatial position of the metal obstacle that causes multipath refraction of the laser beam. Spatial overlap refers to the evaluation index that quantifies the degree of fit between the hypothetical inversion plane and the surface of the objective environment point cloud. Specifically, it can be realized by calculating the area of ​​the two-dimensional projection bounding box of the coplanar point cloud cluster on the plane and the ratio with the preset reflective surface reference area. It is used to physically confirm or disprove the existence of the reflective mirror.

[0017] Among them, dynamic semantic map data refers to a new type of navigation data structure that overlays the verified high reflectivity environment contours onto the original topology map. Specifically, it can be achieved by persistently storing entity reflective surface attributes through coordinate system transformation and feature rewriting mechanisms, and is used to provide a dynamic scene reference benchmark with anti-multipath interference early warning capabilities for heavy-duty AGVs.

[0018] The working process and principle of this application are as follows: First, the odometer data of the heavy-duty AGV chassis and the inertial navigation data are fused to generate prior pose data. Then, the set of highly reflective targets and their corresponding polar coordinate data are extracted from the current frame of lidar point cloud data. Global reflective column topology map data and environmental contour point cloud data are called. Next, based on the prior pose data, the global reflective column topology map data is projected onto the current radar coordinate system to generate a theoretical reference point set. A local observation topology map is constructed using the polar coordinate data. The local observation topology map and the theoretical reference point set are graph isomorphic matched. The actual angular velocity values ​​of successfully matched nodes in continuous frame polar coordinate data are extracted and compared with the theoretical angular velocity values. When the difference between the two is less than a preset angular velocity threshold, the corresponding node combination is marked as a seed ground value point set. Then, a ghost removal loop is triggered. Based on the seed ground value point set, the theoretical coordinate data of the global reflective column topology map data in the current radar coordinate system is calculated. The empty coordinate data of the remaining candidate targets in the set of highly reflective targets and the theoretical coordinate data are calculated. The system calculates the spatial distance difference and marks the remaining candidate targets with spatial distance differences greater than a preset distance threshold as ghost candidate points. It further extracts the perpendicular bisector data of the lines connecting the corresponding points in the seed ground truth point set to the ghost candidate points, and compares the spatial overlap between the perpendicular bisector data and the environmental contour point cloud data. If the spatial overlap is greater than a preset overlap threshold, the ghost candidate point is determined to be a reflective ghost and is removed from the high-reflectivity target set. Unremoved points continue to participate in the ghost removal cycle until the spatial distance differences are all no greater than the preset distance threshold, at which point a purified ground truth reflective column point set is output. Finally, based on the purified ground truth reflective column point set, the system calculates the global pose data and performs autonomous navigation. The perpendicular bisector data with spatial overlap greater than the preset overlap threshold is converted into high-reflectivity entity surface data and updated to the global reflective column topology map data to generate dynamic semantic map data. In this way, physical-level dynamic removal of optical ghosts in highly reflective complex environments is achieved, ensuring that the high-precision pose calculation and autonomous navigation of the heavy-duty AGV always follow a safe and reliable route.

[0019] Furthermore, a local observation topology map is constructed using polar coordinate data, including: The radial distance and polar angle values ​​of each target within the set of highly reflective targets are extracted from the polar coordinate data; Arrange all targets in the highly reflective target set in the same direction according to the increasing polar angle values ​​to generate a target ring distribution sequence; A local subset is formed by extracting a predetermined number of consecutive targets from the target circular distribution sequence; Iterate through any two targets within the local subset and calculate the difference in their corresponding polar angle values; Extract the radial distance values ​​of the two targets, and use the cosine value of the difference between the polar angle values ​​to perform trigonometric geometric mapping to calculate the spatial straight line length between the two targets. Use the spatial straight line length as Euclidean distance data. The absolute value of the difference between the polar angles of two adjacent targets in the circular distribution sequence of the local subset is extracted and set as the relative included angle data; A local observation topology map is constructed based on Euclidean distance data and relative angle data.

[0020] In a specific embodiment, the specific implementation process of constructing a local observation topology map first performs data dimensionality reduction and feature stripping from the polar coordinate data, and accurately decomposes the radial distance value and polar angle value of each observation target in the set of highly reflective targets. The radial distance value represents the absolute straight-line distance from the geometric center point of the target to the physical origin of the lidar transmitter, and the polar angle value represents the counterclockwise rotation angle of the target relative to the zero-degree heading baseline of the radar chassis.

[0021] Next, in ascending order of polar angle values, all targets within the set of highly reflective targets are arranged in the same direction to generate a ring-shaped distribution sequence of targets that reflects the angular adjacency relationship of targets in real physical space.

[0022] Subsequently, a predetermined number of consecutive targets are extracted from the circular distribution sequence of the targets to form a local subset. The predetermined number is set within a dynamic integer range of 3 to 5. Since fewer than 3 targets cannot form a closed polygonal topological mesh in the two-dimensional plane, the system lacks geometric rigidity constraints. More than 5 targets will geometrically increase the computational overhead of the subsequent graph isomorphism matching matrix and introduce far-end occlusion interference. In order to balance feature uniqueness and environmental deformation tolerance, this embodiment preferably sets the predetermined number to 4, that is, each time it is scrolled, 4 adjacent reflective targets are extracted to form a quadrilateral local subset to perform topology verification.

[0023] For the extracted local subset, the system iterates through any two targets within it and calculates the difference between their corresponding polar angle values. The calculation formula is as follows:

[0024] In the above formula, This represents the spatial straight-line length between target i and target j within a local subset. The system directly outputs this as the Euclidean distance data for subsequent topology graph construction. and These are the radial distance values ​​between target i and target j, respectively. Let be the absolute value of the difference between the polar angles of target i and target j; the trigonometric function terms in the formula. is the pure geometric projection scaling factor, and is a dimensionless parameter.

[0025] Simultaneously, the system extracts the difference in polar angle values ​​between two adjacent targets within the local subset's circular distribution sequence. The calculation formula is as follows: . in the formula This refers to the relative angle data between adjacent target m and target m+1; This represents the polar angle value for a single target. Represents the complete physical scan circumference constant; The minimum value selection operator is used. The advantage of this calculation is that it forces the observed angles to converge to the effective range of minor angles from zero to a straight angle, ensuring the geometric authenticity of the interior angle data of the polygonal topology and effectively avoiding false large-angle distortion misjudgments caused by zero-point data truncation. Finally, the system maps the obtained Euclidean distance data to the rigid weight attribute of the topology graph's side lengths and the relative angle data to the node connection constraint attribute of the topology graph, thus successfully constructing a local observation topology graph that is invariant to both translation and rotation scales.

[0026] Furthermore, graph isomorphic matching is performed between the local observation topology graph and the theoretical reference point set, including: Calculate the theoretical reference distance characteristics and theoretical reference angle characteristics between each reference point within the theoretical reference point set; The distance residual matrix is ​​obtained by subtracting the Euclidean distance data from the theoretical reference distance features, and the angle residual matrix is ​​obtained by subtracting the relative angle data from the theoretical reference angle features. Assign fusion weight ratios to distance residual matrix and angle residual matrix, and generate topological similarity score by weighted fusion; When the topological similarity score is greater than the preset similarity threshold, the local observed topological graph is determined to be a successful isomorphic match with the theoretical reference point set graph.

[0027] In a specific embodiment, the specific implementation process of graph isomorphic matching between the local observation topology map and the theoretical reference point set firstly extracts the geometric arrangement information of each reference point in the theoretical reference point set in the current radar coordinate system based on the prior projection relationship. Then, by calculating the spatial straight-line mapping distance between any two reference points and the polar angle difference between the two reference points relative to the origin, theoretical reference distance features and theoretical reference angle features are generated.

[0028] For the constructed local observation topology map, the system performs a bit-by-bit deviation comparison between the actual observation attributes it carries and the aforementioned theoretical reference attributes. Specifically, the system performs absolute value subtraction on the Euclidean distance data in the edge attributes of the local observation topology map and the corresponding theoretical reference distance features to generate a distance residual matrix representing spatial scale deviation; simultaneously, it performs absolute value subtraction on the relative angle data in the node attributes of the topology map and the corresponding theoretical reference angle features to generate an angle residual matrix representing spatial rotation and deformation deviation.

[0029] Based on the environmental features of the current scene, the fusion weight ratio of the distance residual matrix and the angle residual matrix is ​​assigned, and the final topological similarity score is calculated and generated. The specific formula is as follows:

[0030] In the above formula, The output topological similarity score has a value that strictly converges to between 0 and 1. These are the specific element values ​​in the distance residual matrix; This is the tolerance parameter for the range measurement error of the radar hardware, which is calibrated according to the radar manual. These are the specific element values ​​in the angular residual matrix; This refers to the tolerance parameter for angle errors measured by radar hardware. The proportion of fusion weights assigned to the distance residual matrix. The fusion weights assigned to the angular residual matrix are both dimensionless coefficients that satisfy the constraint that they add up to 1. The advantage of using this formula is that it not only successfully removes the physical dimensions of meters and radians by using the error term as the denominator of the exponent, thus nonlinearly mapping the absolute error to a unified dimensionless confidence probability space, but also applies the bell-shaped decay characteristic of the Gaussian function to impose a very strong numerical penalty on large-deviation noise points, greatly improving the robustness of removing forged structural features.

[0031] Regarding the logic for setting specific parameters, the fusion weight ratio... Set to an adjustable range of 0.5 to 0.7. The corresponding adjustable range is set to 0.3 to 0.5. To reduce the negative interference of slight angular distortion caused by sharp turns of heavy-duty vehicles on the overall matching, while ensuring sensitive capture of rigid distance deformation, the distance fusion weight ratio is preferably set. The angle fusion weight ratio is 0.6. The threshold is 0.4. Meanwhile, the preset similarity threshold for determining whether a match is successful is recommended to be set within an adjustable range of 0.75 to 0.90. To avoid the target centroid shifting due to local light spot diffusion in highly reflective environments, which could lead to the false rejection of correct matches, and to ensure that ghost topological groups with similar but not completely identical geometric distributions can be strictly filtered out, a threshold of 0.85 is preferred as the judgment benchmark threshold.

[0032] Specifically, the values ​​of the tolerance parameters for radar hardware distance measurement error and radar hardware angle measurement error are fundamentally derived from the official manufacturer's technical specifications of the mounted lidar equipment, combined with the actual electrostatic calibration and motion measurement results of the sensor under specific industrial environments. Physically, these two tolerance parameters characterize the inherent statistical random white noise standard deviation and hardware resolution limit of radar ranging accuracy and angular resolution, respectively. This tolerance limit, set based on the actual capabilities of the hardware, can reasonably absorb normal ranging and angle measurement fluctuations caused by high-frequency mechanical vibration or light spot divergence during heavy-duty AGV travel, preventing the algorithm from frequently rejecting true dynamic observation matches due to overly stringent evaluation. It also ensures a precipitous numerical penalty for high-reflectivity false ghost distortions exceeding the inherent physical error range of the equipment, thus laying an objective and solid physical hardware reliability foundation for the probabilistic calculation of topological similarity.

[0033] The fusion weight ratio assigned to the distance residual matrix is ​​set to 0.6, while the fusion weight ratio assigned to the angle residual matrix is ​​set to 0.4. This decision is an optimization engineering decision made after a deep consideration of the kinematic transient distortion characteristics of heavy-duty vehicles and the rigid topological invariance of the local environment. In actual logistics handling conditions, when a heavy-duty AGV accelerates or makes a sharp turn under full load, the chassis mechanical suspension inevitably produces slight roll, pitch, or yaw. In addition, the mechanical LiDAR has a microsecond-level time difference effect when scanning a frame of point cloud. This motion distortion will directly cause transient geometric tangential distortion of the relative polar angle data in the local point cloud network. Therefore, the confidence of angle features in dynamic matching is relatively fragile. Conversely, the absolute Euclidean distance between any two fixed reflective pillars in space, as an absolute invariant in the three-dimensional metric space, is naturally immune to translation and rotation errors of the coordinate system and exhibits extremely high physical constancy and scale rigidity within continuous scan slices. Therefore, by dominantly increasing the trust weight of the distance feature to 0.6, the system can be ensured to always firmly grasp the core rigid skeleton of the environmental topology network. Meanwhile, the angle feature, as an auxiliary verification dimension to break the symmetry ambiguity, is reduced to 0.4. This not only retains the screening constraint on similar topological structures under complex multipath reflection, but also filters out nonlinear angle noise induced by complex kinematic disturbances of the vehicle body in advance, achieving the optimal numerical balance between the algorithm's anti-skid robustness and strict spatial matching.

[0034] Furthermore, the process of obtaining the theoretical angular velocity value includes: Extract the chassis linear velocity and chassis yaw rate values ​​of the heavy-duty AGV at the current moment from the prior pose data; Extract the radial distance and polar angle values ​​corresponding to the successfully matched nodes; The chassis linear velocity value is decomposed into the normal direction corresponding to the polar angle value to obtain the normal linear velocity component. The theoretical linear velocity value is obtained by rigid body superposition calculation of the normal linear velocity component combined with the chassis yaw rate value. The theoretical linear velocity value is obtained by dividing the radial distance value.

[0035] In a specific embodiment, the specific implementation process of obtaining the theoretical angular velocity value is first established on a multi-source sensor data fusion link. The system accurately extracts the chassis linear velocity value and chassis yaw angular velocity value of the heavy-duty AGV at the current moment from the prior pose data. The chassis linear velocity value represents the instantaneous translational rate of the vehicle in the reference plane coordinate system, and the chassis yaw angular velocity value represents the instantaneous rotational rate of the vehicle about the vertical chassis central axis. Then, for the effective nodes that have completed graph isomorphic matching in the local observation topology graph, the system simultaneously extracts the radial distance value and polar angle value corresponding to the successfully matched node. In order to establish a dynamic observation benchmark model based on sensor movement, the system performs a two-dimensional dimensionality reduction variation of the classic three-dimensional rigid body relative motion vector cross product theorem, decomposes the chassis linear velocity value to obtain the normal linear velocity component corresponding to the polar angle value, and combines the chassis yaw angular velocity value to perform rigid body kinematic superposition calculation on the normal linear velocity component to obtain the theoretical linear velocity value. The variation calculation formula of this process is as follows: In this formula, The theoretical linear velocity value for the calculated output represents the relative tangential linear velocity of the target in the direction perpendicular to the radar line of sight. This is the input value for the chassis linear velocity; This is the input polar angle value; It is a pure geometric trigonometric function mapping operator, and it is a dimensionless parameter; This is the input radial distance value; This represents the chassis yaw rate. The preset dynamic slip compensation coefficient is a dimensionless parameter. The advantage of using this two-dimensional orthogonal decomposition variant formula is that it completely avoids the problems of the traditional three-dimensional homogeneous transformation matrix consuming a lot of computing power on the industrial control computer and easily causing floating-point overflow. It enables the superposition of the relative motion of the target caused by its own translation and rotation to be solved directly with high frequency and low latency through scalar algebra. At the same time, the preset dynamic slip compensation coefficient is set to an adjustable range of 0.95 to 1.05. In order to avoid the distortion of the physical transmission of linear velocity caused by the micro-deformation of polyurethane solid tires when the heavy-duty AGV makes a sharp turn under full load and high inertia, and to ensure that the kinematic solution always has rigid geometric constraints, 1.02 is preferred as the dynamic slip compensation coefficient. After performing the algebraic superposition of linear velocities, the system divides the calculated theoretical linear velocity value by the radial distance of the corresponding node to obtain the theoretical angular velocity value. The calculation formula is as follows: ,in The final output theoretical angular velocity value represents the angular drift rate that the target should produce in the radar field of view under the current motion state. This accurately output theoretical angular velocity value serves as the sole truth benchmark for the temporal motion state and will be directly transmitted to the downstream computing node. It will be rigorously compared with the actual angular velocity value of the target extracted by radar scanning using a residual threshold. This leverages the non-replicable nature of rigid body motion in the real physical world to forcefully peel off and eliminate the positional illusions fabricated by multipath reflection ghosts in static geometric space from the time dynamic dimension.

[0036] Furthermore, based on the seed ground value point set, the theoretical coordinate data of the global reflector topology map data in the current radar coordinate system is calculated, including: Extract the observation coordinate matrix of the seed ground value point set in the current radar coordinate system; Extract the global coordinate matrix of the corresponding seed ground value point in the global coordinate system from the global reflector topology map data; Alignment calculations are performed on the observation coordinate matrix and the global coordinate matrix to solve for the translation vector and rotation matrix between the radar coordinate system and the global coordinate system. Extract the absolute coordinates of all reflector nodes within the global reflector topology map data, and then use translation vectors and rotation matrices to perform coordinate system transformation and reprojection on the absolute coordinates to obtain the theoretical coordinates.

[0037] In a specific embodiment, the process of calculating the theoretical coordinate data of the global reflector topology map data in the current radar coordinate system based on the seed ground truth point set firstly involves extracting the confirmed seed ground truth point set based on high-confidence local geometric matching results. This involves obtaining the absolute coordinates of these nodes in the current radar polar coordinate system scan and transforming them to the radar's two-dimensional Cartesian coordinate system. These coordinates are then arranged as column vectors to construct the observation coordinate matrix. Each column of this matrix represents the x and y coordinates of a reflective target. Simultaneously, based on the node mapping relationship of topology matching, the system extracts the map prior coordinates of the corresponding seed ground truth points in the global absolute coordinate system from the global reflective column topology map data, and constructs the global coordinate matrix. The dimensions of this matrix are exactly the same as those of the observation coordinate matrix.

[0038] Next, the system performs spatial rigid body alignment calculations on the observed coordinate matrix and the global coordinate matrix, solving for the translation vector and rotation matrix between the radar coordinate system and the global coordinate system. The system first calculates the geometric centroid coordinates of the two sets of matrices respectively. and All dimensions are meters; then the original matrix is ​​subtracted by the corresponding centroid coordinates to obtain the decentralized observation residual matrix. With the global residual matrix Then, the cross-covariance matrix of the two is calculated using the following formula: For the covariance matrix Perform singular value decomposition, i.e. ,in, and It is an orthogonal matrix. This is a singular value diagonal matrix. Using the orthogonal matrix obtained from the decomposition, the system calculates the rotation matrix between the radar coordinate system and the global coordinate system, as shown in the formula: In this formula, The output rotation matrix is ​​composed of the product of orthogonal bases of pure direction vectors. This is a dimensionless, purely numerical matrix. To prevent point set degradation from generating incorrect mirror reflection matrices, the system introduces a preset orthogonality penalty tolerance here. Perform a validity check, that is, determine the determinant. Is it greater than The preset orthogonal penalty tolerance is set to... to The adjustable range is optimized to avoid misjudging normal rigid body rotation caused by the truncation of floating-point precision at the industrial control computer's underlying level, while ensuring strict interception of image distortion. This serves as the tolerance benchmark. After the verification passes, the system calculates the translation vector using the rotation matrix. The calculation formula is as follows: In this formula, The output translation vector represents the absolute translation of the radar physical origin in the global coordinate system.

[0039] Finally, the system extracts the absolute coordinates of all reflector nodes (including distant nodes not captured by the current radar field of view) within the global reflector topology map data and integrates them into a global point set matrix. Using the translation vector and rotation matrix calculated above, the system performs a full coordinate system transformation and reprojection on the absolute coordinate data. The mapping formula is as follows: In this projection formula, The final output of theoretical coordinate data represents the theoretical two-dimensional physical coordinates of all real reflective pillars that should appear in the current radar field of view based on the current high-confidence pose. This result accurately locks the theoretical expected position of the far-end real reflective pillars in the current radar coordinate system, providing an extremely solid and insurmountable mathematical judgment benchmark for the subsequent efficient delineation and elimination of multipath reflection ghosts that are outside the theoretical position using spatial distance residuals.

[0040] Furthermore, the perpendicular bisector data of the line connecting the corresponding points in the seed ground truth point set and the ghost candidate points is extracted, including: Convert the corresponding points in the seed truth point set and the ghost candidate points into Cartesian 3D space coordinates; Calculate the midpoint coordinate vector in 3D space, and calculate the pointing vector between the corresponding point and the ghost candidate point as the normal vector; The three-dimensional plane equation is constructed based on the midpoint coordinate vector and the normal vector, and the set of coefficients of the three-dimensional plane equation is defined as the perpendicular bisector data.

[0041] In a specific embodiment, the specific implementation process for extracting the perpendicular bisector data of the line connecting the corresponding points of the seed ground truth point set and the ghost candidate points involves the system first retrieving the corresponding points of the real reflective pillars within the pre-confirmed seed ground truth point set and the ghost candidate points whose spatial distance difference exceeds the limit. The underlying polar coordinate data or two-dimensional Cartesian coordinate data of these two sets are then combined with a preset radar installation height parameter and mapped upwards to Cartesian three-dimensional spatial coordinates under a unified benchmark. The preset radar installation height parameter represents the vertical offset distance of the laser radar's emitting optical center relative to the heavy-duty AGV's reference chassis, and is set to an adjustable range of 0.1 meters to 1.5 meters. To accurately match the obstacle avoidance scanning blind zone at the bottom of the heavy-duty AGV forks and to consider the reflective characteristics of the rack beams, a setting of 0.5 meters is preferred. This operation generates the three-dimensional coordinates of the seed ground truth points. 3D coordinates of ghost candidate points .

[0042] First, calculate the midpoint coordinate vector. And the normalized normal vector from the real point to the ghost point The calculation formula is as follows: as well as In the formula, The output is the midpoint coordinate vector, representing the exact physical penetration point of the unknown reflecting surface on the line connecting the two points; It contains three spatial direction components. This represents the absolute orientation normal vector of the perpendicular bisector in three-dimensional space. Since this vector is obtained by dividing the coordinate difference vector by its own Euclidean norm 2, i.e., the absolute distance scalar, the meter dimension of the length dimension is completely canceled out in the division. It is a pure directional representation and belongs to the dimensionless vector category; The L2 norm operator effectively decouples the nonlinear relationship between the orientation of the reflecting surface and the distance to the reflecting point, enabling the coefficients of the subsequently generated plane equations to have a unified mathematical scale. This greatly reduces the matrix condition number when fitting the subsequent algorithm to the environmental contour point cloud, effectively avoiding the ill-conditioned divergence of the calculation matrix and floating-point overflow caused by the excessive distance of highly reflective targets.

[0043] The system then constructs a classic point-normal form three-dimensional plane equation based on the midpoint coordinate vector and the normal vector, and solves for the intercept constraint parameters of the plane through inner product operations. The calculation formula is: In this formula, its physical meaning represents the absolute distance of the orthogonal projection from the origin of the spatial coordinate system to the perpendicular bisector of the plane.

[0044] Finally, the system defines the set of coefficients of the extracted three-dimensional plane equations as perpendicular bisector data, and encapsulates it in one-dimensional matrix form for output. The vertical bisecting plane data will serve as the core physical constraint benchmark and will be directly input into the downstream spatial overlap comparison module. This module will be used to deterministically find the matching metal shelf surface that produces reflection in the massive environmental contour point cloud. This achieves a lossless reverse derivation closed loop from surface anomaly point data to the essential physical interference surface of the environment, completely eliminating the hidden danger of blindly guessing ghosting through black box models.

[0045] Furthermore, the spatial overlap between the vertical bisecting plane data and the environmental contour point cloud data is compared, including: Calculate the orthogonal projection distance from each point cloud data in the environmental contour point cloud data to the corresponding 3D plane of the perpendicular bisector plane data; Point cloud data with orthogonal projection distances less than a preset thickness threshold are selected to form a coplanar point cloud cluster set; Calculate the area of ​​the two-dimensional projected bounding box of the coplanar point cloud cluster set on the three-dimensional plane; The ratio of the area of ​​the two-dimensional projection bounding box to the preset reflective surface reference area is calculated, and the calculated percentage value is used as the spatial overlap.

[0046] In a specific embodiment, the process of comparing the spatial overlap between the vertical bisecting plane data and the environmental contour point cloud data involves the system first simultaneously extracting a frame of panoramic environmental contour point cloud data and the vertical bisecting plane data matrix output from the previous processing stage. This vertical bisecting plane data matrix represents the set of coefficients of a three-dimensional plane equation, containing the dimensionless normalized normal vector of the plane and the meter-dimensional intercept constraint parameters. To measure the degree of fit between the actual environmental object boundary and the projected plane, the system calculates the orthogonal projection distance from the coordinates of each three-dimensional discrete point in the environmental contour point cloud data to the corresponding three-dimensional plane in the vertical bisecting plane data. The calculation formula for this mapping process is: In the above formula, The output orthogonal projection distance represents the absolute length of the perpendicular projection of the i-th discrete point in the point cloud onto the reflection plane. and The three-dimensional spatial relative coordinate components of the environmental point cloud are all in meters. These are the three spatial axial components of the normalized normal vector, which are purely represented by direction cosines and are dimensionless parameters. Let be the intercept constraint parameter of the plane, with the dimension in meters. The advantage of using this absolute value linear algebra variant formula is that, since the normal vector has already been strictly normalized to a unit length in the previous step, the square root operation of the sum of squares in the denominator of the traditional point-to-plane distance formula is directly omitted here. This greatly reduces the computing power consumption of industrial motherboards when batch processing tens of thousands of point cloud data while maintaining mathematical equivalence.

[0047] Next, the system isolates the base surface with potential reflective properties based on a numerical truncation mechanism, that is, it selects point cloud data combinations with orthogonal projection distances less than a preset thickness threshold to form a coplanar point cloud cluster. The preset thickness threshold is set to an adjustable physical range of 0.05 meters to 0.20 meters. This is to accommodate the beam divergence angle polarization error of the lidar during long-distance scanning and the microscopic metal deformation of the surface of the heavy-duty shelf uprights under heavy pressure, while ensuring strict filtering out interference from stacked independent goods packaging boxes close to the front of the shelf. The preferred preset thickness threshold is 0.08 meters.

[0048] Specifically, the optimal setting of the preset thickness threshold to 0.08 meters is based on the best engineering decision derived from a deep consideration of the physical characteristics of the LiDAR hardware and the complex interference factors of the real industrial warehousing environment. In terms of tolerance, a thickness tolerance of 0.08 meters effectively accommodates and absorbs the beam divergence polarization error that inevitably occurs when the LiDAR scans at long distances. It also reasonably accommodates the microscopic physical deformation of the metal columns of heavy-duty shelves caused by long-term pressure from heavy goods, ensuring that point clouds that truly belong to the reflective base surface are not mistakenly rejected due to an overly stringent threshold. In terms of filtering exclusivity, strictly constraining the threshold to within 0.08 meters is to define a clear physical isolation boundary in three-dimensional space, thereby accurately and strictly filtering out interference from non-coplanar protrusions such as independent cargo boxes temporarily placed or stacked in front of the metal shelves. This achieves the best numerical balance between extracting the true specular reflection contour and stripping away complex background noise.

[0049] Subsequently, to avoid the misjudgment of sparse distant point clouds caused by lidar distance attenuation when using the traditional point counting method to assess area, the system calculates the area of ​​the two-dimensional projected bounding box on the three-dimensional plane for the extracted coplanar point cloud clusters. This process first constructs any pair of mutually orthogonal unit basis vectors on the perpendicular bisector plane. and Both vectors are dimensionless three-dimensional directional bases. The system maps each point in the coplanar point cloud cluster to this basis coordinate system using vector dot product, and the formula for calculating the locally orthogonal expansion coordinate variation is: as well as By utilizing this coplanar dimensionality reduction expansion mechanism, the system can capture the extreme value extension range of feature clusters, and then, through the formula... Calculate the area of ​​the two-dimensional projected bounding box. In this formula, and These represent the positive and negative extreme projections of the point cloud cluster along the direction of the first orthogonal basis, respectively, with units of meters; and Similarly, this represents the extreme projection along the direction of the second orthogonal basis, with the dimension of meters; The area of ​​the two-dimensional projected bounding box is used to calculate the output area, which represents the physical cross-sectional size of the metal reflective surface in three-dimensional space, with the dimension of square meters.

[0050] Finally, the system directly calculates the ratio between the area of ​​the two-dimensional projection bounding box and the preset reflective surface reference area, using the following formula: In this formula, The preset reflective surface reference area is set to an adjustable continuous range of 1.0 square meters to 4.0 square meters. Based on the average physical size of the metal reflective fence on the side of the standardized three-dimensional warehouse rack where heavy-duty AGVs are stationed and operating, it is preferably set to 2.5 square meters here. The spatial overlap of the final output is a percentage probability coefficient.

[0051] Furthermore, the ghost removal loop includes a forced exit mechanism: Initialize the execution count data when the ghost removal loop is triggered; Each time the ghost removal loop is re-executed, the execution count is accumulated. The loop will be forcibly terminated when the number of executions reaches the preset maximum number of loops and there are remaining candidate targets in the set of highly reflective targets whose spatial distance difference is greater than the preset distance threshold. The pose calculation process of the current frame of LiDAR point cloud data is blocked, and the prior pose data is directly used as the global pose data to perform autonomous navigation.

[0052] In one specific embodiment, the triggering of the ghost removal loop includes a forced exit mechanism. The system immediately initializes the execution count data in memory and safely sets it to zero upon triggering the ghost removal loop. This execution count data is a pure scalar representing the iteration depth within the program, and is a dimensionless integer. Then, each time the matching traversal of the environment point cloud and the imaginary plane, and the ghost removal loop, are re-executed, the system performs an increment calculation of this execution count data with a step size of one.

[0053] As the loop continues to explore, the system triggers a conditional branch judgment at the end of each iteration. This involves checking whether the current execution count has reached the preset maximum number of iterations, and simultaneously checking whether there are any remaining candidate targets in the set of highly reflective targets whose spatial distance difference is still greater than a preset distance threshold after the current round of cleaning. The preset maximum number of iterations is set to an adjustable range of 3 to 8 times. To avoid the algorithm getting stuck in extreme multipath reflection traps surrounded by stainless steel equipment, thus exhausting the system's underlying computing resources, and to ensure that most normal physical reflective surface structures can be traversed and verified in regular channels, a setting of 5 times is preferred. The preset distance threshold for synchronous matching is set to an adjustable physical range of 0.10 meters to 0.40 meters. To accommodate the vibration observation noise of the lidar chassis caused by the heavy-duty AGV rolling over uneven ground while carrying several tons of goods, and to strictly and sensitively identify false optical ghosts that undergo obvious nonlinear jumps in physical space, a setting of 0.25 meters is preferred. When the number of executions accumulates to the preferred threshold of 5 times, and there are still a large number of targets in the computational network with spatial distance differences exceeding 0.25 meters that cannot be closed in a loop, the system determines that the current frame point cloud environment has deteriorated to the point where the three-dimensional topology structure is severely ineffective. It immediately triggers a global blocking signal at the underlying instruction level to force the system to break out of the unsolvable high-intensity computation loop.

[0054] After exiting the loop and interrupting the conventional pose calculation process of the current frame's LiDAR point cloud data, to avoid the risk of inertial slippage of heavy-duty cargo due to sudden stopping caused by loss of positioning during vehicle operation, the system forcibly takes over the underlying closed-loop controller, directly using the prior pose data generated through odometer pre-simulation as the core reference. Considering that multiple rounds of ghost removal loops consume a significant amount of system clock cycles, directly applying the outdated state before entering the loop would cause fatal physical spatial following deviations in the heavy-duty AGV during high-speed travel. Therefore, the system creatively introduces a delay-compensated pose estimation variant operator based on the mean integral of chassis kinematics. The system first obtains the absolute time difference accumulated from the current frame's radar hardware trigger to the current forced exit time, output by the high-precision timer of the system motherboard, as the calculation time difference value, with the dimension in seconds. Then, based on the pre-stored prior coordinate system data, the variant operator is used to generate the final takeover-level global pose data. The specific two-dimensional coordinate estimation formula is as follows:

[0055] as well as The formula for synchronous heading angle compensation is: In this formula, and These represent the horizontal and vertical coordinates of the global pose data output for autonomous navigation after compensation. The absolute heading angle representing the compensation output; and This serves as the reference horizontal and vertical coordinates of the previously generated prior pose data in the global coordinate system. This is the corresponding prior heading angle reference; The system collects the average linear velocity of the chassis in real time within this extremely short interval using the chassis wheel speed gauge; The chassis yaw rate is synchronously acquired via the chassis gyroscope; The calculation time difference is precisely extracted; the constant 0.5 is the integral median smoothing factor, a dimensionless pure number. The necessity and benefit of using this variant operator lies in the fact that by explicitly introducing a dynamic smoothing term of angular velocity with half a delay period during the integral forward propagation accumulation process, it perfectly fits the smooth arc-shaped turning trajectory of the vehicle within milliseconds, greatly reducing the steering wheel truncation divergence error caused by pure linear tangent extrapolation. This provides extremely robust blind push takeover protection under extreme conditions of forced circuit failure due to computing power overload. After the newly calculated global pose data is seamlessly sent to the underlying controller, the heavy-duty AGV is driven to steadily leave this deep water of optical noise without any bumps, thus forming an engineering-grade industrial navigation protection data flow graph with extremely high survivability.

[0056] Specifically, the fundamental basis for choosing a constant of 0.5 as the integral median smoothing factor lies in its ability to explicitly introduce a dynamic smoothing term for the angular velocity over half a delay period, perfectly fitting the smooth, arc-shaped turning trajectory of a heavy-duty automated guided vehicle within extremely short millisecond intervals during the integral forward propagation accumulation process. Since the actual physical motion of the vehicle during turning exhibits curvilinear characteristics, relying solely on a single moment's state for purely linear tangent extrapolation will inevitably result in divergent cumulative errors due to the steering wheel truncation effect. Setting it to 0.5 essentially extracts the equivalent center value of the vehicle's heading change within that delay estimation time, thereby greatly mitigating the trajectory distortion caused by purely linear extrapolation. This ensures that even under extreme conditions where the navigation control system is forced to shut down due to computing power overload, it can still provide extremely robust blind takeover protection that conforms to the actual physical kinematics of the vehicle.

[0057] Furthermore, extract the set of highly reflective targets from the current frame of lidar point cloud data, including: Obtain the initial echo intensity data and initial distance data of each original point in the current frame of LiDAR point cloud data; By combining the preset laser emission power benchmark value, the initial distance data is used to derive and calculate the dynamic intensity threshold; Filter out the original points whose initial echo intensity data is less than the dynamic intensity threshold, and then pack the remaining original points into a set of highly reflective targets using a distance clustering algorithm.

[0058] In a specific embodiment, the process of extracting the set of highly reflective targets from the current frame of lidar point cloud data involves the system first acquiring the initial echo intensity data and initial distance data of each discrete scanning origin point within the current frame of lidar point cloud data. The initial echo intensity data represents the dimensionless digital quantized intensity value received and converted by the radar photodiode, and the initial distance data represents the absolute straight-line distance from the origin point to the radar's physical emission origin. Next, the system reads the preset laser emission power reference value from the current radar device's hardware register via the underlying communication bus and, combined with the basic optical radar equations, performs physical derivation calculations on the initial distance data to generate a dynamic intensity threshold. To address the engineering defect of the traditional inverse square law formula, where the denominator approaches zero at extremely close ranges, leading to an infinitely large threshold divergence, the system designs a variant formula for attenuation derivation to resist near-field blind zones.

[0059] In this variant formula, The output dynamic intensity threshold; The extracted preset laser emission power reference value represents the constant electrical power when the laser emits light; This is the initial distance data to be input; The preset photoelectric reflection gain composite coefficient represents the inherent hardware conversion rate that maps optical energy to digital intensity. A near-field blind zone compensation constant is preset. The advantage of using this variant formula that introduces a near-field compensation term is that it effectively smooths the surge in near-field reflection intensity distortion when a heavy-duty AGV travels close to the edge of a narrow passage, completely eliminating the problem of missing key near-field topological nodes due to a sharp increase in the threshold, and ensuring the global robustness of high-reflectivity feature extraction. Regarding the setting logic of specific parameters, the preset photoelectric reflection gain composite coefficient is set to an adjustable range of 100 to 500. To accurately match the high-gain physical properties of standard-grade microprism reflective film in industrial settings, a setting of 325 square meters per watt is preferred. The preset near-field blind zone compensation constant is set to an adjustable continuous range of 0.1 to 1.0. To be compatible with the physical visual blind spot dead zone of about 0.5 meters around the radar base, a setting of 0.25 square meters is preferred.

[0060] The system then traverses thousands of raw points within a frame, comparing their dimensionless initial echo intensity data with their corresponding dynamic intensity thresholds. Ordinary diffuse reflection points with initial echo intensity data less than the dynamic intensity threshold are directly filtered out, leaving the remaining raw points intact. Next, the system processes these remaining raw points using a spatial Euclidean distance clustering algorithm. This involves calculating the absolute three-dimensional Euclidean distance between any two remaining raw points in space. If the spatial distance is less than a preset cluster radius, the points are determined to belong to the same continuous reflective surface and grouped into the same high-reflectivity cluster. After the traversal is complete, the polar coordinates of the geometric center of each cluster are extracted to generate a set of high-reflectivity targets. The preset clustering radius is set to a physical ranging range of 0.05 meters to 0.15 meters. To avoid the dense light spot generated by the same cylindrical reflective column being hit by the radar high-frequency pulse being mistakenly split into multiple isolated structural nodes, it is preferably set to 0.10 meters. This set of highly reflective targets, which has been refined by the underlying physical dimensionality reduction and has extremely high reflectivity confidence, will be directly output and smoothly transferred to the downstream topology calculation stage. As the core geometric material, it participates in the rigid construction of the relative distance and ring angle attributes of the polygons inside the local observation topology map, completely cutting off the ineffective consumption of the upper computer's computing power by irrelevant environmental noise from the source.

[0061] Furthermore, navigation adjustment logic is executed based on dynamic semantic map data, including: Extract the set of hazardous coordinate intervals covered by highly reflective entity surface data from dynamic semantic map data; When calculating global pose data and determining that the current position is within the dangerous coordinate interval set, the preset distance threshold of the corresponding area point cloud data is increased, and the preset overlap threshold is decreased.

[0062] Example 2 This application also discloses a heavy-duty AGV autonomous navigation system based on multi-source fusion and map reconstruction.

[0063] Reference Figure 2 A heavy-duty AGV autonomous navigation system based on multi-source fusion and map reconstruction includes: The fusion module is used to fuse heavy-duty AGV chassis odometer data and inertial navigation data to generate prior pose data; The extraction module is used to extract the set of highly reflective targets and their corresponding polar coordinate data from the current frame of lidar point cloud data, and to call the global reflective column topology map data and environmental contour point cloud data; The module is used to project global reflector topology map data onto the current radar coordinate system based on prior pose data to generate a theoretical reference point set, and to construct a local observation topology map using polar coordinate data, and to perform graph isomorphism matching between the local observation topology map and the theoretical reference point set. The comparison module is used to extract the actual angular velocity values ​​of successfully matched nodes in continuous frame polar coordinate data and compare them with the theoretical angular velocity values. When the difference between the two is less than a preset angular velocity threshold, the corresponding node combination is marked as a seed ground value point set. The loop module is used to trigger the ghost removal loop and calculate the theoretical coordinate data of the global reflector topology map data in the current radar coordinate system based on the seed truth point set; The marking module is used to calculate the spatial distance difference between the polar coordinate data and the theoretical coordinate data of the remaining candidate targets in the set of highly reflective targets, and to mark the remaining candidate targets whose spatial distance difference is greater than a preset distance threshold as ghost candidate points. The calculation module is used to extract the perpendicular bisector data of the line connecting the corresponding points of the seed ground value point set and the ghost candidate points, and compare the spatial overlap between the perpendicular bisector data and the environmental contour point cloud data. The output module is used to determine that if the spatial overlap is greater than a preset overlap threshold, the ghost candidate point is a reflective ghost and is removed from the set of highly reflective targets. The unremoved points continue to participate in the ghost removal loop until the spatial distance difference is no greater than the preset distance threshold, at which point the purified true value reflective column point set is output. The update module is used to calculate the global pose data based on the purified true value reflective column point set to perform autonomous navigation, and convert the vertical bisecting plane data with spatial overlap greater than the preset overlap threshold into highly reflective solid surface data to update the global reflective column topology map data to generate dynamic semantic map data.

[0064] The above content is merely an example and illustration of the concept of the present invention. Those skilled in the art can make various modifications or additions to the specific embodiments described or use similar methods to replace them, as long as they do not deviate from the concept of the invention, they should all fall within the protection scope of the present invention.

[0065] In the description of this specification, references to terms such as "an embodiment," "example," "specific example," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the invention. In this specification, illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples.

[0066] The preferred embodiments of the present invention disclosed above are merely illustrative of the invention. These preferred embodiments do not exhaustively describe all details, nor do they limit the invention to any specific implementation. Clearly, many modifications and variations can be made based on the content of this specification. This specification selects and specifically describes these embodiments to better explain the principles and practical applications of the invention, thereby enabling those skilled in the art to better understand and utilize the invention.

Claims

1. A method for autonomous navigation of heavy-duty AGVs based on multi-source fusion and map reconstruction, characterized in that, Includes the following steps: Prior pose data is generated by integrating odometer data from heavy-duty AGV chassis with inertial navigation data. Extract the set of highly reflective targets and their corresponding polar coordinate data from the current frame of LiDAR point cloud data, and call the global reflective column topology map data and environmental contour point cloud data; Based on prior pose data, the global reflector topology map data is projected onto the current radar coordinate system to generate a theoretical reference point set. The local observation topology map is constructed using polar coordinate data, and the local observation topology map is matched with the theoretical reference point set using graph isomorphism matching. Extract the actual angular velocity values ​​of successfully matched nodes in continuous frame polar coordinate data, and compare them with the theoretical angular velocity values. When the difference between the two is less than a preset angular velocity threshold, mark the corresponding node combination as a seed ground value point set. Trigger the ghost removal loop and calculate the theoretical coordinates of the global reflector topology map data in the current radar coordinate system based on the seed ground value point set; Calculate the spatial distance difference between the polar coordinate data and the theoretical coordinate data of the remaining candidate targets in the set of highly reflective targets, and mark the remaining candidate targets whose spatial distance difference is greater than a preset distance threshold as ghost candidate points; Extract the perpendicular bisector data of the line connecting the corresponding points of the seed ground value point set and the ghost candidate points, and compare the spatial overlap between the perpendicular bisector data and the environmental contour point cloud data. If the spatial overlap is greater than the preset overlap threshold, the ghost candidate point is determined to be a reflective ghost and is removed from the set of highly reflective targets. The unremoved points continue to participate in the ghost removal loop until the spatial distance difference is no greater than the preset distance threshold, at which point the purified true value reflective column point set is output. Autonomous navigation is performed based on global pose data obtained by purifying the true value reflective column point set. Vertical bisecting plane data with spatial overlap greater than a preset overlap threshold are converted into highly reflective solid surface data and updated to global reflective column topology map data to generate dynamic semantic map data.

2. The heavy-duty AGV autonomous navigation method based on multi-source fusion and map reconstruction according to claim 1, characterized in that, Constructing a local observation topology map using polar coordinate data, including: The radial distance and polar angle values ​​of each target within the set of highly reflective targets are extracted from the polar coordinate data; Arrange all targets in the highly reflective target set in the same direction according to the increasing polar angle values ​​to generate a target ring distribution sequence; A local subset is formed by extracting a predetermined number of consecutive targets from the target circular distribution sequence; Iterate through any two targets within the local subset and calculate the difference in their corresponding polar angle values; Extract the radial distance values ​​of the two targets, and use the cosine value of the difference between the polar angle values ​​to perform trigonometric geometric mapping to calculate the spatial straight line length between the two targets. Use the spatial straight line length as Euclidean distance data. The absolute value of the difference between the polar angles of two adjacent targets in the circular distribution sequence of the local subset is extracted and set as the relative included angle data; A local observation topology map is constructed based on Euclidean distance data and relative angle data.

3. The heavy-duty AGV autonomous navigation method based on multi-source fusion and map reconstruction according to claim 2, characterized in that, Perform graph isomorphic matching between the local observation topology and the theoretical reference point set, including: Calculate the theoretical reference distance characteristics and theoretical reference angle characteristics between each reference point within the theoretical reference point set; The distance residual matrix is ​​obtained by subtracting the Euclidean distance data from the theoretical reference distance features, and the angle residual matrix is ​​obtained by subtracting the relative angle data from the theoretical reference angle features. Assign fusion weight ratios to distance residual matrix and angle residual matrix, and generate topological similarity score by weighted fusion; When the topological similarity score is greater than the preset similarity threshold, the local observed topological graph is determined to be a successful isomorphic match with the theoretical reference point set graph.

4. The heavy-duty AGV autonomous navigation method based on multi-source fusion and map reconstruction according to claim 2, characterized in that, The process of obtaining the theoretical angular velocity value includes: Extract the chassis linear velocity and chassis yaw rate values ​​of the heavy-duty AGV at the current moment from the prior pose data; Extract the radial distance and polar angle values ​​corresponding to the successfully matched nodes; The chassis linear velocity value is decomposed into the normal direction corresponding to the polar angle value to obtain the normal linear velocity component. The theoretical linear velocity value is obtained by rigid body superposition calculation of the normal linear velocity component combined with the chassis yaw rate value. The theoretical linear velocity value is obtained by dividing the radial distance value.

5. The heavy-duty AGV autonomous navigation method based on multi-source fusion and map reconstruction according to claim 1, characterized in that, The theoretical coordinate data of the global reflective column topology map data in the current radar coordinate system is calculated based on the seed ground truth point set, including: Extract the observation coordinate matrix of the seed ground value point set in the current radar coordinate system; Extract the global coordinate matrix of the corresponding seed ground value point in the global coordinate system from the global reflector topology map data; Alignment calculations are performed on the observation coordinate matrix and the global coordinate matrix to solve for the translation vector and rotation matrix between the radar coordinate system and the global coordinate system. Extract the absolute coordinates of all reflector nodes within the global reflector topology map data, and then use translation vectors and rotation matrices to perform coordinate system transformation and reprojection on the absolute coordinates to obtain the theoretical coordinates.

6. The heavy-duty AGV autonomous navigation method based on multi-source fusion and map reconstruction according to claim 1, characterized in that, Extract the perpendicular bisector data of the line connecting the corresponding points in the seed ground truth point set and the ghost candidate points, including: Convert the corresponding points in the seed truth point set and the ghost candidate points into Cartesian 3D space coordinates; Calculate the midpoint coordinate vector in 3D space, and calculate the pointing vector between the corresponding point and the ghost candidate point as the normal vector; The three-dimensional plane equation is constructed based on the midpoint coordinate vector and the normal vector, and the set of coefficients of the three-dimensional plane equation is defined as the perpendicular bisector data.

7. The heavy-duty AGV autonomous navigation method based on multi-source fusion and map reconstruction according to claim 6, characterized in that, Compare the spatial overlap between the vertical bisecting plane data and the environmental contour point cloud data, including: Calculate the orthogonal projection distance from each point cloud data in the environmental contour point cloud data to the corresponding 3D plane of the perpendicular bisector plane data; Point cloud data with orthogonal projection distances less than a preset thickness threshold are selected to form a coplanar point cloud cluster set; Calculate the area of ​​the two-dimensional projected bounding box of the coplanar point cloud cluster set on the three-dimensional plane; The ratio of the area of ​​the two-dimensional projection bounding box to the preset reflective surface reference area is calculated, and the calculated percentage value is used as the spatial overlap.

8. The heavy-duty AGV autonomous navigation method based on multi-source fusion and map reconstruction according to claim 1, characterized in that, The ghost culling loop includes a forced exit mechanism: Initialize the execution count data when the ghost removal loop is triggered; Each time the ghost removal loop is re-executed, the execution count is accumulated. The loop will be forcibly terminated when the number of executions reaches the preset maximum number of loops and there are remaining candidate targets in the set of highly reflective targets whose spatial distance difference is greater than the preset distance threshold. The pose calculation process of the current frame of LiDAR point cloud data is blocked, and the prior pose data is directly used as the global pose data to perform autonomous navigation.

9. The heavy-duty AGV autonomous navigation method based on multi-source fusion and map reconstruction according to claim 1, characterized in that, Extract the set of highly reflective targets from the current frame of LiDAR point cloud data, including: Obtain the initial echo intensity data and initial distance data of each original point in the current frame of LiDAR point cloud data; By combining the preset laser emission power benchmark value, the initial distance data is used to derive and calculate the dynamic intensity threshold; Filter out the original points whose initial echo intensity data is less than the dynamic intensity threshold, and then pack the remaining original points into a set of highly reflective targets using a distance clustering algorithm.

10. A heavy-duty AGV autonomous navigation system based on multi-source fusion and map reconstruction, applied to the heavy-duty AGV autonomous navigation method based on multi-source fusion and map reconstruction as described in any one of claims 1-9, characterized in that, include: The fusion module is used to fuse heavy-duty AGV chassis odometer data and inertial navigation data to generate prior pose data; The extraction module is used to extract the set of highly reflective targets and their corresponding polar coordinate data from the current frame of lidar point cloud data, and to call the global reflective column topology map data and environmental contour point cloud data; The module is used to project global reflector topology map data onto the current radar coordinate system based on prior pose data to generate a theoretical reference point set, and to construct a local observation topology map using polar coordinate data, and to perform graph isomorphism matching between the local observation topology map and the theoretical reference point set. The comparison module is used to extract the actual angular velocity values ​​of successfully matched nodes in continuous frame polar coordinate data and compare them with the theoretical angular velocity values. When the difference between the two is less than a preset angular velocity threshold, the corresponding node combination is marked as a seed ground value point set. The loop module is used to trigger the ghost removal loop and calculate the theoretical coordinate data of the global reflector topology map data in the current radar coordinate system based on the seed truth point set; The marking module is used to calculate the spatial distance difference between the polar coordinate data and the theoretical coordinate data of the remaining candidate targets in the set of highly reflective targets, and to mark the remaining candidate targets whose spatial distance difference is greater than a preset distance threshold as ghost candidate points. The calculation module is used to extract the perpendicular bisector data of the line connecting the corresponding points of the seed ground value point set and the ghost candidate points, and compare the spatial overlap between the perpendicular bisector data and the environmental contour point cloud data. The output module is used to determine that if the spatial overlap is greater than a preset overlap threshold, the ghost candidate point is a reflective ghost and is removed from the set of highly reflective targets. The unremoved points continue to participate in the ghost removal loop until the spatial distance difference is no greater than the preset distance threshold, at which point the purified true value reflective column point set is output. The update module is used to calculate the global pose data based on the purified true value reflective column point set to perform autonomous navigation, and convert the vertical bisecting plane data with spatial overlap greater than the preset overlap threshold into highly reflective solid surface data to update the global reflective column topology map data to generate dynamic semantic map data.

Images