Four-way vehicle high-precision positioning system based on multi-source information fusion

Abstract

The application discloses a four-way vehicle high-precision positioning system based on multi-source information fusion, and belongs to the technical field of logistics and storage. The positioning system comprises a data alignment module, an IMU timestamp is used for aligning multi-source data; a preprocessing reconstruction module, which is used for generating a de-distortion point cloud and a spatial line feature set; a point cloud registration module, which is used for global point cloud registration; a degradation quantization module, which is used for calculating visual and laser degradation factors according to image gradients, re-projection residuals and eigenvalues of a Hessian matrix; a covariance reshaping module, which is used for performing hierarchical adaptive reshaping on filter covariance based on double factors; a feedforward prediction module, which is used for estimating an intermediate state; a constraint construction and posterior correction module, which is used for constructing a non-motion-direction absolute zero-bias pseudo-measurement constraint equation to inject a filter to perform posterior correction. The application can effectively solve the problems of positioning drift and deviation under the degradation working condition of a sensor.

Description

Technical Field

[0001] This invention belongs to the field of logistics and warehousing technology, specifically relating to a four-way vehicle high-precision positioning system based on multi-source information fusion. Background Technology

[0002] With the rapid development of intelligent warehousing and modern logistics technologies, high-density automated warehouses (AS / RS) have been widely adopted due to their extremely high space utilization. Four-way shuttles (SLS), as the core handling robots in AS / RS, are capable of two-dimensional planar movement on crisscrossing rack tracks. To ensure the high-speed, safe, and accurate loading and unloading of goods by SLS in dense racking systems, the accuracy and robustness of its positioning system are crucial; the industry typically requires positioning accuracy to reach the millimeter level.

[0003] Currently, to overcome the limitations of single sensors, four-way vehicles generally adopt positioning schemes based on multi-source information fusion. Mainstream hardware configurations typically include LiDAR, vision cameras, inertial measurement units (IMUs), and wheeled servo encoders. At the underlying software architecture level, algorithms such as Extended Kalman Filter (EKF) or Factor Graph Optimization (FGO) are often used to fuse high-frequency local extrapolation data from the IMU and encoders with low-frequency global observation data from the LiDAR and vision cameras, thereby outputting state estimation results.

[0004] However, in real-world, high-density metal shelving environments, existing multi-source fusion localization algorithms face significant challenges. Specifically: First, the light blind spots of visual sensors degrade. There are often severe lighting blind spots deep inside automated shelving. Although this can be compensated for by built-in supplemental lighting, uneven lighting in extremely narrow aisles can cause local overexposure or complete darkness in the image, resulting in a large loss of visual odometry (VO) feature points based on traditional corner or edge extraction or serious mismatches.

[0005] Second, there are issues with the metallic reflection and geometric degradation of lidar. Dense shelving systems consist of numerous repetitive, smooth metal columns and beams. When a laser beam strikes the metal surface, it easily produces specular reflection and multipath effects, generating a large number of distorted points located inside the shelving in the point cloud. Simultaneously, this homogeneous, long, straight corridor-style metal structure causes geometric degradation in point cloud matching algorithms (such as NDT or ICP algorithms) along the longitudinal constraints parallel to the shelving, leading to slippage errors.

[0006] In actual operation, the two extreme conditions mentioned above often occur concurrently. Existing multi-source fusion algorithms typically use statically preset trust levels for data from different sensors, or simply discard data based on a basic residual threshold. When the four-way vehicle enters deep into the shelving, and both visual and laser data experience severe environmental degradation simultaneously, the static or sluggish covariance matrix cannot accurately map the error distribution of the current observations. This causes the underlying fusion filter to be unable to accurately determine which data to trust, leading to the introduction of a large amount of dirty data contaminated by metal reflections and visual mismatches into the state update equation, directly causing the filter estimator to diverge.

[0007] Once fusion divergence occurs, the system is forced to degrade, relying entirely on the IMU and wheel encoders for dead reckoning. However, in the absence of effective external observation calibration, the accumulated drift error will amplify rapidly in a very short time. Since the four-way vehicle travels close to an extremely narrow physical track, once the accumulated horizontal or lateral positioning error exceeds the track redundancy tolerance of a few millimeters, the system will deviate from its course and misjudge coordinates, which can easily lead to serious safety accidents such as the vehicle jamming or even the cargo falling from a great height.

[0008] Therefore, how to adaptively quantify the degree of degradation and dynamically schedule the fusion weights of multi-source data from the underlying fusion algorithm level under extreme working conditions of multiple sensors concurrently degrading deep in metal shelves, while suppressing pose drift during filtering divergence, completely solving the risk of falling caused by misjudgment of deviation, and achieving stable and reliable millimeter-level positioning is a core technical problem that urgently needs to be solved in this field. Summary of the Invention

