Vehicle positioning method and system

Abstract

The invention discloses a vehicle positioning method and system, and relates to unmanned driving. According to IMU data and laser radar data, a fast-lio algorithm is used to construct a global prior map; constructing a local point cloud map according to laser radar data collected in real time; recognizing and rejecting dynamic point clouds, and carrying out NDT registration on residual static point clouds and the global prior map to obtain a laser odometer pose; extracting feature points of the camera data by adopting a shii-tomasi algorithm; a Lucas-Kanade optical flow method is used to acquire motion vectors of the feature points; calculating the speed of the feature points according to the motion vector, removing the dynamic feature points with the speed exceeding a preset threshold value, and obtaining the pose of the visual odometer; reckoning the position and course of the vehicle through a CTRV model to obtain a track reckoning result; and carrying out multi-source heterogeneous data fusion by adopting a non-destructive Kalman filtering algorithm. Aiming at the defects of dynamic environment interference and non-linear motion trail modeling existing in traditional open air and mines, the positioning stability of the unmanned vehicle is improved.

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, driverless technology is increasingly widely used in mining environments. 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 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] Firstly, the problem of dynamic environment interference seriously affects 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] Secondly, 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. However, 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 a dynamic environment. 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 dynamic environments in underground coal 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, lacks auxiliary verification of other modalities such as vision, and has limited recognition accuracy in complex dynamic environments; at the same time, it uses a simple pose superposition method 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 problems of dynamic environment interference and insufficient non-linear motion trajectory modeling in traditional open pits and mines, this application provides a vehicle positioning method and system. 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., the positioning stability of driverless vehicles in complex dynamic environments is improved.

[0011] One aspect of this 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, lidar and vision cameras provide perception information in different dimensions and complement each other. Lidar is good at accurate ranging and spatial structure perception, and vision cameras are good at texture recognition and feature extraction. IMU provides high-frequency attitude change information to make up for the low sampling rate limitations of lidar and cameras. Chassis data provides direct motion information and is not affected by environmental perception failure.

