Positioning method and device of intelligent guided vehicle, storage medium and electronic equipment

Abstract

The application provides a positioning method and device of an intelligent guided vehicle, a storage medium and an electronic device. The method comprises the following steps: acquiring motion data and point cloud data of the intelligent guided vehicle; performing time synchronization and space alignment processing on the point cloud data to obtain a fused point cloud frame; performing distortion removal processing on the fused point cloud frame to obtain a distortion-removed point cloud frame; constructing an iterative error state Kalman filter based on Huber M-estimation; determining a local pose transformation matrix of the intelligent guided vehicle by using the Kalman filter according to the distortion-removed point cloud frame; determining a key frame point cloud frame in the distortion-removed point cloud frame, and determining a drift correction matrix of the intelligent guided vehicle according to the key frame point cloud frame; performing synthesis processing on the drift correction matrix and the local pose transformation matrix to obtain a global pose of the intelligent guided vehicle; and determining the position of the intelligent guided vehicle according to the global pose. The method solves the problem of poor positioning accuracy of the laser inertial SLAM system of the intelligent guided vehicle in the prior art.

Description

Technical Field

[0001] This application relates to the field of positioning technology for intelligent guided vehicles, and more specifically, to a positioning method, device, storage medium, and electronic device for intelligent guided vehicles. Background Technology

[0002] Intelligent guided vehicles (IGVs) break through the physical path constraints of traditional magnetic track guidance, enabling high-precision trackless navigation in unstructured, large-span mixed working conditions, making them a core carrier for building flexible logistics systems. However, in actual operations, IGVs often alternate between large-scale outdoor parks and geometrically degraded indoor environments. This cross-scenario mixed working condition poses a severe challenge to the environmental adaptability and long-term positioning consistency of SLAM systems. Specifically, this is mainly reflected in the following three dimensions:

[0003] First, geometric feature degradation can easily lead to perceptual aliasing, causing the registration algorithm to get stuck in local minima, which in turn leads to odometry pose divergence or incorrect closed-loop constraints generated by backend optimization.

[0004] Second, the unobservable elevation in large-scale scenarios severely restricts the global consistency of mapping. Logistics parks typically have operating radii on the order of kilometers and have flat, open roads, making the geometric constraints of LiDAR on the Z-axis extremely weak. In the absence of external absolute observations such as GNSS, state drift caused by long-distance travel accumulates in the vertical direction, resulting in layering or ghosting phenomena in the constructed global map at closed loops, directly affecting the accessibility analysis of path planning.

[0005] Third, the challenges of state estimation arise from highly dynamic, nonholonomic constrained motion and limited computational resources. Highly dynamic maneuvers such as sharp turns cause lidar observation data to exhibit high temporal nonlinearity, leading to a sharp decrease in inter-frame overlap and easily inducing degradation of the front-end odometer. Simultaneously, limited onboard computing power of embedded platforms makes maintaining global map consistency while ensuring real-time processing of high-concurrency point clouds a key bottleneck for engineering implementation.

[0006] Currently, laser inertial odometry (LIO) mainly employs two mainstream technical approaches: factor graph optimization and error state Kalman filtering. Factor graph optimization-based schemes experience a dramatic increase in computational complexity over time, easily clogging the front end; while error state Kalman filtering-based schemes, although computationally fast, lack global correction capabilities.

[0007] Therefore, existing laser inertial SLAM systems for intelligent guided vehicles (IGVs) suffer from several problems when facing large-scale mixed working conditions (such as alternating outdoor logistics parks and indoor geometrically degraded environments). These problems include difficulty in achieving high-frequency real-time performance and global consistency, severe Z-axis drift, poor robustness, and unstable tracking under high dynamic motion. These issues can lead to problems such as inaccurate positioning, incorrect path planning, increased collision risk, map layering and ghosting, navigation interruption, or loss of control. In severe cases, this can result in logistics operations being halted, equipment damage, or production safety accidents. Summary of the Invention

[0008] The main objective of this application is to provide a positioning method, device, storage medium, and electronic device for intelligent guided vehicles, so as to at least solve the problem of poor positioning accuracy in the laser inertial SLAM system of intelligent guided vehicles in the prior art.

[0009] To achieve the above objectives, according to one aspect of this application, a positioning method for an intelligent guided vehicle is provided, comprising: acquiring motion data collected by the inertial measurement unit (IMU) of the intelligent guided vehicle and point cloud data collected by each lidar of the intelligent guided vehicle; performing time synchronization and spatial extrinsic parameter alignment processing on the point cloud data to obtain a fused point cloud frame; and using the motion data to perform distortion correction processing on the fused point cloud frame to obtain a distorted point cloud frame; constructing an iterative error state Kalman filter based on Huber M-estimation; using the iterative error state Kalman filter to determine the local pose transformation matrix of the intelligent guided vehicle based on the distorted point cloud frame; determining key frame point cloud frames in the distorted point cloud frame; determining the drift correction matrix of the intelligent guided vehicle based on the key frame point cloud frames; synthesizing the drift correction matrix and the local pose transformation matrix through a Lie group synthesis mechanism to obtain the global pose of the intelligent guided vehicle; and determining the position of the intelligent guided vehicle based on the global pose.

[0010] Optionally, determining the key frame point cloud frame in the distortion-free point cloud frame includes: acquiring key frame triggering conditions, determining the key frame of the intelligent guided vehicle according to the key frame triggering conditions, and extracting the key frame point cloud frame corresponding to the key frame from the distortion-free point cloud frame according to the key frame, wherein the key frame triggering conditions are triggering conditions set according to the straight-line driving distance and turning angle of the intelligent guided vehicle.

[0011] Optionally, the motion data is used to perform distortion correction processing on the fused point cloud frame to obtain a distortion-corrected point cloud frame, including: calculating the relative pose transformation of the fused point cloud frame from the start time of scanning to the end time of scanning using the median integral method based on the motion data; and projecting each point cloud of the fused point cloud frame back onto the vehicle coordinate system of the intelligent guided vehicle at the end time of scanning according to the acquisition timestamp based on the relative pose transformation to obtain the distortion-corrected point cloud frame.

[0012] Optionally, determining the drift correction matrix of the intelligent guided vehicle based on the key frame point cloud frame includes: constructing a global factor map of the intelligent guided vehicle based on the key frame point cloud frame and the laser odometry factor, altitude prior factor, and loop closure constraint factor corresponding to the key frame point cloud frame; and determining the drift correction matrix of the intelligent guided vehicle based on the global factor map.

[0013] Optionally, determining the drift correction matrix of the intelligent guided vehicle based on the global factor graph includes: performing incremental smoothing and global relinearization optimization on the global factor graph using an incremental nonlinear optimization algorithm to obtain an updated global factor graph, and determining the drift correction matrix of the intelligent guided vehicle based on the updated global factor graph.

[0014] Optionally, the global pose of the intelligent guided vehicle is obtained by synthesizing the drift correction matrix and the local pose transformation matrix using a Lie group synthesis mechanism, including: according to the formula: Determine the global pose of the intelligent guided vehicle, wherein, , This represents the globally optimal pose at the trigger moment. The drift correction matrix is... This refers to the local pose of the front end at the triggering moment. Let be the local pose transformation matrix. The global pose is defined as the moment when the keyframe triggering condition is met.

[0015] Optionally, after acquiring the motion data collected by the IMU of the intelligent guided vehicle and the point cloud data collected by each lidar, the method further includes: performing preprocessing operations on the point cloud data to obtain preprocessed point cloud data, wherein the preprocessing operations include voxel filtering and outlier removal.

[0016] According to another aspect of this application, a positioning device for an intelligent guided vehicle is provided, comprising: an acquisition unit, configured to acquire motion data collected by an inertial measurement unit (IMU) of the intelligent guided vehicle and point cloud data collected by each lidar of the intelligent guided vehicle, perform time synchronization and spatial extrinsic parameter alignment processing on the point cloud data to obtain a fused point cloud frame, and use the motion data to perform distortion correction processing on the fused point cloud frame to obtain a distorted point cloud frame; a first determination unit, configured to construct an iterative error state Kalman filter based on Huber M-estimation, and use the iterative error state Kalman filter to determine the local pose transformation matrix of the intelligent guided vehicle according to the distorted point cloud frame; a second determination unit, configured to determine key frame point cloud frames in the distorted point cloud frame, and determine the drift correction matrix of the intelligent guided vehicle according to the key frame point cloud frames, synthesize the drift correction matrix and the local pose transformation matrix through a Lie group synthesis mechanism to obtain the global pose of the intelligent guided vehicle, and determine the position of the intelligent guided vehicle according to the global pose.