[0009] To address the shortcomings of existing technologies, the present invention aims to provide a high-precision four-way vehicle positioning system based on multi-source information fusion, thus solving the problems in existing technologies.

[0010] The objective of this invention can be achieved through the following technical solutions: The four-way vehicle high-precision positioning method based on multi-source information fusion includes the following steps: The system acquires 3D point cloud data from the lidar, image sequences from the vision camera, and motion data from the inertial measurement unit, and performs alignment processing based on the hardware timestamp of the inertial measurement unit. The three-dimensional point cloud data is subjected to distortion reconstruction to generate a distortion-free local point cloud, and the image sequence is subjected to feature extraction to generate a spatial line feature set; The distortion-corrected local point cloud is then registered with the pre-constructed global point cloud map. The visual degradation factor is calculated based on the local pixel gradient distribution of the image sequence and the reprojection residual of the spatial line feature set; and the laser degradation factor is calculated based on the minimum eigenvalue of the approximate Hessian matrix in the point cloud registration process. Based on the visual degradation factor and the laser degradation factor, hierarchical adaptive reshaping is performed on the observation noise covariance matrix and process noise covariance matrix of the multi-source fusion filter; Based on the adaptively reshaped observation noise covariance matrix, the process noise covariance matrix, and the multi-source observation data execution time prediction and observation update, the intermediate state vector and intermediate covariance matrix estimated by the multi-source fusion filter are output. The driving axis is determined according to the current track scheduling instructions of the four-way vehicle. The rotation matrix from the world coordinate system to the local coordinate system of the vehicle body is extracted based on the intermediate state vector, and the absolute zero bias pseudo-measurement constraint equation for the non-motion direction under the local coordinate system of the vehicle body is constructed. The absolute zero-biased pseudo-measurement constraint equation is injected into the multi-source fusion filter as an absolute observation term. Based on the calculated constrained Kalman gain, posterior correction is performed on the intermediate state vector and the intermediate covariance matrix, and the pose estimation result after forced convergence is output.

[0011] Further, the step of reconstructing the distortion from the 3D point cloud data to generate a distortion-free local point cloud includes: Based on the set reflection intensity threshold, distorted reflective points are identified and extracted from the three-dimensional point cloud data; The effective point cloud around the distorted reflective point is used to fit a local reference plane, the centroid of the local reference plane is extracted as the local origin coordinates, and the surface unit normal vector of the local reference plane is extracted. Based on the local origin coordinates and the surface unit normal vector, the distorted reflective points are reverse-folded and projected through mirror geometric symmetry transformation to generate reconstructed three-dimensional coordinate points, which are then merged into the distortion-free local point cloud.

[0012] Furthermore, the formula for calculating the inverse folding projection of the distorted reflective points through mirror geometric symmetry transformation is as follows: in, The distorted reflective point, The coordinates of the local origin are... Let be the unit normal vector of the surface. Represents the dot product of vectors. These are the reconstructed 3D coordinate points.

[0013] Further, the step of extracting features from the image sequence to generate a spatial line feature set includes: Extract two-dimensional pixel line segments from the current image sequence and calculate the orientation angle of each line segment; Based on the topological orthogonal prior of the intersecting beams and columns of the metal shelf, oblique noise line segments whose direction angle deviates from the horizontal or vertical baseline by more than the angle deviation threshold are removed, and a filtered two-dimensional orthogonal line segment set is obtained. The two-dimensional orthogonal line segment set is inversely projected onto the three-dimensional camera coordinate system using the camera intrinsic parameter matrix, and parameterized into Plück coordinates to form the spatial line feature set.

[0014] Furthermore, the Plück coordinates of each spatial line in the spatial line feature set are defined as including the direction vector of the line and the moment vector from the origin to the line, and the inner product condition of the direction vector and the moment vector is set to strictly zero; the angle deviation threshold is set to ±5°.

[0015] Furthermore, the process of calculating the visual degradation factor and the laser degradation factor includes: The probability distribution of pixel gradient magnitudes within local regions of the image sequence is statistically analyzed to calculate local feature information entropy. The spatial line feature set is reprojected onto a two-dimensional image plane to calculate the average reprojection residual of the line features, and the visual degradation factor is calculated by combining the nonlinear mapping of the local feature information entropy. The minimum eigenvalue of the approximate Hessian matrix is ​​mapped to the laser degradation factor based on the negative exponential decay model.

[0016] Furthermore, hierarchical adaptive reshaping is performed on the observation noise covariance matrix and process noise covariance matrix of the multi-source fusion filter, including: The initial observation noise covariance matrix is ​​decoupled into a visual observation submatrix and a laser observation submatrix; The corresponding sub-matrices are exponentially independently expanded based on the visual degradation factor and the laser degradation factor, respectively, and a dynamic global observation noise covariance matrix is ​​generated by block diagonal splicing reconstruction. Determine whether the visual degradation factor and the laser degradation factor are greater than the preset visual safety threshold and laser safety threshold, respectively. When the concurrent degradation condition is met, perform extreme value shrinkage calculation on the initial process noise covariance matrix based on the product of the two.

