A vehicle positioning method and system

Abstract

The present application discloses a vehicle positioning method and system, relating to driverless driving: according to IMU data and lidar data, a global prior map is constructed using the fast-lio algorithm; according to the lidar data collected in real time, a local point cloud map is constructed; dynamic point clouds are identified and removed, and the remaining static point clouds are registered with the global prior map by NDT to obtain the lidar odometry pose; the shi-tomasi algorithm is used to extract feature points of camera data; and the Lucas-Kanade optical flow method is used to obtain the motion vectors of the feature points; the velocity of the feature points is calculated according to the motion vectors, and the dynamic feature points with velocities exceeding a preset threshold are removed to obtain the visual odometry pose; the position and heading of the vehicle are deduced by the CTRV model to obtain the dead reckoning result; and the unscented Kalman filter algorithm is used for multi-source heterogeneous data fusion. Aiming at the problems of dynamic environment interference and insufficient modeling of non-linear motion trajectories in traditional open pits and mines, the present application improves the positioning stability of driverless vehicles.

Description

Technical Field

[0001] This application relates to the field of driverless, and particularly to a vehicle positioning method and system. Background Art

[0002] With the intelligent development of underground and open-pit mining operations, the application of driverless technology in mining environments is becoming increasingly widespread. However, traditional positioning technologies face great challenges in such environments. The interior of mines often has complex terrain structures, variable obstacles, and highly dynamic environments, such as the movement of workers, the movement of equipment, and the activities of vehicles. These characteristics result in the ineffective coverage of satellite positioning signals such as GPS, and also pose a severe test for positioning methods based on environmental perception.

[0003] In a mine driverless system, precise positioning technology is the key to ensuring the safe and efficient operation of vehicles. Vehicles must accurately perceive their own positions and postures in complex terrains including long corridors, sharp turns, narrow roadways, intersections, etc. However, existing positioning technologies face two core problems:

[0004] First, the problem of dynamic environment interference seriously affects the positioning accuracy. Frequent dynamic objects (such as walking workers, other operating vehicles, etc.) in the mining environment can cause traditional laser SLAM technology to generate serious positioning errors due to misclassifying dynamic objects into the static environment model. Especially in narrow spaces, the interference of dynamic objects on environmental perception is particularly obvious, often resulting in registration failures and positioning jumps.

[0005] Second, the problem of insufficient non-linear motion trajectory modeling limits the improvement of positioning accuracy. Mine vehicles often need to perform complex motions such as turning, speed changing, and reversing during operation, while existing technologies mostly use simple pose transformation accumulation methods for dead reckoning, which cannot accurately describe the true trajectory of the vehicle in a non-linear motion state, resulting in a rapid increase in cumulative errors.

[0006] Existing mine vehicle positioning technologies usually adopt multi-sensor fusion schemes such as IMU, odometer, lidar, UWB, etc., but each has obvious limitations: odometer data comes from chassis information and has a large cumulative error problem; UWB has a high deployment cost and cannot guarantee real-time performance; laser SLAM is prone to false matching in dynamic environments. In addition, existing technologies still have deficiencies in solving the above two core problems:

[0007] In terms of dynamic object recognition, it mainly relies on a single modality (such as only based on lidar) to recognize dynamic objects, lacking a multi-modal complementary verification mechanism, resulting in incomplete or misrecognition of dynamic objects;

[0008] In terms of motion trajectory modeling, most solutions use simple pose transformation accumulation methods, which cannot accurately describe the non-linear motion characteristics of vehicles, especially in complex motion scenarios such as turning, where the accuracy drops significantly.

[0009] For example, the related patent document CN119124173B discloses a fusion positioning method and system for the dynamic environment of underground mines. The method includes steps such as establishing a prior map, obtaining real-time point cloud data, judging the dynamicity of the point cloud and removing dynamic points, and fusing IMU and chassis vehicle speed information. However, this solution only relies on lidar to identify dynamic objects and lacks the auxiliary verification of other modalities such as vision, resulting in limited recognition accuracy in complex dynamic environments; at the same time, a simple pose superposition method is used for odometer update, which cannot accurately describe the non-linear motion trajectory of the vehicle, and there is still much room for improvement in positioning accuracy and stability. Summary of the Invention

[0010] Aiming at the dynamic environment interference and insufficient non-linear motion trajectory modeling existing in traditional open pits and mines, the present application provides a vehicle positioning method and system, which improves the positioning stability of driverless vehicles in complex dynamic environments by using a lidar and vision collaborative perception strategy to identify and filter dynamic objects, and introducing a CVRT model to describe the non-linear motion trajectory of the vehicle, etc.

[0011] One aspect of the present application provides a vehicle positioning method for open pits and mines, including: collecting IMU data, lidar data, camera data, and vehicle chassis data; among them, the lidar and the vision camera provide perceptual information in different dimensions and complement each other. The lidar is good at accurate ranging and spatial structure perception, and the vision camera is good at texture recognition and feature extraction. The IMU provides high-frequency attitude change information to make up for the low sampling rate limitations of the lidar and the camera. The chassis data provides direct motion information and is not affected by environmental perception failure.