[0017] According to another aspect of this application, a computer-readable storage medium is provided, the computer-readable storage medium including a stored program, wherein, when the program is executed, it controls the device where the computer-readable storage medium is located to perform any of the positioning methods of the intelligent guided vehicle described above.

[0018] According to another aspect of this application, an electronic device is provided, comprising: one or more processors, a memory, and one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, the one or more programs including methods for performing any of the positioning methods of the intelligent guided vehicle described above.

[0019] By applying the technical solution of this application, motion data from the inertial measurement unit (IMU) and point cloud data from multiple lidar sensors of the intelligent guided vehicle are acquired. After time synchronization and spatial extrinsic parameter alignment, the data is fused and distorted. An iterative error state Kalman filter based on Huber M-estimation is constructed to effectively suppress geometric degradation and the risk of point cloud mismatch in large-scale environments, thereby improving the robustness and real-time performance of local pose estimation. Simultaneously, by dynamically selecting key frame point cloud frames and calculating the drift correction matrix, and combining the Lie group synthesis mechanism, high-frequency local extrapolation and low-frequency global correction are decoupled. Under the premise of avoiding back-end optimization blocking front-end real-time processing, Z-axis drift and accumulated errors are accurately compensated, ensuring the long-term consistency and accuracy of global pose in high-dynamic motion and complex environments. This systematically solves the problems of inaccurate global pose estimation caused by the difficulty in balancing front-end real-time performance and back-end global consistency, uncontrollable Z-axis drift, positioning divergence due to mismatch, and tracking instability under high dynamic conditions in existing technologies, achieving stable, reliable, and high-precision positioning results for the intelligent guided vehicle. This solves the problem of poor positioning accuracy in existing laser inertial SLAM systems for intelligent guided vehicles. Attached Figure Description

[0020] The accompanying drawings, which form part of this application, are used to provide a further understanding of this application. The illustrative embodiments and descriptions of this application are used to explain this application and do not constitute an undue limitation of this application. In the drawings:

[0021] Figure 1 A hardware structure block diagram of a mobile terminal for performing a positioning method for an intelligent guided vehicle according to an embodiment of this application is shown.

[0022] Figure 2 A flowchart illustrating a positioning method for an intelligent guided vehicle according to an embodiment of this application is shown.

[0023] Figure 3 A structural diagram of a robust laser inertial SLAM system provided according to an embodiment of this application is shown;

[0024] Figure 4 A structural block diagram of a positioning device for an intelligent guided vehicle provided according to an embodiment of this application is shown. Detailed Implementation

[0025] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. This application will now be described in detail with reference to the accompanying drawings and embodiments.

[0026] To enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present application, and not all embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative effort should fall within the scope of protection of the present application.

[0027] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this application are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate for the embodiments of this application described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0028] As described in the background section, the existing laser inertial SLAM systems for intelligent guided vehicles have poor positioning accuracy. To address this issue, embodiments of this application provide a positioning method, apparatus, storage medium, and electronic device for intelligent guided vehicles.

[0029] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention.