[0012] According to the IMU data and 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 cumulative errors, and improve 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 according to 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] 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 (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 fuse the lidar odometry pose, the visual odometry pose, and the dead reckoning result of multi-source heterogeneous data to output the global pose.

[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 of 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 verified.

[0017] In addition, the CTRV model breaks through the limitations of the traditional pose accumulation method, adopts different motion equations according to whether the angular velocity is zero, and accurately describes 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, a global prior map is constructed using the fast-lio algorithm, including: calibrating the spatial coordinate relationships between the lidar and the vehicle body, the lidar and the IMU, and the camera and the IMU; In particular, the calibration between the lidar and the vehicle body establishes a rigid body transformation between the perception data and the vehicle motion reference system, enabling all subsequent environmental perceptions to be accurately mapped to the vehicle state; The calibration between the lidar and the IMU is a prerequisite for the Fast-LIO algorithm, which solves the spatial alignment problem of two sensors with different physical principles; The calibration between the camera and the IMU establishes the geometric basis for the fusion of vision and inertial data. These three layers of calibration are not independent operations, but rather constitute a complete coordinate transformation chain, enabling the data of four heterogeneous sensors (camera, lidar, IMU, chassis) to work together under a unified geometric framework. The calibration accuracy directly determines the upper limit of all subsequent positioning calculations and is the primary guarantee for the system accuracy.

[0019] The Precision Time Protocol (PTP) time synchronization strategy is used to synchronize the IMU data and the lidar data in terms of time; In particular, the IMU usually samples at a high frequency (100 - 200Hz) while the lidar is relatively low frequency (10 - 20Hz). When the vehicle is moving at high speed or turning, even a time deviation of a few milliseconds can lead to serious spatial inconsistencies. PTP ensures the strict alignment of different sensor data in the time dimension through a high-precision clock synchronization protocol, and together with the spatial calibration, it constitutes a complete spatio-temporal consistency guarantee system.

[0020] Based on the time-synchronized IMU data and lidar data, a point cloud map is constructed using the fast-lio algorithm; In particular, the IMU provides an initial pose estimate and reduces the lidar registration search space, while the laser point cloud provides geometric observation constraints to correct the IMU cumulative error. Different from the loose coupling method, Fast-LIO realizes fusion at the filter state estimation level, processes the complementary characteristics of the IMU and laser data, and forms a synergistic enhancement effect. The algorithm directly uses the original point cloud instead of extracting feature points, retaining richer environmental information and laying a foundation for high-precision positioning.

[0021] The point cloud map is downsampled, radius filtered, and ground point clouds are removed to obtain a processed point cloud map as the global prior map; Among them, the coordinate system of the global prior map is the map coordinate system. Among them, downsampling improves the calculation efficiency while maintaining the map structure characteristics through voxel filtering; Radius filtering removes unstable outliers to improve the map consistency; Removing the ground point cloud highlights the vertical structure characteristics and avoids the interference of ground points on NDT registration.

[0022] Furthermore, a local point cloud map is constructed using a sliding window algorithm, 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 time continuity and avoids the "jump" 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, build local point cloud map through coordinate transformation and point cloud merging ; Among them, point cloud merging is not a simple superposition. The system needs to use the established calibration relationship and the previously estimated pose to accurately transform the 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; 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 dynamic environments.

[0024] In particular, LiDAR measures direct distance by emitting lasers and provides accurate 3D geometry, but lacks texture information and has geometric ambiguity on flat surfaces and symmetrical environments; visual cameras capture rich texture features through passive optical imaging, but lack direct depth information and are unstable under changing lighting. LiDAR can detect large dynamic objects through geometric changes in point clouds; visual systems can identify small or distant dynamic objects through texture and optical flow. The combination of the two forms a multi-dimensional dynamic judgment mechanism, which significantly improves the system's environmental perception capabilities. As a bridge connecting LiDAR and the visual system, the local point cloud map provides a spatial structure reference for the local environment, supports 3D positioning and dynamic verification of visual features, and promotes 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 of 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 group 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 of 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 perspective changes.

[0029] Constructing an optical flow constraint equation for 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; 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.

[0030] Further, the optical flow constraint equation:

[0031] Through Taylor expansion and the small displacement assumption, we get: where is the intensity value of the pixel point (x, y) in the image at time t, is the gradient of the image in the x direction, , is the gradient of the image in the y direction, 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 is 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 method based on local optimization is particularly suitable for handling small displacement scenarios, highly matching the characteristics of the vehicles in the mining area that usually run at a low speed and stably. At the same time, the system can handle larger displacement situations through the pyramid implementation (although not explicitly described in the description), improving the adaptability of the algorithm.

[0033] Furthermore, the dead reckoning result is obtained, including: obtaining the pose of the lidar odometer as the initial pose of the vehicle; extracting the vehicle speed v from the vehicle chassis data, extracting 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 curved 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 the 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] On the other hand, the dynamic object information identified and eliminated by this application directly improves the quality of the input data of the CTRV model. In traditional methods, the movement of dynamic objects in the environment may be misjudged as the movement of the vehicle itself, resulting in state estimation errors. This system significantly improves the reliability of the underlying motion data through prior dynamic object elimination.

[0038] Conversely, the accurate prediction of vehicle movement by the CTRV model provides a more accurate motion reference benchmark for dynamic / static object determination. The system can more reliably distinguish real dynamic objects and "visual motion" caused by the movement of the vehicle itself by comparing the movement of objects in the environment with the vehicle movement predicted by the CTRV.

[0039] Furthermore, calculate 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: ; ; the vehicle heading angle change amount : ; when the angular velocity is zero, the vehicle position change amount is: ; ; ; where, v is the vehicle speed, is the angular velocity, θ is the current heading angle, is the system sampling time interval.

[0040] Specifically, the vehicle speed v comes from the chassis sensor, usually provided by the wheel encoder or the vehicle CAN bus; the angular velocity is measured by the gyroscope of the IMU; the sampling time interval is calculated by the high-precision clock. These three kinds of data have different physical characteristics and noise characteristics. The CTRV model organically combines them through physical motion equations to form a comprehensive estimation of the vehicle motion state. Using the pose of the laser odometer as the initial pose and taking it as the starting point of motion integration, a connection mechanism with the perception system is established. This initialization strategy avoids the long-term accumulation of cumulative errors and realizes periodic state reset.

[0041] Furthermore, output the global pose, including: construct the state vector X, , where, x, y are the position coordinates, θ is the heading angle, is the speed 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; 's 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 slippage, making the prediction model robust to handle complex mining environments. The state transition formula adopted by the system is actually a local linearization 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, where the observation data 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 realizes deep cooperation 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 and open-pit mining areas, due to the presence of a large number of mobile workers, operating vehicles and equipment, the environment shows highly dynamic characteristics. At the same time, the complex terrain structure in the mining area requires the vehicle to frequently perform non-linear movements such as turning and speed change. 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 camera to identify and eliminate dynamic objects, introduces a CTRV model to accurately describe the non-linear motion characteristics of the vehicle, and combines an 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 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, IMU data, LiDAR data, camera data and vehicle chassis data are collected; a global prior map is constructed using the fast-lio algorithm based on the IMU data and LiDAR data; a local point cloud map is constructed using a sliding window algorithm based on the real-time collected LiDAR data, and the local point cloud map contains a point cloud set formed by merging the point cloud data of the most recent N frames; by comparing the current frame point cloud with the local point cloud map, the temporal consistency of the point cloud is calculated, the dynamic point cloud is identified and removed, and the remaining static point cloud is NDT-aligned with the global prior map to obtain the laser odometer posture; the shi-tomasi algorithm is used Extract feature points from camera data; and use the Lucas-Kanade optical flow method to obtain the motion vector of the feature point; calculate the speed of the feature point based on the motion vector, eliminate dynamic feature points whose speed exceeds a preset threshold, perform ICP registration on the remaining static feature points, and obtain the visual odometer pose; calculate the vehicle position and heading through the CTRV model based on the vehicle speed in the chassis data, the angular velocity in the IMU data, and the system sampling time interval, and obtain the track calculation result; use the lossless Kalman filtering algorithm to perform multi-source heterogeneous data fusion on the laser odometer pose, visual odometer pose, and track calculation results, and output the global pose.

[0053] like Figure 2 As shown, the point cloud map is constructed by calibrating the laser radar and the vehicle body, the laser radar and the imu, and the camera and the imu to obtain the spatial coordinate relationship between the three. The imu and the laser radar are time synchronized using the ptp time synchronization strategy. The domain controller, camera, lidar and IMU are used in the present invention. The sensor data is collected and stored in the form of a bag package, and each data is input into the fast-lio algorithm for slam mapping to obtain a point cloud map in pcd format. The point cloud map is processed by downsampling, radius filtering, ground point cloud filtering, noise processing, etc. to obtain a point cloud map with higher accuracy as a priori map for positioning. The coordinate system of the point cloud map is the map coordinate system.

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

[0055] Construct a local map: For each frame t, from the previous Frames build a local map. 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 sliding window 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 several frames.

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

[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, which is 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 transient movement of static objects), the system uses temporal consistency detection. Verification is performed by judging whether a point exhibits 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, marked as the points of dynamic objects. The points of 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 images 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 of 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 the feature point is greater than this threshold, it is considered that this point belongs to a dynamic object. If , mark the feature points with a movement speed greater than the threshold as dynamic points and remove them from subsequent map optimization or point cloud processing. The subsequent obtained static feature points are subjected to ICP registration between frames 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, respectively listen to the vehicle speed information of the imu and the chassis. 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 transformation relationship from the odometry coordinate system odom to the vehicle body coordinate system base_link;

[0076] According to the transformation relationship, transform 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), 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 that 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): the speed of the vehicle, the angular velocity of the 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 above has schematically described the present invention and its implementation manners. 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 term "including" does not exclude other elements or steps, and the term "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. The terms such as "first" and "second" are used to indicate 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 point cloud data of the nearest N frames; Calculating the temporal consistency of the point cloud by comparing the current frame of point cloud with the local point cloud map, identifying and removing dynamic point clouds, and performing NDT registration on the remaining static point clouds with the global prior map to obtain the lidar odometry pose; 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 based on 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; Calculating the position and heading of the vehicle through the CTRV model based on the vehicle speed in the vehicle chassis data, the angular velocity in the IMU data, and the system sampling time interval to obtain 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, characterized in that: Constructing a global prior map using the fast-lio algorithm, including: 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 time of the IMU data and the lidar data 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; where the coordinate system of the global prior map is the map coordinate system.