[0012] According to the IMU data and the lidar data, use the Fast-LIO (Fast LiDAR-Inertial Odometry) algorithm to construct a global prior map; among them, Fast-LIO uses the high-frequency data of the IMU to predict the motion state of the lidar point cloud. The Fast-LIO algorithm adopts an iterative Kalman filter framework, which can handle non-linear optimization problems, reduce the cumulative error, and improve the accuracy.

[0013] According to the lidar data collected in real time, a local point cloud map is constructed using a sliding window algorithm. The local point cloud map contains a point cloud set formed by merging the point cloud data of the nearest N frames. By comparing the current frame of point cloud with the local point cloud map, the temporal consistency of the point cloud is calculated, dynamic point clouds are identified and removed, and the remaining static point clouds are registered with the global prior map using NDT (Normal Distribution Transform) to obtain the lidar odometry pose. The shi-tomasi algorithm is used to extract the feature points of the camera data; and the Lucas-Kanade optical flow method is used to obtain the motion vectors of the feature points; the velocity of the feature points is calculated based on the motion vectors, and the dynamic feature points with velocities exceeding the preset threshold are removed, and the remaining static feature points are registered using ICP (Iterative Closest Point) to obtain the visual odometry pose.

[0014] Based on the vehicle speed in the vehicle chassis data, the angular velocity in the IMU data, and the system sampling time interval, the position and heading of the vehicle are estimated using the CTRV (Constant Turn Rate and Velocity) model to obtain the dead reckoning result; where the CTRV model assumes that the vehicle maintains a constant turning rate and linear velocity in a short period of time; it is particularly suitable for describing non-linear motions such as vehicle turning and is more accurate than the simple pose accumulation method. The unscented Kalman filter algorithm is used to perform multi-source heterogeneous data fusion on the lidar odometry pose, the visual odometry pose, and the dead reckoning result, and the global pose is output.

[0015] In particular, on the one hand, the lidar dynamic point processing and the visual dynamic feature processing form a dual verification mechanism. The two modalities use different principles to identify dynamic objects: the lidar is based on the change of 3D spatial position, and the vision is based on the optical flow in the image plane. When one modality fails in a specific environment (such as the lidar in a large flat area, or the camera in a low-light environment), the other modality can provide backup.

[0016] On the other hand, the lidar odometry pose based on the environmental structure, the visual odometry pose based on visual features, and the dead reckoning pose based on the kinematic model. These three pose estimations are based on different principles, independent of each other and mutually verifying.

[0017] In addition, the CTRV model breaks through the limitations of the traditional pose accumulation method. Different motion equations are used according to whether the angular velocity is zero, accurately describing the non-linear motion characteristics such as vehicle turning. The CTRV model requires accurate vehicle speed and angular velocity inputs, and the quality of these inputs is affected by the identification of dynamic objects. When the system can accurately distinguish the static environment and dynamic objects, the estimation of the vehicle's own motion will also be more accurate. Conversely, an accurate vehicle motion model also helps to better judge the dynamicity of objects in the environment.

[0018] Furthermore, the Fast-LIO algorithm is used to construct a global prior map, including calibrating the spatial coordinate relationships between the lidar and vehicle, the lidar and IMU, and the camera and IMU. Specifically, lidar and vehicle calibration establishes a rigid-body transformation between the perception data and the vehicle's motion reference frame, enabling all subsequent environmental perceptions to be accurately mapped to the vehicle's state. Lidar and IMU calibration, a prerequisite for the Fast-LIO algorithm, addresses the spatial alignment of two sensors based on different physical principles. Camera and IMU calibration establishes the geometric foundation for the fusion of visual and inertial data. These three levels of calibration are not independent operations; rather, they form a complete coordinate transformation chain, enabling the four heterogeneous sensor data (camera, lidar, IMU, and chassis) to work together within a unified geometric framework. Calibration accuracy directly determines the upper limit of all subsequent positioning calculations and is the primary guarantee of system accuracy.

[0019] PTP (Precision Time Protocol) is used to synchronize IMU and LiDAR data. In particular, IMUs typically sample at a high frequency (100-200 Hz), while LiDARs sample at a relatively low frequency (10-20 Hz). Even a few milliseconds of time deviation can lead to significant spatial inconsistencies when the vehicle is moving at high speed or turning. PTP, through its high-precision clock synchronization protocol, ensures strict temporal alignment of data from different sensors. Together with spatial calibration, this system forms a complete system for ensuring spatiotemporal consistency.

[0020] The Fast-LIO algorithm constructs a point cloud map based on time-synchronized IMU and LiDAR data. Specifically, the IMU provides an initial pose estimate and reduces the LiDAR registration search space, while the LiDAR point cloud provides geometric observation constraints to correct IMU cumulative errors. Unlike loosely coupled methods, Fast-LIO achieves fusion at the filter state estimation level, exploiting the complementary characteristics of IMU and LiDAR data to achieve a synergistic enhancement effect. By directly using the raw point cloud rather than extracting feature points, the algorithm preserves richer environmental information, laying the foundation for high-precision positioning.