[0030] The methods and embodiments provided in this application can be executed on a mobile terminal, computer terminal, or similar computing device. Taking running on a mobile terminal as an example, Figure 1 This is a hardware structure block diagram of a mobile terminal for a positioning method of an intelligent guided vehicle according to an embodiment of the present invention. For example... Figure 1 As shown, a mobile terminal may include one or more ( Figure 1 Only one is shown in the diagram. A processor 102 (which may include, but is not limited to, a microprocessor MCU or a programmable logic device FPGA, etc.) and a memory 104 for storing data are also shown. The mobile terminal may further include a transmission device 106 for communication functions and an input / output device 108. Those skilled in the art will understand that... Figure 1 The structure shown is for illustrative purposes only and does not limit the structure of the mobile terminal described above. For example, the mobile terminal may also include components that are more... Figure 1 The more or fewer components shown, or having the same Figure 1 The different configurations shown.

[0031] The memory 104 can be used to store computer programs, such as application software programs and modules, like the computer program corresponding to the positioning method of the intelligent guided vehicle in this embodiment of the invention. The processor 102 executes various functional applications and data processing by running the computer program stored in the memory 104, thereby implementing the above-described method. The memory 104 may include high-speed random access memory and non-volatile memory, such as one or more magnetic storage devices, flash memory, or other non-volatile solid-state memory. In some instances, the memory 104 may further include memory remotely located relative to the processor 102, and these remote memories can be connected to the mobile terminal via a network. Examples of the aforementioned networks include, but are not limited to, the Internet, corporate intranets, local area networks, mobile communication networks, and combinations thereof. The transmission device 106 is used to receive or send data via a network. Specific examples of the aforementioned networks may include wireless networks provided by the mobile terminal's communication provider. In one example, the transmission device 106 includes a network interface controller (NIC), which can be connected to other network devices via a base station to communicate with the Internet. In one example, the transmission device 106 may be a radio frequency (RF) module, which is used to communicate with the Internet wirelessly.

[0032] This embodiment provides a positioning method for an intelligent guided vehicle that runs on a mobile terminal, computer terminal, or similar computing device. It should be noted that the steps shown in the flowchart in the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions. Also, although a logical order is shown in the flowchart, in some cases, the steps shown or described may be executed in a different order than that shown here.

[0033] Figure 2 This is a flowchart of a positioning method for an intelligent guided vehicle according to an embodiment of this application. Figure 2 As shown, the method includes the following steps:

[0034] Step S201: Obtain motion data collected by the inertial measurement unit (IMU) of the intelligent guided vehicle and point cloud data collected by each lidar of the intelligent guided vehicle. Perform time synchronization and spatial extrinsic parameter alignment processing on the point cloud data to obtain a fused point cloud frame. Then, use the motion data to perform distortion correction processing on the fused point cloud frame to obtain a distortion-corrected point cloud frame.

[0035] Specifically, firstly, motion data collected by the inertial measurement unit (IMU) of the intelligent guided vehicle and point cloud data collected by each lidar of the intelligent guided vehicle are acquired. Through time synchronization and spatial extrinsic parameter alignment processing, the multi-source sensor data are unified to the same spatiotemporal reference to form a structurally consistent fused point cloud frame. Subsequently, the IMU motion data is used to perform distortion correction processing on the fused point cloud frame to eliminate the spatial distortion of the point cloud caused by the vehicle's movement during lidar scanning. This ensures that each feature point in the point cloud corresponds to its true position in the rigid body coordinate system at the same moment, thereby guaranteeing that the geometric observation data on which the subsequent state estimation depends has inherent temporal and spatial consistency.

[0036] Step S202: Construct an iterative error state Kalman filter based on Huber M-estimation, and use the iterative error state Kalman filter to determine the local pose transformation matrix of the intelligent guided vehicle based on the above-mentioned distorted point cloud frame.

[0037] Specifically, an iterative error state Kalman filter based on Huber M-estimation is first constructed. This filter uses a distorted point cloud frame as the observation input and iteratively corrects the system state estimation error. Internally, the filter employs the Huber M-estimation mechanism, adaptively adjusting the observation weights based on the distribution characteristics of the point cloud registration residuals, thereby reducing the interference of anomalously matched point clouds on the estimation results during state updates. In each iteration, the filter linearizes the observation residuals and, combined with the covariance propagation model of the error state, achieves recursive optimization of the intelligent guided vehicle's pose. Finally, based on the optimal error state after filter convergence, the nominal state is updated through an exponential mapping on a Lie group, outputting the local pose transformation matrix of the current frame in the local odometry coordinate system. This process directly achieves robust suppression of non-Gaussian observation noise in geometrically degraded environments and stabilizes the local pose output through an iterative correction mechanism.

[0038] Step S203: Determine the key frame point cloud frame in the above-mentioned distortion-reduced point cloud frame, and determine the drift correction matrix of the above-mentioned intelligent guided vehicle based on the above-mentioned key frame point cloud frame. Combine the above-mentioned drift correction matrix and the above-mentioned local pose transformation matrix through the Lie group synthesis mechanism to obtain the global pose of the above-mentioned intelligent guided vehicle, and determine the position of the above-mentioned intelligent guided vehicle based on the above-mentioned global pose.

[0039] Specifically, firstly, key frame point cloud frames are identified and determined from the distorted point cloud frames. This process, based on the geometric change characteristics of the point cloud data, selects representative frames sufficient to represent changes in the environmental structure, serving as benchmark samples for subsequent pose correction. Subsequently, a relative pose deviation model between the local and global coordinate systems is constructed based on these key frame point cloud frames, thereby deriving the drift correction matrix of the intelligent guided vehicle in global space. This matrix characterizes the systematic pose offset between the front-end local deduction and the back-end global optimization results. Through a Lie group synthesis mechanism, the drift correction matrix and the local pose transformation matrix output in real time from the front end are subjected to non-commutative composite operation on a Lie group manifold to achieve dynamic correction of the local pose, ensuring that it maintains consistency with the global optimization results in mathematical structure. Based on the synthesized global pose, the precise spatial position of the intelligent guided vehicle in the environment is directly determined, completing the closed-loop mapping from point cloud features to pose output.

[0040] In this embodiment, by applying the above steps S201, S202, and S203, the motion data of the inertial measurement unit (IMU) and the point cloud data of multiple lidar sensors of the intelligent guided vehicle are acquired. After time synchronization and spatial extrinsic parameter alignment, the data is fused and distorted. An iterative error state Kalman filter based on Huber M-estimation is constructed to effectively suppress the risk of geometric degradation and point cloud mismatch in large-scale environments, thereby improving the robustness and real-time performance of local pose estimation. At the same time, by dynamically selecting key frame point cloud frames and calculating the drift correction matrix, and combining the Lie group synthesis mechanism, the high-frequency local extrapolation and low-frequency global correction are decoupled. Under the premise of avoiding back-end optimization blocking front-end real-time processing, the Z-axis drift and accumulated error are accurately compensated, ensuring the long-term consistency and accuracy of global pose in high-dynamic motion and complex environments. This systematically solves the problems of inaccurate global pose estimation caused by the difficulty in balancing front-end real-time performance and back-end global consistency, uncontrollable Z-axis drift, positioning divergence due to mismatch, and tracking instability under high dynamic conditions in the prior art, achieving a stable, reliable, and high-precision intelligent guided vehicle positioning effect. This solves the problem of poor positioning accuracy in existing laser inertial SLAM systems for intelligent guided vehicles.

[0041] In the specific implementation process, determining the key frame point cloud frame in the above-mentioned distortion-free point cloud frame includes: obtaining the key frame triggering condition, determining the key frame of the above-mentioned intelligent guided vehicle according to the key frame triggering condition, and extracting the key frame point cloud frame corresponding to the above-mentioned key frame from the above-mentioned distortion-free point cloud frame according to the above-mentioned key frame, wherein the key frame triggering condition is a triggering condition set according to the straight-line driving distance and turning angle of the above-mentioned intelligent guided vehicle.

[0042] The keyframe triggering condition is achieved by setting a threshold for the straight-line travel distance of the intelligent guided vehicle (e.g., 1.0m) and a threshold for the turning angle of the intelligent guided vehicle (e.g., 15°). During the travel of the intelligent guided vehicle, if any of the thresholds for the straight-line travel distance and the turning angle are met, the keyframe point cloud frame at the current moment is extracted.

[0043] In this embodiment, by dynamically setting keyframe triggering conditions based on the straight-line travel distance and turning angle of the intelligent guided vehicle, the system can automatically insert keyframes under geometrically degraded conditions such as high angular velocity sharp turns and low linear velocity, thereby significantly improving the angular resolution and point cloud feature overlap rate of the local sub-map and effectively alleviating the problem of front-end tracking instability caused by inter-frame matching failure.

[0044] Specifically, the above motion data is used to perform distortion correction processing on the above fused point cloud frame to obtain a distortion-corrected point cloud frame, including: calculating the relative pose transformation of the above fused point cloud frame from the start time of scanning to the end time of scanning using the median integral method based on the above motion data; and projecting each point cloud of the above fused point cloud frame back onto the vehicle coordinate system of the above intelligent guided vehicle at the end time of scanning according to the acquisition timestamp based on the above relative pose transformation to obtain the above distortion-corrected point cloud frame.

[0045] In this embodiment, by acquiring motion data collected by the inertial measurement unit (IMU) of the intelligent guided vehicle and point cloud data collected by each lidar, and after completing time synchronization and spatial extrinsic parameter alignment, the motion data of the IMU is combined with the median integral method to accurately calculate the complete relative pose transformation of the fused point cloud frame from the start time of the scan to the end time of the scan. Then, based on the original acquisition timestamp of each point cloud, the point cloud is back-projected onto the vehicle coordinate system at the end time of the scan, so as to achieve accurate correction of the spatial distortion of the point cloud caused by vehicle motion, forming a distortion-free point cloud frame with significantly improved geometric consistency, thereby effectively eliminating the observation error introduced by the front-end odometer due to motion distortion.

[0046] More specifically, determining the drift correction matrix of the intelligent guided vehicle based on the aforementioned key frame point cloud frame includes: constructing a global factor map of the intelligent guided vehicle based on the aforementioned key frame point cloud frame, and the laser odometry factor, altitude prior factor, and lap-loop constraint factor corresponding to the aforementioned key frame point cloud frame; and determining the aforementioned drift correction matrix of the intelligent guided vehicle based on the aforementioned global factor map.

[0047] In this embodiment, by acquiring IMU motion data and lidar point cloud data of the intelligent guided vehicle, and after time synchronization and spatial extrinsic parameter alignment, distortion removal processing is performed to construct a local pose transformation matrix. Key frame point cloud frames are then selected from this matrix, and each key frame point cloud frame is further associated with a corresponding laser odometry factor, altitude prior factor, and loop closure constraint factor to construct a global factor map. Among them, the laser odometry factor provides continuous motion constraints, the altitude prior factor imposes physical constraints on the Z-axis pose in a large-scale flat environment to suppress long-term cumulative drift, and the loop closure constraint factor introduces a global correction signal when environmental revisit is detected to eliminate cumulative errors. By jointly optimizing the global factor map constructed by the three types of factors, the drift correction matrix is ​​accurately calculated, and a Lie group synthesis mechanism is used to fuse it with the local pose transformation matrix to obtain a high-precision and high-stability global pose estimation. This effectively solves the problem of positioning divergence and pose inaccuracy caused by the unobservable Z-axis and lack of closed-loop correction in geometric degradation and large-scale environments, thus realizing the robustness and global consistency of the laser inertial SLAM system in high-dynamic and long-distance operation.

[0048] Further, determining the drift correction matrix of the intelligent guided vehicle based on the global factor graph includes: performing incremental smoothing and global relinearization optimization on the global factor graph using an incremental nonlinear optimization algorithm to obtain an updated global factor graph, and determining the drift correction matrix of the intelligent guided vehicle based on the updated global factor graph.

[0049] In this embodiment, by acquiring IMU motion data and LiDAR point cloud data of the intelligent guided vehicle, a fused point cloud frame is constructed after time synchronization, spatial extrinsic parameter alignment, and distortion correction. A local pose transformation matrix is ​​output based on the Huber M-estimated Kalman filter. Subsequently, a global factor map containing LiDAR odometry factors, altitude prior factors, and closure constraint factors is constructed based on the key frame point cloud frame. To address the problem of factor map size expansion and computational delay accumulation caused by the continuous increase of key frames, an incremental nonlinear optimization algorithm is introduced to perform incremental smoothing and global relinearization processing on the global factor map. Local updates and optimizations are performed only on newly added key frames and their associated factors to avoid recalculating the entire map, thereby efficiently generating the updated global factor map and accurately deriving the drift correction matrix based on it.

[0050] Furthermore, by synthesizing the aforementioned drift correction matrix and local pose transformation matrix using a Lie group synthesis mechanism, the global pose of the intelligent guided vehicle is obtained, including: according to the formula: The global pose of the aforementioned intelligent guided vehicle is determined, wherein, , This represents the globally optimal pose at the trigger moment. The above drift correction matrix, This refers to the local pose of the front end at the aforementioned trigger moment. The above local pose transformation matrix is... Given the global pose described above, the triggering time represents the moment when the keyframe triggering condition is met.

[0051] In this embodiment, by introducing the trigger time as a temporal constraint for the Lie group synthesis operation, it is ensured that the drift correction matrix and the local pose transformation matrix are synthesized synchronously only when the keyframe trigger condition is met. At this time, the drift correction matrix generated by the back-end optimization exactly corresponds to the local pose transformation matrix of the latest keyframe of the front end. The two are strictly aligned in the time dimension, thereby completely avoiding pose mismatch and synthesis error caused by asynchronous data at non-keyframe times. This mechanism effectively suppresses the global pose divergence problem caused by Z-axis drift, mismatch or high dynamic motion, so that the global pose estimation is always based on the optimal state update with temporal consistency, which significantly improves the positioning stability and accuracy of the system in geometric degradation and large-scale environments, and finally achieves efficient collaboration between front-end real-time performance and back-end global consistency.

[0052] Specifically, after acquiring the motion data collected by the IMU of the intelligent guided vehicle and the point cloud data collected by each lidar, the above method further includes: performing preprocessing operations on the point cloud data to obtain preprocessed point cloud data, wherein the preprocessing operations include voxel filtering and outlier removal.

[0053] In this embodiment, after acquiring the IMU motion data and lidar point cloud data of the intelligent guided vehicle, voxel filtering and outlier removal are first performed on the original point cloud data. This effectively reduces the point cloud density and removes noise and outliers, thus completing data purification before time synchronization and spatial extrinsic parameter alignment. This makes the geometric structure of the subsequently generated fused point cloud frame clearer and the features more stable, significantly reducing the redundant computational burden and mismatch risk in the distortion correction and front-end registration stages. It also improves the convergence speed and estimation accuracy of the iterative error state Kalman filter based on Huber M-estimation for the local pose transformation matrix, thereby enhancing the stability and robustness of the entire positioning system in geometric degradation and large-scale environments. Ultimately, it achieves high accuracy and anti-drift capability in global pose estimation, solving the problems of positioning divergence and uncontrollable Z-axis drift caused by the lack of preprocessing of the original point cloud.

[0054] To enable those skilled in the art to better understand the technical solution of this application, the implementation process of the positioning method of the intelligent guided vehicle of this application will be described in detail below with reference to specific embodiments.

[0055] This application provides a robust laser inertial SLAM system for geometrically degraded and large-scale environments, and a specific positioning method for intelligent guided vehicles, mainly addressing the following technical problems:

[0056] 1. Resolving the contradiction between high-frequency real-time performance and global consistency in embedded platforms. In existing technologies, tightly coupled factor graph optimization schemes experience a dramatic increase in computational load over time, easily blocking the front end; while pure filtering schemes, although fast, lack global correction capabilities. This application aims to achieve consistent optimization of a large-scale global map in the back end on a resource-constrained embedded platform by using a spatiotemporally decoupled asynchronous parallel architecture.

[0057] 2. Addressing the issue of accumulated elevation drift caused by the unobservable Z-axis in large-scale flat environments. In scenarios with flat and open roads, such as logistics parks, LiDAR lacks effective geometric constraints in the vertical direction (Z-axis). Long-distance travel can easily lead to accumulated state drift, causing layering or ghosting phenomena in the global map. This application addresses this by constructing a regularization factor map based on the ground manifold assumption and an elevation prior factor. This suppresses unbounded Z-axis drift at the probabilistic graphical model level, ensuring global consistency in long-distance, large-scale mapping.

[0058] 3. Addressing the localization divergence problem in environments with degraded geometric features or repetitive textures. Existing algorithms are prone to perceptual aliasing in scenarios such as open workshops and long corridors with regular steel structures, leading to registration getting trapped in local minima. This application introduces dual-radar spatiotemporal alignment and an M-estimation mechanism based on the Huber kernel function to automatically identify and reduce the weight of mismatched point clouds, improving the system's robustness in non-Gaussian observation noise environments and preventing localization failure.

[0059] 4. Solving the problem of unstable front-end tracking under high-dynamic nonholonomic constraint motion (such as sharp turns). This addresses the issue of odometer degradation caused by a sharp decrease in the overlap between LiDAR frames when a vehicle makes sharp turns in narrow passages (low linear velocity, high angular velocity). This application employs an angular velocity-driven adaptive keyframe strategy to automatically encrypt keyframes under high-dynamic conditions, ensuring the geometric coverage of the sub-map and the stability of registration constraints.

[0060] This application constructs a two-layer state estimation architecture based on spatiotemporal decoupling and defines a local odometry coordinate system. With the global world coordinate system The dual-state space decomposes the SLAM problem into two relatively independent but interactive stochastic processes: high-frequency local deduction and low-frequency global correction. Figure 3 As shown, the robust laser inertial SLAM system for geometrically degraded and large-scale environments mainly consists of four core processing units:

[0061] (1) Multi-source sensing input layer: responsible for data acquisition, time synchronization and spatial alignment of multiple lidar and IMU, providing complete geometric observation input for subsequent state estimation.

[0062] (2) High-frequency odometer front end: runs in the local odometer coordinate system and performs iterative error state Kalman filter (IESKF) state estimation based on Huber kernel function.

[0063] (3) Low-frequency incremental optimization backend: runs in the global world coordinate system, maintains the global factor graph, and focuses on introducing height prior factors to suppress Z-axis drift.

[0064] (4) Asynchronous communication and synchronous layer: responsible for dynamically correcting the pose of the front-end output by using the Lie group synthesis mechanism after the back-end optimization is completed.

[0065] This embodiment relates to a specific positioning method for an intelligent guided vehicle, including the following steps:

[0066] Step 1: Unifying the spatiotemporal reference of multi-source sensing data:

[0067] To address the blind spot issue inherent in IGV systems with a single radar configuration, the system employs two 32-line mechanical lidar units arranged diagonally. Therefore, in order to construct a unified... Omnidirectional environmental perception requires solving the problems of time synchronization and spatial calibration among multiple sensors.

[0068] In the time domain, the system achieves hardware-level time synchronization between the two radars and the IMU based on the PTP protocol, unifying the timestamps of all sensors to microsecond-level accuracy. In the spatial domain, a vehicle coordinate system is defined. With the IMU center as the origin, the coordinate systems of the two lidar units are denoted as follows: and , its relative The extrinsic parameter transformation is denoted as For the first Frame scan, let the original point cloud sets collected by the left and right radars be... and The radar points in the two sets are denoted as follows: and The system uses an extrinsic parameter matrix to uniformly transform the two point clouds to... Coordinate system, generating fused point cloud frames through union operation :

[0069] (1);

[0070] However, due to the continuous rotation scanning characteristics of mechanical lidar, the point cloud frames are fused. any point in In fact, it is at different times within the scanning cycle. The data was collected. When the IGV is in dynamic motion, ignoring the acquisition time difference will lead to severe motion distortion in the point cloud. Therefore, this system utilizes high-frequency IMU data... The pre-integration results on the manifold are backpropagated to correct distortion. The system state vector is defined. , For attitude rotation matrix, For position vector, For velocity vector, Additive bias in IMU angular velocity measurements. Additive bias in IMU acceleration measurements. The continuous-time kinematic model of the IMU is as follows:

[0071] (2);

[0072] in, For IMU measurements, It is Gaussian white noise. This represents antisymmetric matrix operations. The IMU data is discretized using the median integral method. For adjacent IMU time steps... and Time interval The state recurrence equation is:

[0073] (3);

[0074] in , which is the median acceleration term. The acceleration is accumulated by integration from the radar point at the time of data collection. Until the end of the current frame scan Relative pose transformation of the vehicle body , will the distortion point Projected to the vehicle coordinate system at the end of the scan Below, we obtain the distortion correction point: (4);

[0075] in, Indicates the vehicle system from the sampling time. Vehicle system until scanning ends Relative rotation, Indicates from arrive The relative translation. After this step, the point cloud is merged. Unified to the same timestamp The rigid body coordinate system provides a geometrically consistent foundation for subsequent state estimation.

[0076] Step 2: Robust front-end odometry based on Huber M-estimation:

[0077] In open workshop environments, the regularly arranged steel columns and uniform walls lead to high repetition of environmental geometric features, making it prone to problems such as incorrect correspondence, heavy-tailed residuals, and local degradation during laser point cloud matching. To improve the robustness of the front-end odometry under non-Gaussian observation noise conditions, this application constructs an iterative error state Kalman filter front-end estimation method based on Huber M-estimation. This method integrates point-to-surface geometric observations based on IMU prediction priors to achieve robust optimization estimation of the current frame pose.

[0078] Define the error state vector , Indicates position error, Indicates speed error, Indicates attitude error, Indicates gyroscope bias error, This represents the accelerometer bias error. This application employs left-multiplied attitude error injection and defines a perturbation update operator. for:

[0079] (5);

[0080] Linearized propagation of the continuous-time IMU kinematic model at discrete time points. Prediction error state at time 1 satisfy: (6);

[0081] in For process noise, and These are the state transition matrix and noise injection matrix of the discrete-time error system, respectively, obtained by discretizing the continuous-time error dynamics over the sampling interval and linearizing it at the current nominal state. Accordingly, covariance matrix The recursive form is:

[0082] (7);

[0083] in for The state covariance matrix of the prediction error at time t. This represents the process noise covariance. In subsequent observation update phases, It will be used as a prior information matrix to participate in incremental optimization in equations (13)-(14), and will be used to provide regularization constraints on the incremental solution when there is insufficient observation information or outlier residuals.

[0084] After obtaining the predicted state and its covariance at the current moment, the system further constructs a laser observation update term by combining it with the local map. This application uses an ikd-Tree to dynamically maintain the local manifold map. Specifically, after downsampling the distortion-free point cloud of the current frame, its increment is inserted into the local map according to the current predicted pose, and a local sub-map composed of the most recent keyframes is maintained through a sliding window mechanism. Thus, the local map can not only provide geometric reference for subsequent point-to-surface matching, but also realize online incremental updates of the local environment map.

[0085] In addition to point-to-surface matching based on ikd-Tree, other registration techniques such as point-to-point matching, normal distribution transformation, or feature extraction can also be used.

[0086] Based on the aforementioned predicted state and local map, this application further utilizes point-to-surface geometric consistency to construct observation constraints. For points in the distortion-free point cloud... By establishing a point-to-face correspondence through nearest neighbor search, the unit normal vector of the matching plane is obtained. With a reference point on the plane For the current iteration nominal pose The residual is defined as: (8);

[0087] Within the IESKF framework, at the... Sub-iteration estimation At this point, the residuals are linearized to the first order:

[0088] (9);

[0089] in Let be the Jacobian matrix of the residual with respect to the error state. From formula (7), we can obtain the partial derivative: (10);

[0090] Since point-to-plane correspondences may contain mismatches and heavy-tailed residuals, this application introduces the Huber kernel function. Construct the M-estimate and utilize the weighting function. The observation residuals are reweighted to reduce the impact of outliers on the state estimation results. The Huber loss function is defined as: (11);

[0091] The corresponding weights are: (12);

[0092] threshold The median of the point cloud residual in the current frame is dynamically set to improve the system's adaptability to different environmental noise levels.

[0093] By incorporating both the prior terms and the weighted observation terms into the quadratic approximation objective function, we can obtain the error state increment. Optimization issues:

[0094] (13);

[0095] in For the first The robust weights corresponding to each residual To predict the state covariance of the error, To observe the noise covariance matrix, For the number of points-to-faces participating in the optimization, This is the prior information matrix. Regarding the error state increment... Solving the optimization problem formula yields the normal equation in the form of an information matrix:

[0096] (14);

[0097] Therefore, the error state increment can be expressed as: (15);

[0098] Based on the obtained error state increment, a perturbation injection update is performed on the current nominal state. The system iterates through equations (9) to (15) until the preset convergence criterion is met or the maximum number of iterations is reached, thereby obtaining the optimal error state increment. and in accordance with The perturbation injection rule compensates for this to the current nominal state, resulting in the posterior nominal state of the current frame. The system then extracts the rotation matrix from the updated posterior nominal state. With translation vector Based on this, the vehicle body is assembled in the local odometer coordinate system at the current moment. High-frequency local pose transformation matrix This local pose, as the final output of the front-end odometry, will be directly provided to the subsequent keyframe selection and asynchronous state synchronization processes for the final global pose result of the dynamic synthesis system.

[0099] Step 3: Adaptive keyframe filtering based on translation-rotation joint threshold:

[0100] To address the tracking loss issue during sharp turns and ensure geometric coverage under high-dynamic rotation, this application proposes an adaptive keyframe insertion criterion that couples rotation and displacement. The current frame is defined. Compared to the previous keyframe The relative motion is The keyframe triggering conditions are designed as follows:

[0101] (16);

[0102] in, A boolean indicator variable triggered by a keyframe. and These are the relative translation and rotation components, respectively. Let be the logarithmic mapping from Lie groups to Lie algebras. This indicates the distance of the rotated geodesic. This application defaults to using the translation trigger threshold. =1.0m, rotation trigger threshold =15°. In long, straight corridors or open surfaces where translation is the primary mode of travel, the system mainly relies on the translation criterion. Keyframes are extracted uniformly; however, in high-dynamic conditions such as sharp turns, although the absolute displacement of the vehicle body is small, the spatial attitude changes drastically, making it easier to meet the rotation threshold. This improves the spatial angular resolution of the local sub-map. When the current frame meets the keyframe triggering conditions, in addition to adding its corresponding pose node to the backend factor map, the downsampled point cloud of that frame is also constructed as a keyframe sub-map for subsequent loop closure matching and global map stitching.

[0103] Step 4: Backend optimization and global mapping based on the ground manifold assumption:

[0104] To address the issues of Z-axis observability degradation in long-distance flat road scenarios and the tendency of conventional six-DOF factor graph optimization to generate cumulative drift in the vertical direction, this application constructs a back-end factor graph model in a global world coordinate system. It introduces the ground manifold assumption and height prior constraints, achieving global pose optimization while simultaneously reshaping the keyframe sub-map for consistency, thereby incrementally generating a globally consistent map. Furthermore, in addition to constructing height prior factors based on the "ground manifold assumption," it also allows for the introduction of barometer data as absolute height constraints, GNSS elevation data as constraints, and the use of ranging data from ultrasonic sensors or millimeter-wave radar to construct ground plane constraints.

[0105] The global state vector of the system is defined as ,in Indicates the first The pose state of each keyframe in the world coordinate system The constraint factors in the factor diagram include the laser odometry factor, the lap-loop constraint factor, and the height prior factor.

[0106] Factor graph construction and optimization follow a keyframe event-driven mechanism, specifically including two types of triggering events:

[0107] 1) Keyframe Insertion Event: When the adaptive keyframe criterion is met, the new keyframe node and its corresponding laser odometry factor and altitude prior factor are added to the factor map, triggering ISAM2 for incremental smoothing. Simultaneously, the optimized keyframe pose is used to transform the corresponding sub-map to the world coordinate system and update the globally consistent map. In addition to using the ISAM2 algorithm for incremental smoothing, G2O, Ceres Solver, Levenberg-Marquardt algorithms, or Gauss-Newton methods can also be used for batch or incremental nonlinear optimization.