[0017] Furthermore, the absolute zero-bias pseudo-measurement constraint equations for non-motion directions in the local coordinate system of the vehicle body are constructed, including: Define the pseudo-measurement vector in the non-motion direction as a strictly zero vector; Using the physical track constraint projection matrix and the rotation matrix, extract the vehicle's Y-axis velocity, Z-axis velocity, and Y-axis position offset from the intermediate state vector; By combining the pre-configured constrained noise covariance, a mapping relationship is constructed to constrain the estimated values ​​of the vehicle body's Y-axis velocity, Z-axis velocity, and Y-axis position offset to zero.

[0018] The four-way vehicle high-precision positioning system based on multi-source information fusion, which executes the above system, includes: The data alignment module is used to acquire the 3D point cloud data of the lidar, the image sequence of the vision camera, and the motion data of the inertial measurement unit, and perform alignment processing based on the hardware timestamp of the inertial measurement unit. The preprocessing and reconstruction module is used to perform distortion reconstruction on the three-dimensional point cloud data to generate a distortion-free local point cloud, and to extract features from the image sequence to generate a spatial line feature set. The point cloud registration module is used to register the distortion-free local point cloud with the pre-constructed global point cloud map. The degradation quantization module is used to calculate the visual degradation factor based on the local pixel gradient distribution of the image sequence and the reprojection residual of the spatial line feature set; and to calculate the laser degradation factor based on the minimum eigenvalue of the approximate Hessian matrix in the point cloud registration process. The covariance reshaping module is used to perform hierarchical adaptive reshaping of the observation noise covariance matrix and process noise covariance matrix of the multi-source fusion filter based on the visual degradation factor and the laser degradation factor. The feedforward prediction module is used to perform time prediction and observation update based on the adaptively reshaped observation noise covariance matrix, the process noise covariance matrix, and multi-source observation data, and output the intermediate state vector and intermediate covariance matrix estimated by the multi-source fusion filter. The constraint construction and posterior correction module is used to determine the driving axis according to the current track scheduling command of the four-way vehicle, extract the rotation matrix from the world coordinate system to the vehicle local coordinate system based on the intermediate state vector, and construct the absolute zero bias pseudo-measurement constraint equation for the non-motion direction in the vehicle local coordinate system; inject the absolute zero bias pseudo-measurement constraint equation as the absolute observation term into the multi-source fusion filter, perform posterior correction on the intermediate state vector and the intermediate covariance matrix based on the calculated constrained Kalman gain, and output the pose estimation result after forced convergence.

[0019] A computer storage medium storing a computer program, which, when executed by a processor, implements the steps of the four-way vehicle high-precision positioning method based on multi-source information fusion as described above.

[0020] The beneficial effects of this invention are: 1. This invention extracts a set of orthogonal line segments from an image and parameterizes them into spatial Plück coordinates. Simultaneously, it combines this with a local reference plane fitted to an effective point cloud, and then performs a reverse folding projection along the surface unit normal vector onto distorted reflective points that reach the reflection intensity threshold. This technical solution utilizes the structural topological prior and physical ray geometric symmetry of metal shelves, effectively preserving visual structural features in low-light environments and restoring reflective noise points that would otherwise be directly eliminated by traditional filtering algorithms into useful geometric contour points. This significantly improves the usable data density of the sensor under uneven lighting and strong reflection conditions, providing a more robust geometric constraint foundation for backend matching.

[0021] 2. This invention calculates structural information entropy by statistically analyzing the probability distribution of pixel gradient magnitudes within local image regions, and extracts the minimum eigenvalue of the approximate Hessian matrix during point cloud iterative registration. This technical solution no longer relies on external light sensing or simple residual fixed thresholds, but directly delves into the underlying algorithm, equating visual texture loss to the centralization of gradient probabilities, and equating geometric slippage caused by the homogenization of long straight corridor features to the convergence of eigenvalues ​​to zero. This quantization method can continuously and accurately reflect the degree of sensor degradation in a specific space, providing a reliable mathematical basis for the dynamic weight allocation of the fusion algorithm.

[0022] 3. This invention decouples the global observation noise covariance matrix of the filter and performs independent exponential expansion on its submatrices using the aforementioned degradation factor. Simultaneously, when the visual and laser sensors meet the concurrent degradation condition, the process noise covariance matrix undergoes extreme value contraction. This mechanism overcomes the limitations of statically setting the covariance in traditional fusion algorithms, adaptively adjusting the trust ratio between external observation data and internal pre-integrated extrapolated data when single-point or concurrent performance degradation occurs in different sensors. Through this hierarchical adaptive reshaping, contaminated observation data is effectively prevented from entering the state update equation, maintaining the filter's smooth transition and stable convergence under complex operating conditions.