[0021] The point cloud map is downsampled, radius filtered, and ground point cloud removed to obtain a processed point cloud map, which serves as the global prior map. The coordinate system of the global prior map is the map coordinate system. Downsampling improves computational efficiency while maintaining map structural features through voxel filtering; radius filtering removes unstable outliers to improve map consistency; and filtering the ground point cloud emphasizes vertical structural features, preventing interference from ground points on NDT registration.

[0022] Furthermore, a sliding window algorithm is used to construct a local point cloud map, including: setting the size of the time sliding window , Indicates the number of point cloud data frames included; at the current time step t, the sliding window contains arrive Continuous Frame point cloud data; the sliding window algorithm is essentially a spatiotemporal information processing technology that captures the dynamics of the environment by maintaining a fixed length of time series data: setting the window size is a key optimization parameter that must be precisely balanced based on vehicle speed, environmental dynamics, and computing resources. If the window is too small, the time span is insufficient to reliably detect dynamic objects; if the window is too large, it not only increases the computational burden but may also introduce outdated environmental information. In a mining environment, Usually, it is necessary to cover a time range of 3 to 5 seconds to effectively capture the motion characteristics of typical dynamic objects such as people and vehicles. Frame selection ( arrive ) ensures temporal continuity and avoids the "jumping" phenomenon of dynamic objects that may be caused by discrete sampling. This continuity is a prerequisite for subsequent dynamic object detection.

[0023] According to the current time step Frame point cloud data, construct a local point cloud map through coordinate transformation and point cloud merging Point cloud merging is more than a simple overlay. The system leverages established calibration relationships and previously estimated poses to accurately transform point clouds collected at different times into a common local coordinate system. The local point cloud map and the global prior map form a two-layer map representation architecture: the global map provides a long-term stable reference, while the local map captures recent environmental changes. This layered design balances positioning stability and environmental adaptability, and is a key strategy for robust positioning in complex and dynamic environments.

[0024] Specifically, LiDAR measures direct distances by emitting laser light, providing precise 3D geometry. However, it lacks texture information and suffers from geometric ambiguity on flat surfaces and in symmetrical environments. Vision cameras capture rich texture features through passive optical imaging, but lack direct depth information and exhibit unstable performance under varying lighting conditions. LiDAR can detect large dynamic objects through geometric changes in point clouds, while vision systems can identify small or distant dynamic objects through texture and optical flow. The combination of these two forms a multi-dimensional dynamic judgment mechanism, significantly improving the system's environmental perception capabilities. Serving as a bridge between LiDAR and vision systems, local point cloud maps support 3D positioning and dynamic verification of visual features by providing a spatial structural reference for the local environment, thereby promoting the deep fusion of multimodal sensors.

[0025] Further, obtaining the pose of the laser odometer includes: using the RANSAC algorithm to perform ground removal and downsampling on the local point cloud map to obtain the processed local point cloud map ; among them, ground removal not only reduces the data volume, but more importantly, eliminates the interference of ground points in the vertical direction, making the NDT registration more focused on the vertical structure features, significantly improving the registration accuracy and efficiency. For each point in the current frame point cloud , obtain the corresponding nearest neighbor point in the local point cloud map ; calculate the Euclidean distance between the point and the corresponding nearest neighbor point ; when the Euclidean distance is greater than the preset threshold , mark the corresponding point as a potential dynamic point; use the DBSCAN clustering algorithm to cluster the potential dynamic points, and classify the points with a spatial distance less than the threshold into the same object ; according to the positions of the object at different time frames, calculate the position change of the object and the corresponding time interval , and according to and , calculate the speed of the object , ; when the speed is greater than the preset threshold, mark all points in the corresponding object as dynamic points; remove all the marked dynamic points from the current frame point cloud to obtain the static lidar point cloud.

[0026] Perform NDT registration on the static lidar point cloud and the global prior map to obtain the pose of the laser odometer in the global coordinate system. Among them, different from the traditional ICP algorithm, NDT represents the point cloud as a series of normal distributions, and estimates the pose transformation by optimizing the matching degree between these distributions. This representation method based on probability distribution has stronger anti-noise ability and better convergence characteristics, and is especially suitable for the irregular features of the mining area environment.

[0027] In particular, the accurate removal of the aforementioned dynamic point cloud directly determines the accuracy of the NDT registration. If the dynamic points are not completely removed, they will introduce incorrect matches during the registration process, resulting in pose estimation deviation.

[0028] Further, obtaining the visual odometry pose includes: performing time consistency processing on camera data and lidar data; extracting feature points using the shi-tomasi algorithm based on the camera data after time consistency processing, where the feature points represent points in the image with a gradient change greater than a threshold; among them, different from simple edge detection, the Shi-Tomasi algorithm analyzes the eigenvalues of the image gradient covariance matrix and uses to judge the quality of feature points. This eigenvalue-based judgment ensures that the feature points have significant gradient changes in multiple directions, making these points easier to be reliably tracked between consecutive frames. This feature extraction method is particularly suitable for the complex texture environment in mining areas and can maintain high repeatability under different lighting conditions and viewing angle changes.