[0108] 2) Loop closure detection confirmation event: When a historical loop is detected and passes geometric consistency verification, the loop closure constraint factor is added to the factor graph, triggering global relinearization and optimization.

[0109] Driven by the aforementioned events, the backend optimizer jointly solves all constraints in the current factor graph, minimizing the Mahalanobis distance weighted sum of the odometry residual, loop closure residual, and height prior residual, to obtain the globally optimal estimate of the poses of all current keyframes. :

[0110] (17);

[0111] in, , and These are, respectively, odometry constraints, laparoscopy constraints, and height anisotropy prior factor residuals; The Mahalanobis distance, weighted by the inverse of the observation noise covariance, is used to characterize the differences in the reliability of observations of different factors.

[0112] (2) Construction of highly anisotropic prior factors:

[0113] To suppress Z-axis drift while retaining adaptability to real-world slopes, this application constructs a quasi-two-dimensional manifold constraint. A univariate height prior factor is constructed, whose residual is defined as the residual deviation between the vehicle's height component and the reference height in the world coordinate system: (18);

[0114] in, The Z-axis basis vector, State Node The translation vector of the vehicle coordinate system in the world coordinate system. To initialize the baseline height, the covariance matrix is ​​modeled using a height-anisotropic noise model. (19);

[0115] in This indicates the construction of a diagonal matrix. Let Variance be the noise variance in the horizontal direction. Let Z be the noise variance in the Z direction. In formula (17), the penalty weight corresponding to this height prior factor is given by the inverse of the covariance matrix. The decision ordered This is equivalent to imposing a weak constraint on the horizontal direction and a strong constraint on the Z direction, so that the prior terms mainly apply regularization to the error of the Z direction deviating from the reference height, while having a smaller impact on the trajectory optimization in the XY plane.