[0023] 4. This invention extracts the rotation matrix in the vehicle coordinate system based on scheduling instructions, constructs a zero-bias pseudo-measurement constraint equation in the non-motion direction, and injects it as a high-confidence observation term into the filter to calculate the physically constrained Kalman gain, thereby performing posterior correction on the state vector. It cleverly introduces the physical constraint characteristics of the mechanical track into a nonlinear state space, effectively suppressing the position and velocity errors of the vehicle body in the lateral and vertical directions under conditions where the quality of external sensor data is significantly reduced and drift is easily generated by relying solely on inertial calculations. This ensures that the final output pose estimate meets the physical tolerance requirements of track operation, effectively reducing the risk of deviation and fall accidents. Attached Figure Description

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

[0025] Figure 1 This is a flowchart of the high-precision positioning process for four-way vehicles based on multi-source information fusion according to the present invention. Detailed Implementation

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

[0027] Example 1 like Figure 1 As shown, the four-way vehicle high-precision positioning method based on multi-source information fusion includes the following steps: S1, Low-level multi-source data anti-distortion preprocessing based on feature reconstruction and pre-integration; The system acquires raw 3D point cloud data from LiDAR, image sequences from a vision camera, and high-frequency motion data from an inertial measurement unit (IMU) and a wheel encoder, and performs time alignment based on the IMU's hardware timestamp. Based on the topological orthogonal prior of the intersecting beams and columns of the metal shelving, it extracts a set of filtered 2D orthogonal line segments from the image sequences and inversely projects them onto the 3D camera coordinate system using the camera intrinsic matrix and point cloud depth, parameterizing them into a spatial Plück coordinate feature set. Simultaneously, based on a set reflection intensity threshold, it identifies distorted reflective points in the raw 3D point cloud data and uses the surrounding effective point cloud to fit a local reference plane to extract the surface unit normal vector. Through a mirror geometric symmetry transformation, it inversely folds and projects the distorted reflective points along this surface unit normal vector, reconstructing a distortion-free local point cloud. Simultaneously, it performs Lie algebra integration on the high-frequency motion data based on the continuous inter-frame time difference, merging the results to generate motion pre-integrated data. This step aims to eliminate physical distortions caused by light blind spots and reflections from metallic mirrors, specifically including: S11: Acquisition of multi-source heterogeneous data and hard synchronization of timestamps; The system acquires raw 3D point cloud data from lidar, image sequences from a vision camera, and high-frequency motion data from an inertial measurement unit (IMU) and a wheel encoder. Based on the hardware timestamp of the IMU, the system performs linear interpolation timestamp alignment processing on the image sequences and raw 3D point cloud data.

[0028] S12: Visual line feature extraction and 3D parameterization based on orthogonal constraint prior; To address the issue of feature point mismatches in image sequences within a metal shelving environment, the LSD (Line Segment Detector) edge detection algorithm is used to extract two-dimensional pixel line segments from the current image frame. Based on the topological orthogonality prior of strictly horizontal and vertical intersections of the metal shelving beams and columns, the orientation angle of each two-dimensional pixel line segment is calculated, and segments with orientation angles deviating from the horizontal or vertical baseline by more than an angle deviation threshold are discarded (the preferred angle deviation threshold is...). The output is a set of filtered two-dimensional orthogonal line segments, representing the oblique noise line segments. The depth values ​​projected onto the periphery of the two-dimensional orthogonal line segment set from the original 3D point cloud data are extracted. Combined with the camera's intrinsic parameter matrix, the two-dimensional orthogonal line segment set is inversely projected onto the 3D camera coordinate system to construct a spatial straight line, and parameterized as a spatial Plück coordinate feature set. The Plück coordinates of each spatial line are defined as follows: Let be the direction vector of the line in this space. Let be the moment vector from the origin to the straight line in this space, and satisfy . .

[0029] S13: Laser point cloud reflection distortion identification and inverse reconstruction based on geometric plane fitting; Based on the set reflection intensity threshold Filter the original 3D point cloud data, selecting those with a reflection intensity greater than [missing value]. Points with a ranging depth greater than the average depth of their local neighborhood are identified as distorted reflective points. Using the non-reflective effective point cloud surrounding the distorted reflective points, a local reference plane for the current shelf metal surface is fitted in real time using the RANSAC (Random Sample Consensus) algorithm. The centroid of this local reference plane is then extracted as the local origin coordinate. The normal direction of this local reference plane is used as the unit normal vector of the metal column surface. ; By performing a mirror geometric symmetry transformation, the distorted reflective points are inversely folded and projected along the unit normal vector, outputting the reconstructed distortion-free 3D coordinate points. This is then combined to generate a distortion-free local point cloud. The underlying calculation formula for the inverse folding projection is: In the formula, It is a spatial distortion vector; Represents the vector dot product operation; reflection intensity threshold The value range is [0.85, 0.95] of the normalized intensity of the lidar.