[0029] Constructing an optical flow constraint equation for the feature points in two adjacent frames; using the Lucas-Kanade optical flow method to solve the optical flow constraint equation to obtain the optical flow vector of each feature point ; calculating the motion vector of each feature point according to the optical flow vector ; when the motion vector of the feature point is greater than a preset threshold, marking the corresponding feature point as a dynamic feature point; When the motion vector of the feature point is greater than the preset threshold, the corresponding feature point is marked as a dynamic feature point;

[0030] Removing all marked dynamic feature points to obtain a set of static feature points; performing ICP registration on the set of static feature points to obtain the relative pose transformation between adjacent frames; accumulating the relative pose transformation and converting the accumulated pose transformation to the global map coordinate system to obtain the visual odometry pose.

[0031] Further, the optical flow constraint equation: , through Taylor expansion and small displacement assumption, we get: , where is the intensity value of the pixel point (x, y) in the image at time t, , are the gradients of the image in the x and y directions, It is the gradient of the image in the time t direction, u and v are the optical flows of the pixel point in the x and y directions of the image; represents the displacement of the pixel point in the x direction, represents the displacement of the pixel point in the y direction, represents the time interval between adjacent frames.

[0032] Specifically, the Taylor expansion of the optical flow constraint equation An underconstrained system (one equation with two unknowns) is constructed. The Lucas-Kanade method introduces an additional constraint of local optical flow constancy to establish an overconstrained system of equations within a small window of feature points and uses the least squares method to solve for the optimal optical flow vector. This locally optimized method is particularly suitable for handling small displacement scenarios and highly matches the characteristics of mining vehicles that usually operate stably at low speeds. At the same time, the system can handle larger displacement situations through pyramids (although not explicitly described in the description) to improve the adaptability of the algorithm.

[0033] Furthermore, the dead reckoning result is obtained, including: obtaining the pose of the laser odometer as the initial pose of the vehicle; extracting the vehicle speed v from the vehicle chassis data and the angular velocity w from the IMU data, and obtaining the system sampling time interval between adjacent data frames. ; calculating the vehicle position and heading change amount according to the CTRV model; accumulating the vehicle position and heading change amount to the initial pose to obtain the pose at the current moment; establishing the transformation relationship between the odometer coordinate system odom and the vehicle body coordinate system base_link; according to the transformation relationship, transforming the pose at the current moment from the odometer coordinate system odom to the global map coordinate system as the dead reckoning result.

[0034] On the one hand, traditional odometers usually adopt simple pose accumulation methods (such as linear extrapolation or constant velocity model), which perform well when the vehicle is moving in a straight line, but will produce significant cumulative errors in non-linear motion scenarios such as turning. The CTRV model constructs an accurate mathematical description suitable for curve motion by introducing the angular velocity as a key state variable.

[0035] The CTRV model uses different kinematic equations according to whether the angular velocity is zero. This piecewise processing method avoids the numerical instability problem when the angular velocity is close to zero. For the turning situation ( ), the position change is described by the circular arc motion equation; for the straight-line motion ( ), it degenerates into a linear motion equation. This adaptive mathematical structure enables the model to smoothly handle the transitions between various motion states.

[0036] The system first calculates the relative pose change in the odometer coordinate system odom, which is a local reference frame with the initial position as the origin; then, through the transformation relationship from odom to base_link, the pose representation is mapped to the vehicle body coordinate system, and this transformation takes into account the sensor installation position and vehicle geometric characteristics; finally, the pose is transformed from the local coordinate system to the global map coordinate system, realizing the position representation in a unified reference framework.

[0037] Furthermore, the dynamic object information identified and removed by this application directly improves the input data quality of the CTRV model. In traditional methods, the motion of dynamic objects in the environment can be misinterpreted as vehicle motion, leading to biased state estimation. This system significantly improves the reliability of the underlying motion data by removing dynamic objects a priori.

[0038] Conversely, the CTRV model's precise prediction of vehicle motion provides a more accurate motion reference for determining whether objects are dynamic or static. By comparing the motion of objects in the environment with the CTRV-predicted vehicle motion, the system can more reliably distinguish between real dynamic objects and "visual motion" caused by the vehicle's own motion.

[0039] Furthermore, the vehicle position and heading changes are calculated based on the CTRV model, including: when the angular velocity w is not zero, the vehicle position change is: ; ;Vehicle heading angle change : ; When the angular velocity When is zero, the vehicle position change is: ; ; ; where v is the vehicle speed, is the angular velocity, θ is the current heading angle, is the system sampling interval.

[0040] Specifically, the vehicle speed v comes from chassis sensors, usually provided by wheel encoders or the vehicle CAN bus; the angular velocity Measured by the IMU's gyroscope; sampling time interval These three data types have different physical properties and noise characteristics. The CTRV model organically combines them through the physical equations of motion to form a comprehensive estimate of the vehicle's motion state. The laser odometry pose is used as the initial pose and the starting point for motion integration, establishing a connection mechanism with the perception system. This initialization strategy avoids the long-term accumulation of cumulative errors and enables periodic state reset.