3. The vehicle positioning method according to claim 1, characterized in that: Constructing a local point cloud map using a sliding window algorithm based on the real-time collected lidar data, including: Set the size of the time sliding window , indicating the number of frames of the included point cloud data; At the current time step t, the sliding window contains consecutive to and frame point cloud data; Based on the point cloud data of 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, characterized in that: Obtaining the lidar odometry pose, including: Use the RANSAC algorithm to process the local point cloud map Perform ground removal and downsampling to obtain the processed local point cloud map ; For each point in the current frame point cloud , obtain 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; The DBSCAN clustering algorithm is used to cluster potential dynamic points, and points with a spatial distance less than the threshold are grouped into the same object according to the clustering results ; Based on the position of the object at different time frames, calculate the change in the position of the object and the corresponding time interval , and based on and , calculate the speed of the object , ; When the speed is greater than the preset threshold, all points in the corresponding object are marked as dynamic points; Remove all the marked dynamic points from the current frame point cloud to obtain a static lidar point cloud; Performing NDT registration on the static lidar point cloud with the global prior map to obtain the lidar odometry pose in the global coordinate system.

5. The vehicle positioning method according to claim 1, characterized in that: 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 based on 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 temporal consistency processing on the camera data and the lidar data; Extracting feature points using the shi-tomasi algorithm based on the camera data after temporal consistency processing, where the feature points represent points in the image with a gradient change greater than the threshold; Constructing an optical flow constraint equation for the 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 a preset threshold, mark the corresponding feature point as a dynamic feature point; 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. Accumulate the relative pose transformation and transform the accumulated pose transformation into 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 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 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.

7. The vehicle positioning method according to claim 1, wherein: Obtain the dead reckoning result, including: Obtain the laser odometry pose 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 ; Calculate the vehicle position and heading change amount according to the CTRV model; Accumulate the vehicle position and heading change amount to the initial pose to obtain the pose at the current moment; Establish the transformation relationship from the odometry coordinate system odom to the vehicle body coordinate system base_link; According to the transformation relationship, transform the pose at the current moment from the odometry coordinate system odom to the global map coordinate system as the dead reckoning result.

8. The vehicle positioning method according to claim 7, wherein: Calculate 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 turning speed is zero, the vehicle position change is as follows: ； ； ； where \(v\) is the vehicle speed, is the angular velocity, is the current heading angle, is the system sampling time interval.

9. 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, according to 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 estimation, output the global pose including the position coordinates and the heading angle.

10. A vehicle positioning system, characterized in that, Including: At least one processing unit; used to execute instructions to implement the vehicle positioning method described in any one of claims 1 to 9.

Images