[0030] S14: Motion pre-integration calculation based on inter-frame time difference Using the time difference between two adjacent frames of images or point clouds as the integration interval, based on the three-axis angular velocity and three-axis acceleration collected by the IMU, combined with the single-axis velocity output by the wheel encoder, the relative rotation, velocity and displacement changes of the four-way vehicle within the integration interval are calculated using the continuous-time integration equation on the Lie Algebra, and the combined output is the motion pre-integration data.

[0031] S2, Quantification of Concurrent Degradation Sensor Confidence Based on Underlying Data Matching Parameters The probability distribution of pixel gradient magnitudes within a local region of the current image sequence is statistically analyzed to calculate the local feature information entropy. The spatial Plück coordinate feature set is then reprojected onto a two-dimensional image plane using the camera intrinsic and extrinsic parameter matrices to calculate the average reprojection residual of the line features. Based on the joint nonlinear mapping of the local feature information entropy and the average reprojection residual of the line features, a visual degradation factor is calculated and output. Simultaneously, during the iterative registration of the distorted local point cloud with the global point cloud map, the Jacobian matrix of the registration objective function with respect to the state variables is calculated, and an approximate Hessian matrix is ​​constructed accordingly. Eigenvalue decomposition is performed on the Hessian matrix to extract its minimum eigenvalue. Based on the proximity of this minimum eigenvalue to zero, a laser degradation factor reflecting the state of missing longitudinal geometric constraints is calculated and output.

[0032] This step aims to quantify the degree of failure of different sensors under specific physical conditions, specifically including: S21: Entropy Quantization of Visual Structural Information Based on Local Gradient Probability Distribution of Images Extract pixel gradient magnitudes within the effective region of interest in the current image sequence, and statistically analyze the probability distribution of each pixel gradient magnitude within a preset gradient interval. Calculate the local feature information entropy H based on this distribution. In the formula, g is the number of pixel gradient magnitude levels after discretization; G is the set maximum gradient level; and p(g) is the proportion of pixels with a gradient magnitude of g. Simultaneously, the maximum feature information entropy calibrated at a well-lit and clear textured reference position during the offline initialization phase of the four-way vehicle is read. .

[0033] S22: Calculation of reprojection error of linear features based on Plück space manifold; The Plück coordinate feature set output in step S1 is obtained using the camera's intrinsic and extrinsic parameter matrices. Projecting back onto the 2D camera image plane yields the predicted 2D projected line; calculating the perpendicular distance from the endpoints of the actual extracted 2D orthogonal line segments in the image sequence to the corresponding predicted 2D projected line, and averaging the distances, yields the average reprojection residual of the line features. Combined with the local feature information entropy H, the visual degradation factor is calculated. : In the formula, the normalized weight coefficient and The preferred range is [0.4, 0.6], and 1.

[0034] S23: Laser geometric constraint degradation extraction based on point cloud matching Jacobian matrix; The distortion-corrected local point cloud output in step S1 is registered with the global point cloud map (e.g., using the NDT algorithm). During the iterative optimization process, the Jacobian matrix of the objective function with respect to the state variables is calculated. Constructing the Hessian matrix based on the Gauss-Newton approximation of the Jacobian matrix. ,in, The information weight matrix; for the Hessian matrix Perform eigenvalue decomposition to extract its minimum eigenvalue. The laser degradation factor, reflecting the longitudinal geometrical defect state, is calculated based on this minimum eigenvalue. : In the formula, the sensitivity adjustment coefficient The preferred range is [10, 50].

[0035] S3, based on a dual-dimensional degradation factor, underlying fusion covariance adaptive reshaping The initial global observation noise covariance matrix of the underlying fusion filter is decoupled into a visual observation submatrix and a lidar observation submatrix. Based on the calculated visual degradation factor and lidar degradation factor, the corresponding observation submatrix is ​​subjected to exponential independent expansion calculation. Then, a dynamic global observation noise covariance matrix is ​​generated by block diagonal splicing reconstruction. At the same time, it is determined whether the visual degradation factor and lidar degradation factor are greater than preset visual safety thresholds and lidar safety thresholds, respectively. When the concurrent degradation condition is met, the initial process noise covariance matrix of the filter is subjected to extreme value shrinkage operation based on the two-dimensional degradation factor, and the dynamically updated process noise covariance matrix is ​​output. This step prevents filter divergence by dynamically adjusting the trust weights of the filter for each sensor data point. Specifically, it includes the following steps: S31: Decoupling and independent expansion calculation of the observation noise covariance matrix of the bottom layer filter; Obtain the initial global observation noise covariance matrix pre-calibrated by the underlying multi-source fusion filter. And decouple it into a visual observation submatrix and lidar observation sub-matrix According to the results obtained in step S2 and To undergo independent exponential expansion: In the formula, and The dynamically updated observation submatrix; visual photometric penalty coefficient. and laser geometric penalty coefficient The preferred value range is [2, 5].