[0116] Under flat and open road conditions, the constraints of odometer observations in the Z-direction are relatively weakened, and the optimization results are more susceptible to cumulative errors in the vertical direction. In this case, height priors can provide a vertical reference to suppress height drift. For conditions such as slopes where there are real elevation changes, the constraints provided by IMU and laser observations in the Z-direction are enhanced, and the relative weights of the observation terms in the vertical direction are increased. This avoids imposing strong constraints on real terrain changes by fixed height priors, allowing the system to maintain its ability to track terrain undulations.

[0117] Step 5: Asynchronous State Synchronization and Dynamic Output:

[0118] To avoid the computational latency of backend factor graph optimization directly affecting the real-time state of the frontend and thus causing time alignment errors, this application designs an asynchronous state synchronization mechanism for a two-layer state estimation architecture. When the frontend is in the local odometry coordinate system... When continuously performing high-frequency state simulations, the backend operates in the global world coordinate system. The keyframe pose is optimized using low-frequency optimization; the two are decoupled and synchronized through snapshot locking and drift correction.

[0119] When the backend meets the optimization trigger conditions, the system performs an atomic snapshot operation, locking the current local state of the frontend, the corresponding covariance matrix, and the current drift correction matrix, and sends this state snapshot to the backend optimization thread. During this period, the frontend thread is not blocked and continues to operate based on the local Markov assumption. The system continuously extrapolates the pose of the current frame, thereby ensuring the high frequency and continuity of the local odometer output.