[0041] Furthermore, the global pose is output, including: constructing the state vector X, , where x, y are the position coordinates, θ is the heading angle, is the velocity component, is the angular velocity; specifically, different from the traditional three-dimensional state representation that only includes position and attitude, this state vector simultaneously models the kinematic state (position and heading) and dynamic state (linear velocity and angular velocity) of the vehicle. This high-dimensional state representation can not only describe where the vehicle is currently, but also express how the vehicle moves, providing a complete mathematical basis for non-linear trajectory prediction. The velocity components and enable the system to handle non-heading motions (such as skidding), which is particularly important in the complex road conditions of mines; The explicit modeling directly supports the accurate trajectory description under turning conditions.

[0042] Construct the state transition matrix A according to the CTRV motion model; according to the state transition matrix A, calculate the state prediction value according to the state transition equation where A is the state transition matrix, is the control input, B is the control input matrix, is the process noise; specifically, the state transition equation of the CTRV model captures the non-linear trajectory characteristics during the vehicle turning process, significantly improving the prediction accuracy compared with the linear model. The control input term in the state transition equation allows the system to combine vehicle control signals (such as driving force and steering angle), further improving the prediction accuracy. The process noise models the uncertainties in vehicle dynamics, such as factors like uneven road surfaces and tire skidding, making the prediction model robust to handle the complex mining environment. The state transition formula adopted by the system is actually a locally linearized representation of the non-linear CTRV model, which is achieved through first-order Taylor expansion or sigma-point sampling at each time step.

[0043] Obtain the observation data, which includes the pose of the laser odometer, the pose of the visual odometer, and the dead reckoning result; process the observation data according to the observation equation where H is the observation matrix, mapping the state vector to the sensor observation value, is the observation noise; calculate the Kalman gain where, is the prediction covariance matrix, R is the observation noise covariance matrix; H is the observation matrix; update the state estimate according to the Kalman gain K ; update the covariance matrix where I is the identity matrix; based on the updated state estimate, output the global pose including the position coordinates and the heading angle.

[0044] Specifically, when visual perception may be unstable in certain scenarios (such as sudden changes in lighting), the system will automatically increase the weights of the lidar and CTRV models; when the vehicle is turning at high speed, the system will rely more on the predictions of the CTRV model; when the static environment is rich, the system will rely more on the lidar NDT registration results. This intelligent data fusion strategy achieves deep collaboration between different modules.

[0045] Another aspect of the present application also provides a vehicle positioning system for implementing a vehicle positioning method of the present application.

[0046] Compared with the prior art, the advantages of the present application are as follows:

[0047] In underground coal mines and open-pit mining areas, due to the presence of a large number of mobile workers, operating vehicles and equipment, the environment exhibits highly dynamic characteristics. At the same time, the complex terrain structure in the mining area requires the vehicle to frequently perform non-linear motions such as turning and speed changing. However, the prior art often uses a single sensor modality to identify dynamic objects and a simple pose accumulation method to model the vehicle's motion trajectory, which has serious defects such as incomplete identification of dynamic objects and inaccurate description of the motion trajectory, resulting in low positioning accuracy and system instability. The present application uses a dual-modal collaborative perception strategy of lidar and vision cameras to identify and eliminate dynamic objects, introduces a CTRV model to accurately describe the non-linear motion characteristics of the vehicle, and combines the unscented Kalman filter to achieve multi-source heterogeneous data fusion, effectively solving the problem of high-precision positioning in complex dynamic environments in mining areas. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] The present application will be further described in the form of exemplary embodiments, and these exemplary embodiments will be described in detail through the drawings. These embodiments are not restrictive. In these embodiments, the same numbers represent the same structures, where:

[0049] Figure 1 is an exemplary flowchart of a vehicle positioning method shown according to some embodiments of the present application;

[0050] Figure 2 is a schematic diagram of spatio-temporal calibration shown according to some embodiments of the present application. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0051] The methods and systems provided in the embodiments of the present application will be described in detail below with reference to the drawings.

[0052] As Figure 1As shown in the figure, IMU data, lidar data, camera data, and vehicle chassis data are collected; according to the IMU data and lidar data, a global prior map is constructed using the fast-lio algorithm; according to the real-time collected lidar data, a local point cloud map is constructed using a sliding window algorithm, and the local point cloud map contains a point cloud set formed by merging the nearest N frames of point cloud data; by comparing the current frame of point cloud with the local point cloud map, the temporal consistency of the point cloud is calculated, dynamic point clouds are identified and removed, and the remaining static point clouds are registered with the global prior map using NDT to obtain the lidar odometry pose; the Shi-Tomasi algorithm is used to extract feature points from the camera data; and the Lucas-Kanade optical flow method is used to obtain the motion vectors of the feature points; the speed of the feature points is calculated based on the motion vectors, dynamic feature points with speeds exceeding a preset threshold are removed, and ICP registration is performed on the remaining static feature points to obtain the visual odometry pose; according to the vehicle speed in the vehicle chassis data, the angular velocity in the IMU data, and the system sampling time interval, the position and heading of the vehicle are estimated using the CTRV model to obtain the dead reckoning result; the unscented Kalman filter algorithm is used to perform multi-source heterogeneous data fusion on the lidar odometry pose, visual odometry pose, and dead reckoning result, and the global pose is output.