[0036] S32: Block diagonal reconstruction of the global observation noise covariance matrix; The two sub-matrices, after independent dilation and reshaping, are then concatenated diagonally to reconstruct a dynamic global observation noise covariance matrix for filter state updates. : S33: Process noise covariance extreme value shrinkage based on concurrent degradation judgment mechanism; Obtain the initial process noise covariance matrix Determine visual degradation factors Is it greater than the visual safety threshold? (Preferred range [0.6, 0.75]), and laser degradation factor Are they both greater than the laser safety threshold? (Preferred range [0.7, 0.85]); When this concurrent degradation condition is met, extreme value shrinkage of the process noise covariance is forcibly triggered: In the formula, The dynamically updated process noise covariance matrix; concurrency penalty coupling coefficient. The preferred range is [3, 6], where e is the natural constant. When this condition is not met, maintain... .

[0037] S4, Fusion pose estimation and precise localization with one-dimensional absolute physical trajectory constraints. The time prediction calculation of the fusion filter is performed based on the motion pre-integration data and the dynamically updated process noise covariance matrix. External observation updates are then performed using the dynamically updated global observation noise covariance matrix and external observation data to generate an intermediate state vector containing global three-dimensional position and velocity, as well as an intermediate covariance matrix. Subsequently, the driving axis of the four-way vehicle is determined according to the current track scheduling instructions. Based on the intermediate state vector, a rotation matrix from the world coordinate system to the vehicle's local coordinate system is extracted. The absolute physical characteristic of the four-way vehicle being restricted by physical metal tracks rather than having its direction of motion locked is transformed into an absolute zero-bias pseudo-measurement constraint equation mapped onto the vehicle's local coordinate system. Finally, this absolute zero-bias pseudo-measurement constraint equation is injected into the filter as an absolute observation term with high confidence. The physically constrained Kalman constraint gain is calculated, and the Kalman constraint gain is used to perform forced pose convergence updates on the intermediate state vector and intermediate covariance matrix, outputting millimeter-level six-degree-of-freedom high-precision pose data for controlling the precise driving of the four-way vehicle. Specifically, it includes the following steps: S41: Feedforward prediction and external observation update of multi-source fusion filter; Based on the motion pre-integration data obtained in step S1 and the process noise covariance matrix updated in step S3 Perform the time prediction step of Kalman filtering; combine the updated global observation noise covariance matrix from step S3. Based on actual observation data, perform external observation updates and output a global 3D position vector. and global three-dimensional velocity vector intermediate state vector and intermediate covariance matrix .

[0038] S42: Construction of the absolute zero bias pseudo-observation equation based on coordinate system rotation transformation; The vehicle's driving axis is determined based on the current track scheduling instructions; based on the intermediate state vector... Extracting the rotation matrix from the world coordinate system to the vehicle's local coordinate system Construct pseudo-measurement constraint equations mapped onto the vehicle's local coordinate system: In the formula, This is a pseudo-measurement vector in the non-motion direction; The orbital constraint noise is Gaussian distributed, and the corresponding constraint noise covariance matrix is... (The variance of its diagonal elements is preferably set to a minimum value) (in meters or meters per second). This is the projection matrix for physical trajectory constraints. When traveling along the X-axis of the vehicle body, Using rotation matrix Forcefully extract the vehicle's Y-axis velocity, Z-axis velocity, and Y-axis position offset from the state space, and set their true values ​​to zero.

[0039] S43: Low-level posterior state correction based on strong physical constraints; Inject the constraint equation as an absolute observation term and calculate the physically constrained Kalman constraint gain. This forces a convergence and pose update: In the formula, This is the posterior optimal state vector (i.e., the final millimeter-level six-DOF high-precision pose data). This is the posterior optimal covariance matrix. When environmental concurrency degradation leads to... When the maximum value is reached, the constraint noise covariance is minimized. Forced gain Approaching the upper limit, the algorithm absolutely trusts the physical trajectory constraints, completely eliminating the cumulative lateral and vertical drift errors.