[0120] After the backend completes a factor graph update based on the above snapshot, it can obtain the globally optimal pose corresponding to the snapshot at the trigger time. Based on this, a drift correction matrix is ​​constructed from the local odometry coordinate system to the world coordinate system:

[0121] (20);

[0122] in, It is used to characterize the drift correction relationship between the local trajectory at the front end and the global optimal trajectory at the back end at the current moment. The local pose at the beginning of the snapshot is used to trigger the snapshot. For any subsequent time... The system combines the aforementioned drift correction matrix with the local pose continuously output from the front end. By performing Lie group synthesis, we obtain the global high-frequency pose for output: (twenty one);

[0123] Therefore, without blocking the real-time simulation at the front end, the system can continuously synchronize the low-frequency global optimization results from the back end to the high-frequency pose output at the front end, achieving a balance between global consistency and real-time performance.

[0124] The system ultimately outputs a global high-frequency pose and a globally consistent map. The former is used for real-time pose calculation and motion control, while the latter is continuously regenerated from the optimized keyframe sub-map in the backend and can be used as the environmental map result for subsequent navigation or relocalization tasks.

[0125] This application specifically achieves the following technical effects:

[0126] 1. This invention resolves the conflict between the limited computing power of embedded platforms and the need for globally consistent computation, significantly improving system real-time performance. Existing technologies (such as LIO-SAM) employ a serial processing mechanism. As the map size increases, the backend optimization time increases linearly, easily blocking the front-end odometer and causing control command delays. This application completely decouples high-frequency front-end deduction and low-frequency backend optimization at the physical thread level through an asynchronous parallel two-layer architecture and a "send-and-drop" communication mechanism. The front-end output frequency is only limited by the sensor sampling rate and is not affected by the backend loopback optimization time. This design ensures that even on embedded platforms with limited computing resources, the system can still meet the requirements of the underlying controller for high-frequency, low-latency pose feedback.

[0127] 2. This invention solves the map "layering" problem caused by the unobservable Z-axis in large-scale flat environments, significantly improving mapping accuracy. Existing technologies (such as FAST-LIO2), as pure filtering frameworks, cannot suppress the cumulative drift of the Z-axis in flat logistics parks lacking GNSS. This application introduces a height prior factor based on the ground manifold assumption, constructs an anisotropic noise model, and applies a regularization constraint on the Z-axis to the factor map.

[0128] 3. Improved localization robustness in environments with missing geometric features and repetitive textures, effectively preventing perceptual aliasing. Existing technologies are prone to mismatches due to feature repetition in open workshops or regular steel structure environments, leading to registration getting trapped in local minima. This application integrates Huber M-estimation and adaptive weight adjustment mechanisms into the IESKF front-end. The system can automatically identify abnormal observations based on the residual distribution and dynamically reduce their weights, thus converging to the correct pose even in severely degraded environments.

[0129] 4. Enhanced vehicle tracking stability under high-dynamic, nonholonomic constraint motion. Existing technologies typically select keyframes using fixed distances or time thresholds. When the vehicle makes sharp turns (low linear velocity, high angular velocity), this leads to a sharp decrease in the overlap of point clouds between frames, inducing tracking loss. This application proposes an angular velocity-driven adaptive keyframe strategy, introducing geodesic distance on the rotating manifold as a criterion. When drastic fluctuations in angular velocity are detected, the system automatically switches to a high-frequency keyframe insertion mode, ensuring that the geometric coverage and feature overlap rate of the local sub-map are always maintained above 60%, guaranteeing the connectivity and optimized numerical stability of the back-end pose graph under drastic motion conditions.

[0130] This application also provides a positioning device for an intelligent guided vehicle. It should be noted that the positioning device for the intelligent guided vehicle in this application can be used to execute the positioning method for intelligent guided vehicles provided in this application. This device is used to implement the above embodiments and preferred embodiments; details already described will not be repeated. As used below, the term "module" can refer to a combination of software and / or hardware that implements a predetermined function. Although the device described in the following embodiments is preferably implemented in software, hardware implementation, or a combination of software and hardware, is also possible and contemplated.

[0131] The following describes the positioning device for the intelligent guided vehicle provided in the embodiments of this application.

[0132] Figure 4 This is a schematic diagram of a positioning device for an intelligent guided vehicle according to an embodiment of this application. Figure 4 As shown, the device includes:

[0133] The acquisition unit 41 is used to acquire motion data collected by the inertial measurement unit (IMU) of the intelligent guided vehicle and point cloud data collected by each lidar of the intelligent guided vehicle. The point cloud data is time-synchronized and spatial extrinsic parameter aligned to obtain a fused point cloud frame. The motion data is then used to perform distortion correction on the fused point cloud frame to obtain a distortion-corrected point cloud frame.

[0134] The first determining unit 42 is used to construct an iterative error state Kalman filter based on Huber M-estimation, and to determine the local pose transformation matrix of the intelligent guided vehicle using the iterative error state Kalman filter according to the above-mentioned distorted point cloud frame.

[0135] The second determining unit 43 is used to determine the key frame point cloud frame in the above-mentioned distortion-free point cloud frame, and determine the drift correction matrix of the above-mentioned intelligent guided vehicle based on the above-mentioned key frame point cloud frame. The drift correction matrix and the above-mentioned local pose transformation matrix are synthesized and processed by the Lie group synthesis mechanism to obtain the global pose of the above-mentioned intelligent guided vehicle, and the position of the above-mentioned intelligent guided vehicle is determined based on the above-mentioned global pose.