[0053] As Figure 2 shown, point cloud map construction: Calibrate the lidar with the vehicle body, the lidar with the imu, and the camera with the imu to obtain the spatial coordinate relationships among the three. The PTP time synchronization strategy is used to synchronize the time of the imu and the lidar. In the present invention, a domain controller, a camera, a lidar, and an IMU are used. The data of the sensors are collected and stored in the form of a bag file, and each data is input into the fast-lio algorithm for SLAM mapping to obtain a point cloud map in the pcd format. The point cloud map is subjected to downsampling, radius filtering, ground point cloud filtering, noise processing, etc. to obtain a point cloud map with higher accuracy as the prior map for positioning. The coordinate system of the point cloud map is the map coordinate system.

[0054] Dynamic point cloud detection is performed through a time sliding window: Set the size of a time sliding window , for example, set it to 10 frames of point cloud data (or an equivalent time period). At each time step t, the sliding window will contain the point cloud data.

[0055] Construct a local map: For each frame t, construct a local map from the first frames within the sliding window. The static point clouds in these frames can be used to compare with the current frame. The local map is constructed in the following way:

[0056] The The frame point clouds are merged into a larger static point cloud set. , which is a local map containing the static scene information of the recent few frames.

[0057] Ground removal and filtering: The merged point cloud is processed by RANSAC for ground removal and downsampling to reduce the computational load.

[0058] Dynamic point detection: By comparing the current frame and the local map to detect dynamic points. For each point in , the nearest neighbor point in is obtained, and the Euclidean distance is calculated: , if , the corresponding point is considered a dynamic point; represents a point in the current frame; represents a point in the local map; represents the dynamic point threshold, usually set according to the scene and the resolution of the point cloud.

[0059] Temporal consistency detection (dynamic point verification). To reduce the misjudgment of dynamic points (such as the short-term movement of static objects), the system uses temporal consistency detection. It is verified by judging whether a point shows similar dynamic behavior in multiple frames.

[0060] Suppose a set of dynamic points has been detected in the sliding window. In two frames at time t and t−1, if the same dynamic object maintains consistent dynamic changes (such as displacement and velocity), then the object can be judged as a dynamic object. The points in consecutive frames are clustered by the DBSCAN clustering algorithm to judge whether they are the same object. If two identical objects maintain a small spatial displacement in different frames, they can be judged as dynamic objects. , where V is the velocity vector of the object, is the time difference; if the velocity v of a point exceeds a certain threshold, the point is considered dynamic.

[0061] The dynamic point cloud detected through these steps can be used as the output, marking the points of the dynamic objects. The points of the dynamic objects are removed to obtain the real-time static lidar point cloud, which is registered with the prior pcdmap by NDT to output the pose of the lidar odometry in the global (map) coordinate system;

[0062] First, ensure that the image and point cloud data obtained from the camera or visual sensor are synchronized. Feature points are tracked by the optical flow method to detect and separate dynamic objects.

[0063] The optical flow method is based on the optical flow constraint equation, which describes the assumption of intensity invariance of points on the object surface in two consecutive frames of images. Assuming that the intensity of the points remains unchanged, the following equation is obtained: , where: is the intensity value of the pixel point in the image at time t . , is the displacement of the pixel point, is the time difference.

[0064] By performing Taylor expansion and assuming that the displacement of points on the object surface is small, the optical flow constraint equation can be obtained: , where: u and v are the optical flows of the pixel point in the x and y directions in the image (i.e., the displacement speed of the pixel), , are the gradients of the image in the x and y directions, is the gradient of the image in the time t direction

[0065] The steps to extract dynamic feature points using the optical flow method are as follows: Extract feature points. In each frame of the image, use the Shi-Tomasi corner detection to extract feature points. The extracted feature points are usually the points with large local variations in the image, and their movements need to be calculated in the subsequent optical flow estimation process.

[0066] Calculate the optical flow. Estimate the optical flow of the feature points through methods such as the pyramid Lucas-Kanade optical flow algorithm. This method calculates the movement (optical flow) of each feature point in the image between two consecutive frames. The specific steps are as follows:

[0067] a. Calculate the gradients of the image in the x and y directions ( and ).

[0068] b. Calculate the temporal gradient of the image ( ).

[0069] c. Use the optical flow constraint equation to estimate the displacement of each feature point between two frames. The formula for the Lucas-Kanade optical flow method is: , , is the optical flow vector (the speed of the pixel point in the x and y directions), is the temporal gradient.

[0070] Calculate the movement speed. After obtaining the optical flow vector of each feature point, the movement speed of these feature points can be calculated to determine whether they are dynamic objects: , the feature points of dynamic objects usually have a relatively high speed, while the feature points of the static background have a relatively small movement speed. Therefore, a threshold can be set to determine whether a feature point belongs to a dynamic object.

[0071] Screening and removing dynamic feature points, removing dynamic points according to the speed threshold: Set a speed threshold , if the movement speed of a feature point is greater than this threshold, it is considered that this point belongs to a dynamic object. If , mark the feature points with movement speed greater than the threshold as dynamic points and remove them from subsequent map optimization or point cloud processing. The static feature points obtained subsequently are subjected to inter-frame ICP registration to output the position under visual odometry, and it is converted to the global map coordinate system.