[0040] Example 2 In this embodiment, a four-way vehicle high-precision positioning system based on multi-source information fusion is proposed, specifically including: The data alignment module is used to acquire the 3D point cloud data of the lidar, the image sequence of the vision camera, and the motion data of the inertial measurement unit, and perform alignment processing based on the hardware timestamp of the inertial measurement unit. The preprocessing and reconstruction module is used to perform distortion reconstruction on the three-dimensional point cloud data to generate a distortion-free local point cloud, and to extract features from the image sequence to generate a spatial line feature set. The point cloud registration module is used to register the distortion-free local point cloud with the pre-constructed global point cloud map. The degradation quantization module is used to calculate the visual degradation factor based on the local pixel gradient distribution of the image sequence and the reprojection residual of the spatial line feature set; and to calculate the laser degradation factor based on the minimum eigenvalue of the approximate Hessian matrix in the point cloud registration process. The covariance reshaping module is used to perform hierarchical adaptive reshaping of the observation noise covariance matrix and process noise covariance matrix of the multi-source fusion filter based on the visual degradation factor and the laser degradation factor. The feedforward prediction module is used to perform time prediction and observation update based on the adaptively reshaped observation noise covariance matrix, the process noise covariance matrix, and multi-source observation data, and output the intermediate state vector and intermediate covariance matrix estimated by the multi-source fusion filter. The constraint construction and posterior correction module is used to determine the driving axis according to the current track scheduling command of the four-way vehicle, extract the rotation matrix from the world coordinate system to the vehicle local coordinate system based on the intermediate state vector, and construct the absolute zero bias pseudo-measurement constraint equation for the non-motion direction in the vehicle local coordinate system; inject the absolute zero bias pseudo-measurement constraint equation as the absolute observation term into the multi-source fusion filter, perform posterior correction on the intermediate state vector and the intermediate covariance matrix based on the calculated constrained Kalman gain, and output the pose estimation result after forced convergence.

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

[0042] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely illustrative of the principles of the invention. Various changes and modifications can be made to the invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the claimed invention.

Claims

1. A high-precision four-way vehicle positioning method based on multi-source information fusion, characterized in that, Includes the following steps: The system acquires 3D point cloud data from the lidar, image sequences from the vision camera, and motion data from the inertial measurement unit, and performs alignment processing based on the hardware timestamp of the inertial measurement unit. The three-dimensional point cloud data is subjected to distortion reconstruction to generate a distortion-free local point cloud, and the image sequence is subjected to feature extraction to generate a spatial line feature set; The distortion-reduced local point cloud is then registered with the pre-constructed global point cloud map. The visual degradation factor is calculated based on the local pixel gradient distribution of the image sequence and the reprojection residual of the spatial line feature set. The laser degradation factor is calculated based on the minimum eigenvalue of the approximate Hessian matrix obtained during the point cloud registration process. Based on the visual degradation factor and the laser degradation factor, hierarchical adaptive reshaping is performed on the observation noise covariance matrix and process noise covariance matrix of the multi-source fusion filter; Based on the adaptively reshaped observation noise covariance matrix, the process noise covariance matrix, and the multi-source observation data execution time prediction and observation update, the intermediate state vector and intermediate covariance matrix estimated by the multi-source fusion filter are output. The driving axis is determined according to the current track scheduling instructions of the four-way vehicle. The rotation matrix from the world coordinate system to the local coordinate system of the vehicle body is extracted based on the intermediate state vector, and the absolute zero bias pseudo-measurement constraint equation for the non-motion direction under the local coordinate system of the vehicle body is constructed. The absolute zero-biased pseudo-measurement constraint equation is injected into the multi-source fusion filter as an absolute observation term. Based on the calculated constrained Kalman gain, posterior correction is performed on the intermediate state vector and the intermediate covariance matrix, and the pose estimation result after forced convergence is output.

2. The four-way vehicle high-precision positioning method based on multi-source information fusion according to claim 1, characterized in that, The steps of reconstructing the distortion from the 3D point cloud data to generate a distortion-free local point cloud include: Based on the set reflection intensity threshold, distorted reflective points are identified and extracted from the three-dimensional point cloud data; The effective point cloud around the distorted reflective point is used to fit a local reference plane, the centroid of the local reference plane is extracted as the local origin coordinates, and the surface unit normal vector of the local reference plane is extracted. Based on the local origin coordinates and the surface unit normal vector, the distorted reflective points are reverse-folded and projected through mirror geometric symmetry transformation to generate reconstructed three-dimensional coordinate points, which are then merged into the distortion-free local point cloud.

3. The four-way vehicle high-precision positioning method based on multi-source information fusion according to claim 2, characterized in that, The formula for calculating the distorted reflective points by reverse folding and projection through mirror geometric symmetry transformation is as follows: in, The distorted reflective point, The coordinates of the local origin are... Let be the unit normal vector of the surface. Represents the vector dot product. These are the reconstructed 3D coordinate points.

4. The four-way vehicle high-precision positioning method based on multi-source information fusion according to claim 1, characterized in that, The step of extracting features from the image sequence to generate a spatial line feature set includes: Extract two-dimensional pixel line segments from the current image sequence and calculate the orientation angle of each line segment; Based on the topological orthogonal prior of the intersecting beams and columns of the metal shelf, oblique noise line segments whose direction angle deviates from the horizontal or vertical baseline by more than the angle deviation threshold are removed, and a filtered two-dimensional orthogonal line segment set is obtained. The two-dimensional orthogonal line segment set is inversely projected onto the three-dimensional camera coordinate system using the camera intrinsic parameter matrix, and parameterized into Plück coordinates to form the spatial line feature set.