[0136] In this embodiment, the acquisition unit is used to acquire motion data collected by the inertial measurement unit (IMU) of the intelligent guided vehicle and point cloud data collected by each lidar of the intelligent guided vehicle. The point cloud data is then time-synchronized and spatially aligned to obtain a fused point cloud frame. The motion data is then used to perform distortion correction processing on the fused point cloud frame to obtain a distorted point cloud frame. The first determination unit is used to construct an iterative error state Kalman filter based on Huber M-estimation. The Kalman filter is used to determine the local pose transformation matrix of the intelligent guided vehicle based on the distorted point cloud frame. The second determination unit is used to determine the key frame point cloud frames in the distorted point cloud frame, and to determine the drift correction matrix of the intelligent guided vehicle based on the key frame point cloud frames. The drift correction matrix and the local pose transformation matrix are synthesized using a Lie group synthesis mechanism to obtain the global pose of the intelligent guided vehicle, and the position of the intelligent guided vehicle is determined based on the global pose. By acquiring motion data from the inertial measurement unit (IMU) and point cloud data from multiple lidar sensors of the intelligent guided vehicle, and performing time synchronization and spatial extrinsic parameter alignment followed by fusion and distortion correction, an iterative error state Kalman filter based on Huber M-estimation is constructed. This effectively suppresses the risk of geometric degradation and point cloud mismatches in large-scale environments, improving the robustness and real-time performance of local pose estimation. Simultaneously, by dynamically selecting keyframe point cloud frames and calculating the drift correction matrix, combined with a Lie group synthesis mechanism, high-frequency local extrapolation and low-frequency global correction are decoupled. This accurately compensates for Z-axis drift and accumulated errors while avoiding back-end optimization blocking front-end real-time processing, ensuring long-term consistency and accuracy of global pose in high-dynamic motion and complex environments. This systematically solves the problems of inaccurate global pose estimation caused by the difficulty in balancing front-end real-time performance and back-end global consistency, uncontrollable Z-axis drift, positioning divergence due to mismatches, and tracking instability under high dynamic conditions, achieving stable, reliable, and high-precision positioning for the intelligent guided vehicle. This solves the problem of poor positioning accuracy in existing laser inertial SLAM systems for intelligent guided vehicles.

[0137] As an optional solution, the determining unit includes an acquisition module, which is used to acquire key frame triggering conditions, determine key frames of the intelligent guided vehicle based on the key frame triggering conditions, and extract the key frame point cloud frame corresponding to the key frame from the distortion-free point cloud frame based on the key frame, wherein the key frame triggering conditions are triggering conditions set based on the straight-line driving distance and turning angle of the intelligent guided vehicle.

[0138] In one optional scheme, the acquisition unit includes a calculation module and a projection module; the calculation module is used to calculate the relative pose transformation of the fused point cloud frame from the start time of scanning to the end time of scanning using the median integral method based on the above motion data; the projection module is used to project each point cloud of the fused point cloud frame in reverse according to the acquisition timestamp based on the above relative pose transformation onto the vehicle coordinate system of the intelligent guided vehicle at the end time of scanning, thereby obtaining the above distortion-free point cloud frame.

[0139] In one optional scheme, the second determining unit includes a construction module and a first determining module; the construction module is used to construct a global factor map of the intelligent guided vehicle based on the aforementioned key frame point cloud frame and the laser odometry factor, altitude prior factor, and loop closure constraint factor corresponding to the aforementioned key frame point cloud frame; the first determining module is used to determine the aforementioned drift correction matrix of the intelligent guided vehicle based on the aforementioned global factor map.

[0140] In one alternative approach, the first determining module includes an optimization submodule, which is used to perform incremental smoothing and global relinearization optimization on the global factor graph using an incremental nonlinear optimization algorithm to obtain an updated global factor graph, and to determine the drift correction matrix of the intelligent guided vehicle based on the updated global factor graph.

[0141] In an alternative embodiment, the second determining unit further includes a second determining module, used to determine the formula: The global pose of the aforementioned intelligent guided vehicle is determined, wherein, , This represents the globally optimal pose at the trigger moment. The above drift correction matrix, This refers to the local pose of the front end at the aforementioned trigger moment. The above local pose transformation matrix is... Given the global pose described above, the triggering time represents the moment when the keyframe triggering condition is met.

[0142] In one optional embodiment, the device further includes a preprocessing unit, which performs preprocessing operations on the point cloud data after acquiring the motion data collected by the IMU of the intelligent guided vehicle and the point cloud data collected by each lidar, to obtain preprocessed point cloud data. The preprocessing operations include voxel filtering and outlier removal.

[0143] The positioning device of the aforementioned intelligent guided vehicle includes a processor and a memory. The aforementioned acquisition unit, first determination unit, second determination unit, etc., are all stored as program units in the memory, and the processor executes the aforementioned program units stored in the memory to achieve the corresponding functions. All of the above modules are located in the same processor; or, the above modules are located in different processors in any combination.

[0144] The processor contains a kernel, which retrieves the corresponding program units from memory. One or more kernels can be configured, and adjusting kernel parameters can address the poor positioning accuracy issue in existing laser inertial SLAM systems for intelligent guided vehicles.

[0145] The memory may include non-permanent memory in computer-readable media, such as random access memory (RAM) and / or non-volatile memory, such as read-only memory (ROM) or flash RAM, and the memory includes at least one memory chip.

[0146] This invention provides a computer-readable storage medium including a stored program, wherein, when the program is executed, it controls the device containing the computer-readable storage medium to perform the positioning method of the intelligent guided vehicle.

[0147] Specifically, the positioning methods for intelligent guided vehicles include:

[0148] Step S201: Obtain motion data collected by the inertial measurement unit (IMU) of the intelligent guided vehicle and point cloud data collected by each lidar of the intelligent guided vehicle. Perform time synchronization and spatial extrinsic parameter alignment processing on the point cloud data to obtain a fused point cloud frame. Then, use the motion data to perform distortion correction processing on the fused point cloud frame to obtain a distortion-corrected point cloud frame.

[0149] Step S202: Construct an iterative error state Kalman filter based on Huber M-estimation, and use the iterative error state Kalman filter to determine the local pose transformation matrix of the intelligent guided vehicle based on the above-mentioned distorted point cloud frame.

[0150] Step S203: Determine the key frame point cloud frame in the above-mentioned distortion-reduced point cloud frame, and determine the drift correction matrix of the above-mentioned intelligent guided vehicle based on the above-mentioned key frame point cloud frame. Combine the above-mentioned drift correction matrix and the above-mentioned local pose transformation matrix through the Lie group synthesis mechanism to obtain the global pose of the above-mentioned intelligent guided vehicle, and determine the position of the above-mentioned intelligent guided vehicle based on the above-mentioned global pose.

[0151] This invention provides a processor for running a program, wherein the program executes the positioning method of the intelligent guided vehicle.

[0152] Specifically, the positioning methods for intelligent guided vehicles include:

[0153] Step S201: Obtain motion data collected by the inertial measurement unit (IMU) of the intelligent guided vehicle and point cloud data collected by each lidar of the intelligent guided vehicle. Perform time synchronization and spatial extrinsic parameter alignment processing on the point cloud data to obtain a fused point cloud frame. Then, use the motion data to perform distortion correction processing on the fused point cloud frame to obtain a distortion-corrected point cloud frame.

[0154] Step S202: Construct an iterative error state Kalman filter based on Huber M-estimation, and use the iterative error state Kalman filter to determine the local pose transformation matrix of the intelligent guided vehicle based on the above-mentioned distorted point cloud frame.

[0155] Step S203: Determine the key frame point cloud frame in the above-mentioned distortion-reduced point cloud frame, and determine the drift correction matrix of the above-mentioned intelligent guided vehicle based on the above-mentioned key frame point cloud frame. Combine the above-mentioned drift correction matrix and the above-mentioned local pose transformation matrix through the Lie group synthesis mechanism to obtain the global pose of the above-mentioned intelligent guided vehicle, and determine the position of the above-mentioned intelligent guided vehicle based on the above-mentioned global pose.

[0156] This invention provides an electronic device, which includes a processor, a memory, and a program stored in the memory and executable on the processor. When the processor executes the program, it performs at least the following steps:

[0157] Step S201: Obtain motion data collected by the inertial measurement unit (IMU) of the intelligent guided vehicle and point cloud data collected by each lidar of the intelligent guided vehicle. Perform time synchronization and spatial extrinsic parameter alignment processing on the point cloud data to obtain a fused point cloud frame. Then, use the motion data to perform distortion correction processing on the fused point cloud frame to obtain a distortion-corrected point cloud frame.

[0158] Step S202: Construct an iterative error state Kalman filter based on Huber M-estimation, and use the iterative error state Kalman filter to determine the local pose transformation matrix of the intelligent guided vehicle based on the above-mentioned distorted point cloud frame.

[0159] Step S203: Determine the key frame point cloud frame in the above-mentioned distortion-reduced point cloud frame, and determine the drift correction matrix of the above-mentioned intelligent guided vehicle based on the above-mentioned key frame point cloud frame. Combine the above-mentioned drift correction matrix and the above-mentioned local pose transformation matrix through the Lie group synthesis mechanism to obtain the global pose of the above-mentioned intelligent guided vehicle, and determine the position of the above-mentioned intelligent guided vehicle based on the above-mentioned global pose.