[0072] Obtain the initial pose through the pose obtained by lidar ndt, and listen to the vehicle speed information of the imu and the chassis respectively. When the angular velocity is not zero, the movement of the vehicle will generate a curved trajectory. Through the CTRV model calculation, the new position of the vehicle is calculated based on the vehicle speed, angular velocity, and heading angle.

[0073] When the angular velocity w is not zero, the vehicle position change is: ; ; The vehicle heading angle change : .

[0074] When the angular velocity is zero, the vehicle position change is: , ; ; where, v is the vehicle speed, is the angular velocity, θ is the current heading angle, is the system sampling time interval.

[0075] Establish the conversion relationship from the odometry coordinate system odom to the vehicle body coordinate system base_link;

[0076] According to the conversion relationship, convert the pose at the current moment from the odometry coordinate system odom to the global map coordinate system as the result of dead reckoning.

[0077] Use Kalman filter to combine data from different sensors (LIDAR, camera, IMU, odometry) to estimate the state of the system (such as position, speed, attitude, etc.). As an optimal estimator, Kalman filter can improve the accuracy and stability of the system by fusing data from multiple sensors.

[0078] Assume that the state of the robot includes the following quantities: x, y are positions (plane coordinates), θ is the attitude (such as heading angle), , is the speed (component in the plane), and is the angular velocity. The set state vector is: .

[0079] Prediction process

[0080] The state update equation of the system is: , where is the predicted next state, A is the state transition matrix, is the control input (such as speed, acceleration, etc.), B is the control input matrix, is the process noise.

[0081] The state transition matrix A is mainly set according to the kinematic model. For example, assuming the robot motion is based on a simple speed control model (acceleration can be ignored), then the state transition matrix is: , is the time step, and the control input is the speed and angular velocity provided by the odometer (imu and wheel speedometer). The prediction step obtains an estimate of the state, and this estimate is updated with the sensor observation data. The observation data of the three sensors are the camera, LIDAR, and odometer respectively. The information of the three obtained from the above steps is as follows: Odometer (odom): vehicle speed, angular velocity of imu. LIDAR: provides the global pose estimate of the vehicle. Camera: provides the relative position and heading angle of the robot through visual odometry.

[0082] Observation process, the observation model is: , are the observations of each sensor. H is the observation matrix, which maps the state vector to the observations of the sensors, is the observation noise.

[0083] Calculate the Kalman gain. The Kalman gain is an important parameter used to weight the sensor observations and predicted values in the update step. Its calculation formula is: , where: is the predicted covariance matrix, H is the observation matrix, and R is the observation noise covariance matrix.

[0084] Update the state estimate. The updated state estimate can be calculated by the following formula: , and the updated covariance matrix represents the estimation error. The update formula is as follows: , where I is the identity matrix.

[0085] The present invention and its implementation manners are schematically described above. This description is not restrictive. Without departing from the spirit or basic characteristics of the present application, the present application can be implemented in other specific forms. What is shown in the drawings is only one of the implementation manners of the present invention, and the actual structure is not limited thereto. Therefore, if those of ordinary skill in the art are inspired by it and, without departing from the purpose of the present invention, design similar structural manners and embodiments to the technical solution without creative efforts, they shall fall within the protection scope of the present application. In addition, the word "including" does not exclude other elements or steps, and the word "a" before an element does not exclude including "a plurality of" such elements. The plurality of elements stated in the product claims can also be implemented by one element through software or hardware. Words such as first and second are used to represent names and do not represent any specific order.

Claims

1. A vehicle positioning method for open-pit and mine, characterized in that, Including: Collecting IMU data, lidar data, camera data, and vehicle chassis data; Constructing a global prior map using the fast-lio algorithm based on the IMU data and lidar data; Constructing a local point cloud map using a sliding window algorithm based on the real-time collected lidar data, where the local point cloud map contains a point cloud set formed by merging the nearest N frames of point cloud data; By comparing the current frame point cloud with the local point cloud map, calculate the temporal consistency of the point cloud, identify and remove dynamic point clouds, and perform NDT registration on the remaining static point cloud and the global prior map to obtain the laser odometry pose, including: using the DBSCAN clustering algorithm to cluster potential dynamic points, and classifying points with a spatial distance less than a threshold into the same object according to the clustering result ; According to the object 's positions at different time frames, calculate the position change of the object and the corresponding time interval , and according to and and , calculate the velocity of the object , , ; When the velocity is greater than the preset threshold, mark all points in the corresponding object as dynamic points; Extracting feature points of the camera data using the shi-tomasi algorithm; and obtaining the motion vectors of the feature points using the Lucas-Kanade optical flow method; Calculating the velocities of the feature points according to the motion vectors, removing dynamic feature points whose velocities exceed a preset threshold, and performing ICP registration on the remaining static feature points to obtain the visual odometry pose; Obtain the pose of the lidar odometer as the initial pose of the vehicle; extract the vehicle speed v from the vehicle chassis data, extract the angular velocity w from the IMU data, and obtain the system sampling time interval between adjacent data frames ; Calculating the vehicle position and heading change amount according to the CTRV model, including: when the angular velocity w is not zero, the vehicle position change amount is: ; ; Vehicle heading angle change : ; When the angular velocity of rotation is zero, the vehicle position change is: ; ; ; where v is the vehicle speed, is the angular velocity of rotation, is the current heading angle, is the system sampling time interval; Accumulating the vehicle position and heading change amount to the initial pose to obtain the pose at the current moment; establishing the transformation relationship from the odometry coordinate system odom to the vehicle body coordinate system base_link; according to the transformation relationship, transforming the pose at the current moment from the odometry coordinate system odom to the global map coordinate system as the dead reckoning result; Performing multi-source heterogeneous data fusion on the lidar odometry pose, visual odometry pose, and dead reckoning result using the unscented Kalman filter algorithm, and outputting the global pose.