5. The four-way vehicle high-precision positioning method based on multi-source information fusion according to claim 4, characterized in that, The Plück coordinates of each spatial line in the spatial line feature set are defined as including the direction vector of the line and the moment vector from the origin to the line, and the inner product condition of the direction vector and the moment vector is set to strictly zero; the angle deviation threshold is set to ±5°.

6. The four-way vehicle high-precision positioning method based on multi-source information fusion according to claim 1, characterized in that, The process of calculating the visual degradation factor and the laser degradation factor includes: The probability distribution of pixel gradient magnitudes within local regions of the image sequence is statistically analyzed to calculate local feature information entropy. The spatial line feature set is reprojected onto a two-dimensional image plane to calculate the average reprojection residual of the line features, and the visual degradation factor is calculated by combining the nonlinear mapping of the local feature information entropy. The minimum eigenvalue of the approximate Hessian matrix is ​​mapped to the laser degradation factor based on the negative exponential decay model.

7. The four-way vehicle high-precision positioning method based on multi-source information fusion according to claim 6, characterized in that, Hierarchical adaptive reshaping is performed on the observation noise covariance matrix and process noise covariance matrix of the multi-source fusion filter, including: The initial observation noise covariance matrix is ​​decoupled into a visual observation submatrix and a laser observation submatrix; The corresponding sub-matrices are exponentially independently expanded based on the visual degradation factor and the laser degradation factor, respectively, and a dynamic global observation noise covariance matrix is ​​generated by block diagonal splicing reconstruction. Determine whether the visual degradation factor and the laser degradation factor are greater than the preset visual safety threshold and laser safety threshold, respectively. When the concurrent degradation condition is met, perform extreme value shrinkage calculation on the initial process noise covariance matrix based on the product of the two.

8. The four-way vehicle high-precision positioning method based on multi-source information fusion according to claim 1, characterized in that, Constructing the absolute zero-bias pseudo-measurement constraint equations in the non-motion directions of the vehicle body local coordinate system includes: Define the pseudo-measurement vector in the non-motion direction as a strictly zero vector; Using the physical track constraint projection matrix and the rotation matrix, extract the vehicle's Y-axis velocity, Z-axis velocity, and Y-axis position offset from the intermediate state vector; By combining the pre-configured constrained noise covariance, a mapping relationship is constructed to constrain the estimated values ​​of the vehicle body's Y-axis velocity, Z-axis velocity, and Y-axis position offset to zero.

9. A four-way vehicle high-precision positioning system based on multi-source information fusion, executing the system according to any one of claims 1-8, characterized in that it comprises: The data alignment module is used to acquire the 3D point cloud data of the lidar, the image sequence of the vision camera, and the motion data of the inertial measurement unit, and perform alignment processing based on the hardware timestamp of the inertial measurement unit. The preprocessing and reconstruction module is used to perform distortion reconstruction on the three-dimensional point cloud data to generate a distortion-free local point cloud, and to extract features from the image sequence to generate a spatial line feature set. The point cloud registration module is used to register the distortion-free local point cloud with the pre-constructed global point cloud map. The degradation quantization module is used to calculate the visual degradation factor based on the local pixel gradient distribution of the image sequence and the reprojection residual of the spatial line feature set. The laser degradation factor is calculated based on the minimum eigenvalue of the approximate Hessian matrix obtained during the point cloud registration process. The covariance reshaping module is used to perform hierarchical adaptive reshaping of the observation noise covariance matrix and process noise covariance matrix of the multi-source fusion filter based on the visual degradation factor and the laser degradation factor. The feedforward prediction module is used to perform time prediction and observation update based on the adaptively reshaped observation noise covariance matrix, the process noise covariance matrix, and multi-source observation data, and output the intermediate state vector and intermediate covariance matrix estimated by the multi-source fusion filter. The constraint construction and posterior correction module is used to determine the driving axis according to the current track scheduling command of the four-way vehicle, extract the rotation matrix from the world coordinate system to the vehicle local coordinate system based on the intermediate state vector, and construct the absolute zero bias pseudo-measurement constraint equation for the non-motion direction in the vehicle local coordinate system; inject the absolute zero bias pseudo-measurement constraint equation as the absolute observation term into the multi-source fusion filter, perform posterior correction on the intermediate state vector and the intermediate covariance matrix based on the calculated constrained Kalman gain, and output the pose estimation result after forced convergence.

10. A computer storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of the four-way vehicle high-precision positioning method based on multi-source information fusion as described in any one of claims 1-8.

Images