[0160] The devices mentioned in this article can be servers, PCs, tablets, mobile phones, etc.

[0161] This application also provides a computer program product, which, when executed on a data processing device, is suitable for executing an initialization program having at least the following method steps:

[0162] Step S201: Obtain motion data collected by the inertial measurement unit (IMU) of the intelligent guided vehicle and point cloud data collected by each lidar of the intelligent guided vehicle. Perform time synchronization and spatial extrinsic parameter alignment processing on the point cloud data to obtain a fused point cloud frame. Then, use the motion data to perform distortion correction processing on the fused point cloud frame to obtain a distortion-corrected point cloud frame.

[0163] Step S202: Construct an iterative error state Kalman filter based on Huber M-estimation, and use the iterative error state Kalman filter to determine the local pose transformation matrix of the intelligent guided vehicle based on the above-mentioned distorted point cloud frame.

[0164] Step S203: Determine the key frame point cloud frame in the above-mentioned distortion-reduced point cloud frame, and determine the drift correction matrix of the above-mentioned intelligent guided vehicle based on the above-mentioned key frame point cloud frame. Combine the above-mentioned drift correction matrix and the above-mentioned local pose transformation matrix through the Lie group synthesis mechanism to obtain the global pose of the above-mentioned intelligent guided vehicle, and determine the position of the above-mentioned intelligent guided vehicle based on the above-mentioned global pose.

[0165] It is obvious to those skilled in the art that the modules or steps of the present invention described above can be implemented using general-purpose computing devices. They can be centralized on a single computing device or distributed across a network of multiple computing devices. They can be implemented using computer-executable program code, and thus can be stored in a storage device for execution by a computing device. In some cases, the steps shown or described can be performed in a different order than those described herein, or they can be fabricated as separate integrated circuit modules, or multiple modules or steps can be fabricated as a single integrated circuit module. Thus, the present invention is not limited to any particular combination of hardware and software.

[0166] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0167] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0168] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0169] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0170] In a typical configuration, a computing device includes one or more processors (CPU), input / output interfaces, network interfaces, and memory.

[0171] Memory may include non-persistent memory in computer-readable media, such as random access memory (RAM) and / or non-volatile memory, such as read-only memory (ROM) or flash RAM. Memory is an example of computer-readable media.

[0172] Computer-readable media includes both permanent and non-permanent, removable and non-removable media that can store information by any method or technology. Information can be computer-readable instructions, data structures, modules of programs, or other data. Examples of computer storage media include, but are not limited to, phase-change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technologies, CD-ROM, digital versatile optical disc (DVD) or other optical storage, magnetic tape, magnetic disk storage or other magnetic storage devices, or any other non-transferable medium that can be used to store information accessible by a computing device. As defined herein, computer-readable media does not include transient computer-readable media, such as modulated data signals and carrier waves.

[0173] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0174] It should also be noted that the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus. Unless otherwise specified, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes that element.

[0175] The above description is merely a preferred embodiment of this application and is not intended to limit this application. Various modifications and variations can be made to this application by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the protection scope of this application.

Claims

1. A positioning method for an intelligent guided vehicle, characterized in that, include: The motion data collected by the inertial measurement unit (IMU) of the intelligent guided vehicle and the point cloud data collected by each lidar of the intelligent guided vehicle are acquired. The point cloud data are time-synchronized and spatial extrinsic parameter aligned to obtain a fused point cloud frame. The motion data is then used to perform distortion correction processing on the fused point cloud frame to obtain a distortion-corrected point cloud frame. An iterative error state Kalman filter based on Huber M-estimation is constructed, and the local pose transformation matrix of the intelligent guided vehicle is determined by the iterative error state Kalman filter according to the distortion-free point cloud frame. The process involves determining keyframe point cloud frames within the distorted point cloud frames, determining the drift correction matrix of the intelligent guided vehicle based on these keyframe point cloud frames, synthesizing the drift correction matrix and the local pose transformation matrix using a Lie group synthesis mechanism to obtain the global pose of the intelligent guided vehicle, and determining the position of the intelligent guided vehicle based on the global pose. Determining the keyframe point cloud frames within the distorted point cloud frames includes: acquiring keyframe trigger conditions, determining the keyframes of the intelligent guided vehicle based on these keyframe trigger conditions, and extracting the keyframe point cloud frames corresponding to the keyframes from the distorted point cloud frames. The keyframe trigger conditions are set based on the straight-line travel distance and turning angle of the intelligent guided vehicle.

2. The method according to claim 1, characterized in that, The motion data is used to perform distortion correction processing on the fused point cloud frame to obtain a distortion-corrected point cloud frame, including: Based on the motion data, the relative pose transformation of the fused point cloud frame from the start of the scan to the end of the scan is calculated using the median integral method. Based on the relative pose transformation, each point cloud of the fused point cloud frame is back-projected according to the acquisition timestamp to the vehicle coordinate system of the intelligent guided vehicle at the end of the scan, to obtain the distortion-free point cloud frame.

3. The method according to claim 1, characterized in that, Determining the drift correction matrix of the intelligent guided vehicle based on the keyframe point cloud frames includes: A global factor map of the intelligent guided vehicle is constructed based on the key frame point cloud frame and the laser odometry factor, altitude prior factor, and loop closure constraint factor corresponding to the key frame point cloud frame. The drift correction matrix of the intelligent guided vehicle is determined based on the global factor graph.

4. The method according to claim 3, characterized in that, Determining the drift correction matrix of the intelligent guided vehicle based on the global factor graph includes: The global factor graph is incrementally smoothed and globally relinearized using an incremental nonlinear optimization algorithm to obtain an updated global factor graph. The drift correction matrix of the intelligent guided vehicle is then determined based on the updated global factor graph.

5. The method according to claim 1, characterized in that, The global pose of the intelligent guided vehicle is obtained by synthesizing the drift correction matrix and the local pose transformation matrix using a Lie group synthesis mechanism, including: According to the formula: Determine the global pose of the intelligent guided vehicle, wherein, , This is the globally optimal pose at the trigger moment. The drift correction matrix is... This refers to the local pose of the front end at the triggering moment. Let be the local pose transformation matrix. The global pose is defined as the moment when the keyframe triggering condition is met.

6. The method according to claim 1, characterized in that, After acquiring motion data collected by the IMU of the intelligent guided vehicle and point cloud data collected by each lidar, the method further includes: The point cloud data is preprocessed to obtain preprocessed point cloud data, wherein the preprocessing operations include voxel filtering and outlier removal.

7. A positioning device for an intelligent guided vehicle, characterized in that, include: The acquisition unit is used to acquire motion data collected by the inertial measurement unit (IMU) of the intelligent guided vehicle and point cloud data collected by each lidar of the intelligent guided vehicle. The point cloud data is time-synchronized and spatial extrinsic parameter aligned to obtain a fused point cloud frame. The motion data is then used to perform distortion correction processing on the fused point cloud frame to obtain a distortion-corrected point cloud frame. The first determining unit is used to construct an iterative error state Kalman filter based on Huber M-estimation, and to determine the local pose transformation matrix of the intelligent guided vehicle using the iterative error state Kalman filter according to the distortion-free point cloud frame. The second determining unit is used to determine key frame point cloud frames in the distortion-free point cloud frames, and determine the drift correction matrix of the intelligent guided vehicle based on the key frame point cloud frames. The drift correction matrix and the local pose transformation matrix are synthesized using a Lie group synthesis mechanism to obtain the global pose of the intelligent guided vehicle, and the position of the intelligent guided vehicle is determined based on the global pose. Determining the key frame point cloud frames in the distortion-free point cloud frames includes: acquiring key frame triggering conditions, determining the key frames of the intelligent guided vehicle based on the key frame triggering conditions, and extracting the key frame point cloud frames corresponding to the key frames from the distortion-free point cloud frames. The key frame triggering conditions are triggering conditions set based on the straight-line travel distance and turning angle of the intelligent guided vehicle.

8. A computer-readable storage medium, characterized in that, The computer-readable storage medium includes a stored program, wherein, when the program is executed, it controls the device containing the computer-readable storage medium to perform the positioning method of the intelligent guided vehicle according to any one of claims 1 to 6.

9. An electronic device, characterized in that, include: One or more processors, a memory, and one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, the one or more programs including a positioning method for performing the intelligent guided vehicle according to any one of claims 1 to 6.

Images