2. The vehicle positioning method according to claim 1, wherein: Constructing a global prior map using the fast-lio algorithm includes: Calibrating the spatial coordinate relationships between the lidar and the vehicle body, between the lidar and the IMU, and between the camera and the IMU; Synchronizing the IMU data and lidar data in time using the PTP time synchronization strategy; Constructing a point cloud map using the fast-lio algorithm based on the time-synchronized IMU data and lidar data; Performing downsampling, radius filtering, and removing ground point clouds on the point cloud map to obtain the processed point cloud map as the global prior map; wherein, the coordinate system of the global prior map is the map coordinate system.

3. The vehicle positioning method according to claim 1, wherein: Constructing a local point cloud map using a sliding window algorithm based on the real-time collected lidar data includes: Set the size of the time sliding window , indicating the number of frames of point cloud data included; At the current time step t, the sliding window contains consecutive to frame point cloud data;​ According to the frame point cloud data at the current time step, a local point cloud map is constructed through coordinate transformation and point cloud merging 。 4. The vehicle positioning method according to claim 1, wherein: Obtaining the lidar odometry pose includes: Use the RANSAC algorithm for the local point cloud map Perform ground removal and downsampling processing to obtain the processed local point cloud map ; For each point in the current frame point cloud , retrieve the corresponding nearest neighbor point in the local point cloud map ; ; Calculation point and the corresponding nearest neighbor point the Euclidean distance between ; When the Euclidean distance is greater than a preset threshold , the corresponding point is marked as a potential dynamic point; Remove all the marked dynamic points from the current frame of point cloud to obtain the static lidar point cloud; Performing NDT registration on the static lidar point cloud and the global prior map to obtain the lidar odometry pose in the global coordinate system.

5. The vehicle positioning method according to claim 1, wherein: Extracting feature points of the camera data using the shi-tomasi algorithm; and obtaining the motion vectors of the feature points using the Lucas-Kanade optical flow method; Calculating the velocities of the feature points according to the motion vectors, removing dynamic feature points whose velocities exceed a preset threshold, and performing ICP registration on the remaining static feature points to obtain the visual odometry pose, including: Performing time consistency processing on the camera data and lidar data; Extracting feature points using the shi-tomasi algorithm according to the time-consistent processed camera data, where the feature points represent points in the image with a gradient change greater than the threshold; Construct the optical flow constraint equation for feature points in two adjacent frames of images; Solve the optical flow constraint equation using the Lucas-Kanade optical flow method to obtain the optical flow vector of each feature point ; According to the optical flow vector , calculate the motion vector of each feature point ; When the motion vector of the feature point is greater than the preset threshold, mark the corresponding feature point as a dynamic feature point; Eliminate all marked dynamic feature points to obtain a set of static feature points; Perform ICP registration on the set of static feature points to obtain the relative pose transformation between adjacent frames; Accumulate the relative pose transformation and convert the accumulated pose transformation to the global map coordinate system to obtain the visual odometry pose.

6. The vehicle positioning method according to claim 5, wherein: Optical flow constraint equation: ; Through Taylor expansion and the small displacement assumption, we get: ; Among them, is the intensity value of the pixel point in the image at time t, , are the gradients of the image in the x and y directions, is the gradient of the image in the time t direction, and u, v are the optical flows of the pixel points in the image in the x and y directions; represents the displacement of the pixel point in the x direction, represents the displacement of the pixel point in the y direction, represents the time interval between adjacent frames.

7. The vehicle positioning method according to claim 1, wherein: Output the global pose, including: Construct the state vector X, , where x and y are position coordinates, and θ is the heading angle, is the velocity component, is the angular velocity; Construct the state transition matrix A according to the CTRV motion model; According to the state transition matrix A and the state transition equation calculate the state prediction value, where A is the state transition matrix, is the control input, B is the control input matrix, is the process noise; Obtain the observation data, where the observation data includes the laser odometry pose, the visual odometry pose, and the dead reckoning result; According to the observation equation Process the observation data, where H is the observation matrix that maps the state vector to the sensor observations, is the observation noise; Calculate the Kalman gain , where is the predicted covariance matrix, R is the observation noise covariance matrix; H is the observation matrix; Update the state estimate according to the Kalman gain K ; Update the covariance matrix , where I is the identity matrix; Based on the updated state estimate, output the global pose including the position coordinates and the heading angle.

8. A vehicle positioning system, characterized in that, Including: At least one processing unit; configured to execute instructions to implement the vehicle positioning method according to any one of claims 1 to 7.

Images