Rail car positioning method and system based on laser radar and sensor information fusion

Abstract

The invention provides a rail car positioning method and system based on information fusion of a laser radar and a sensor. The method comprises the steps that S1, off-line calibration of the laser radar and inertial navigation is carried out; s2, establishing vehicle pose calculation based on point cloud features according to the laser radar point cloud and inertial navigation data; according to an RFID coordinate observation solution graph optimization problem, constructing a globally optimized point cloud map; s3, based on the point cloud map, matching the map with the real-time laser radar data to obtain a vehicle positioning result; and performing pose optimization by using an error state Kalman filtering method and fusing inertial navigation data to obtain an accurate positioning result of the vehicle. Compared with a pure sensor data fusion positioning algorithm, the positioning method combining high-precision point cloud map data and fusing other sensor information has better adaptability and precision.

Description

Technical Field

[0001] The present invention relates to the field of unmanned driving in industrial scenarios, and specifically, to a rail vehicle positioning method and system based on laser radar and sensor information fusion, and more specifically, to real-time positioning of rail unmanned transport vehicles in industrial scenarios. Background Art

[0002] The closest technology in the existing technology (patent with authorization number CN114440881A, which discloses "a method for unmanned vehicle positioning by fusing multi-source sensor information") is to achieve more accurate unmanned vehicle positioning in a simple environment by complementing the advantages and disadvantages of GPS information and IMU information. By comparing the effects of three filtering algorithms under nonlinear systems, fusion positioning in environments with different degrees of nonlinearity can be achieved.

[0003] While other solutions may be limited by the coverage of GPS signals, lidar can provide global map information and achieve positioning in environments without GPS signals.

[0004] At present, the widespread application of rail vehicles in the field of equipment manufacturing has put forward higher requirements for positioning accuracy. Traditional rail vehicle positioning devices based on RFID recognition have problems such as too large recognition range and low accuracy. The positioning accuracy of GPS in docks, mines or closed factories will also be affected by interference such as signal loss, and cannot provide stable and reliable real-time positioning. In order to overcome these technical defects, a new rail vehicle positioning method and system is needed.

[0005] The rail vehicle positioning method based on the fusion of laser radar and sensor information uses laser radar to obtain the environmental point cloud data around the vehicle body, and collects the motion parameters of the rail vehicle through sensors. By fusing laser radar and sensor data, accurate positioning and attitude estimation of the rail vehicle can be achieved.

[0006] Compared with the traditional RFID-based identification device, the rail vehicle positioning method based on the fusion of laser radar and sensor information has higher positioning accuracy and smaller identification range. The application of this patented technology can meet the high requirements for positioning accuracy in the equipment manufacturing field and provide a more accurate, reliable and efficient solution for the automated control of rail vehicles. Summary of the invention

[0007] In view of the defects in the prior art, the object of the present invention is to provide a rail vehicle positioning method and system based on laser radar and sensor information fusion.

[0008] A rail vehicle positioning method based on laser radar and sensor information fusion provided by the present invention includes:

[0009] Step S1: perform offline calibration of laser radar and inertial navigation;

[0010] Step S2: Establishing a vehicle pose estimation based on point cloud features according to the lidar point cloud and inertial navigation data; solving the graph optimization problem according to the RFID coordinate observation and constructing a globally optimized point cloud map;

[0011] Step S3: Based on the point cloud map, match the map with the real-time lidar data to obtain the vehicle positioning result; use the error state Kalman filter method to integrate the inertial navigation data for posture optimization to obtain the vehicle's precise positioning result.

[0012] Preferably, in step S1:

[0013] Step S1.1: External parameter calibration of LiDAR and inertial navigation: When the vehicle is stationary, obtain the point cloud data collected by the LiDAR and the inertial measurement data collected by the IMU, use open source tools to perform external parameter calibration, and calculate the three-axis coordinates and three-axis rotation quaternion of the IMU when the LiDAR is used as the reference coordinate system to form the translation vector and rotation matrix from the radar to the IMU;

[0014] Step S1.2: Inertial navigation internal parameter calibration: When the vehicle is stationary, obtain the inertial measurement data collected by the IMU, and calculate the noise and zero bias through the readings of the accelerometer and gyroscope during the stationary time.

[0015] Preferably, in step S2:

[0016] Step S2.1: Receiving and preprocessing the laser point cloud, using inertial navigation data to dedistort the laser point cloud, and rotating each data point of a frame scan to the end of the frame:

[0017]

[0018] in, is the coordinate of each point in the radar coordinate system at the end of the frame, p t It is the coordinate of a point at any time in a scan time. The subscript of the rotation matrix from right to left represents the transformation relationship T LI , T IL They are the transformation matrices from inertial navigation to lidar and from lidar to inertial navigation, T(t end ) IW and T(t) WI They are the position and posture of the inertial navigation at the end of the frame and at any time in one scan time;

[0019] Step S2.2: Calculate the line beam of each point in the laser radar scanning frame through the field of view of the laser radar and classify it, calculate the difference between each point in the line beam and other adjacent points, that is, the curvature, judge the corner point and the plane point through the curvature of the line beam, and extract the features of the laser point cloud;

[0020] Step S2.3: Execute the matching of the laser point cloud to the local map, search for the nearest neighboring points of the corner point to be matched to fit the straight line, solve the distance from the corner point to be matched to the straight line and the components of the vertical vector in the three axis directions, and extract the stable corner point features; search for the nearest neighboring points of the plane point to be matched to fit the plane, judge the reliability of the plane by the distance from the point to the fitted plane, and extract the stable plane point features; solve the least squares problem based on the point-to-line and point-to-plane distances to obtain the radar odometer;

[0021] Step S2.4: Using the parallel mechanism, loop detection is performed when performing matching; iterative closest point matching is performed by searching historical key frames to identify scenes that have been reached;

[0022] Step S2.5: Use the graph optimization library GTSAM to add the radar odometer and RFID observation factors to the factor graph for optimization; if loop detection is performed, loop information is detected, and then the loop detection factor is added to obtain the globally optimized key frame pose, and the key frame laser point cloud is saved to the map.

[0023] Preferably, in step S3:

[0024] Step S3.1: Receive the lidar point cloud and sensor data, synchronize the data, and perform positioning initialization according to the RFID data;

[0025] Step S3.2: Dedistorting the laser point cloud using inertial navigation data;

[0026] Step S3.3: Use the occupancy grid map for map management and match the current scan with the map; implement the point-to-point ICP method based on the Gauss-Newton method, calculate the nearest neighbors of points in each iteration, and obtain the increment of posture by solving the least squares problem; the least squares problem with the vehicle posture as the state variable is:

[0027]

[0028] Where D(x) is the distance and function of the corresponding point pair, x is the two-dimensional position of the current vehicle, is the scan point in the current scan frame in the world coordinate system, yes The nearest neighbor point in the map, i is the index of the current point pair, and n is the number of all points in this frame of point cloud.

[0029] Preferably, in step S3:

[0030] Step S3.4: Adopt the error state Kalman filter method, fuse the vehicle wheel speed data, inertial navigation data and map matching results, and perform vehicle track estimation. The specific process is as follows: in the prediction stage, integrate the inertial navigation data to obtain the nominal state variable, and the error caused by noise is used as the error variable; in the update stage, take the map matching results and wheel speed data as observations, update the mean and covariance of the error state, and these two values ​​are used to describe the change of the error state; integrate the updated error state into the nominal state variable and reset the error state to zero, completing a cycle;

[0031] The error state variable motion equation is linearized as follows:

[0032] δx(t+Δt)=Jδx+ω

[0033] Among them, δx = [δp, δv, δθ, δb g ,δb a ,g] T is the error state variable, p is the translation, v is the velocity, b g and b a is the zero bias of the gyroscope and accelerometer, g is the acceleration of gravity, ω=[0,-η v ,-η θ ,η g ,η a ,0] T is the noise; t is the absolute time of the current moment, Δt is the time difference between two consecutive moments; η v is the error speed; η g is the measurement noise of the gyroscope; η θ is the rotation error; η a is the measurement noise of the accelerometer;

[0034] J is the Jacobian matrix:

[0035]

[0036] Where I is the identity matrix, and Acceleration and angular velocity readings for inertial navigation;

[0037] In the prediction stage, the predicted value of the noise covariance between the nominal state variables and the motion equation is

[0038] δx pred =Jδx,

[0039] P pred =JPJ T +Q,

[0040] Among them, δxpred is the predicted value of the nominal state variable, Q is the noise covariance during the sampling time;

[0041] In the update phase, the error state update process is:

[0042] K=P pred H T (HP pred H T +V) -1 ,

[0043] δx=K(zh(x pred )),

[0044] x=x pred +δx,

[0045] P=(I-KH)P pred .

[0046] Where K is the Kalman gain, P pred is the predicted covariance matrix, z is the observed data, V is the covariance matrix of the observation noise, h is the observation equation, H is the Jacobian matrix of the relative error state of the observation equation, and P is the optimal covariance matrix;

[0047] The matching result between the current frame point cloud and the map is the observation of translation and rotation. The observation value at a certain moment is p icp With R icp , the observation equation is

[0048] p icp =p+δp,

[0049] R icp =RExp(δθ).

[0050] Among them, δp is the error state of the translation transformation, Exp(δθ) is the Lie algebraic representation corresponding to the nominal state rotation R;

[0051] R icp is the observation of the error state δθ, and the update amount zh(x) in the update process is expressed as:

[0052] zh(x)=[p icp -p,Log(R T R icp )] T .

[0053] The Jacobian matrices of the translation part and the rotation part are both unit matrices;

[0054] The wheel speed meter data is a direct observation of the vehicle's speed in the forward direction. The Jacobian matrix of the nominal state speed is the unit matrix. The Kalman gain is calculated and the covariance is updated;

[0055] Step S3.5: Compare the RFID coordinates in the data queue that are closest to the current frame point cloud time. If the position information deviation is greater than the preset value or is lost, repositioning is performed;

[0056] Step S3.6: Use ROS communication to publish global positioning coordinates.

[0057] According to the present invention, a rail vehicle positioning system based on laser radar and sensor information fusion is provided, comprising:

[0058] Module M1: Offline calibration of LiDAR and inertial navigation;

[0059] Module M2: Establish vehicle posture estimation based on point cloud features according to LiDAR point cloud and inertial navigation data; solve graph optimization problems according to RFID coordinate observations and construct a globally optimized point cloud map;

[0060] Module M3: Based on the point cloud map, match the map with real-time lidar data to obtain the vehicle positioning result; use the error state Kalman filter method to integrate the inertial navigation data for posture optimization to obtain the vehicle's precise positioning result.

[0061] Preferably, in the module M1:

[0062] Module M1.1: External parameter calibration of LiDAR and inertial navigation: When the vehicle is stationary, obtain the point cloud data collected by the LiDAR and the inertial measurement data collected by the IMU, use open source tools to perform external parameter calibration, and calculate the three-axis coordinates and three-axis rotation quaternions of the IMU when the LiDAR is used as the reference coordinate system to form the translation vector and rotation matrix from the radar to the IMU;

[0063] Module M1.2: Inertial navigation internal parameter calibration: When the vehicle is stationary, obtain the inertial measurement data collected by the IMU, and calculate the noise and zero bias through the readings of the accelerometer and gyroscope during the stationary time.

[0064] Preferably, in the module M2:

[0065] Module M2.1: Receiving and preprocessing the laser point cloud, using inertial navigation data to dedistort the laser point cloud, and rotating each data point of a frame scan to the end of the frame:

[0066]

[0067] in, is the coordinate of each point in the radar coordinate system at the end of the frame, p t It is the coordinate of a point at any time in a scan time. The subscript of the rotation matrix from right to left represents the transformation relationship T LI , TIL They are the transformation matrices from inertial navigation to lidar and from lidar to inertial navigation, T(t end ) IW and T(t) WI They are the position and posture of the inertial navigation at the end of the frame and at any time in one scan time;

[0068] Module M2.2: Calculate the line beam of each point in the laser radar scanning frame through the laser radar's field of view and classify it, calculate the difference between each point in the line beam and other adjacent points, that is, the curvature, determine the corner points and plane points through the curvature of the line beam, and extract features from the laser point cloud;

[0069] Module M2.3: Execute the matching of laser point cloud to local map, search for the nearest neighboring points of the corner point to be matched to fit the straight line, solve the distance from the corner point to be matched to the straight line and the components of the vertical vector in the three axis directions, and extract the stable corner point features; search for the nearest neighboring points of the plane point to be matched to fit the plane, judge the reliability of the plane by the distance from the point to the fitted plane, and extract the stable plane point features; solve the least squares problem based on the point-to-line and point-to-plane distances to obtain the radar odometer;

[0070] Module M2.4: Use parallel mechanism to perform loop detection when performing matching; perform iterative closest point matching by searching historical keyframes to identify scenes that have been reached;

[0071] Module M2.5: Use the graph optimization library GTSAM to add the radar odometer and RFID observation factors to the factor graph for optimization; if loop detection is performed, loop information is detected, and then the loop detection factor is added to obtain the globally optimized keyframe pose, and the keyframe laser point cloud is saved to the map.

[0072] Preferably, in the module M3:

[0073] Module M3.1: Receives lidar point cloud and sensor data, performs data synchronization, and performs positioning initialization based on RFID data;

[0074] Module M3.2: Dedistortion of laser point cloud using inertial navigation data;

[0075] Module M3.3: Use the occupancy grid map for map management and match the current scan with the map; implement the point-to-point ICP method based on the Gauss-Newton method, calculate the nearest neighbors of points in each iteration, and obtain the increment of posture by solving the least squares problem; the least squares problem with vehicle posture as the state variable is:

[0076]

[0077] Where D(x) is the distance and function of the corresponding point pair, x is the two-dimensional position of the current vehicle, is the scan point in the current scan frame in the world coordinate system, yes The nearest neighbor point in the map, i is the index of the current point pair, and n is the number of all points in this frame of point cloud.

[0078] Preferably, in the module M3:

[0079] Module M3.4: Adopt the error state Kalman filter method, integrate the vehicle wheel speed data, inertial navigation data and map matching results, and perform vehicle track calculation. The specific process is as follows: in the prediction stage, integrate the inertial navigation data to obtain the nominal state variable, and the error caused by noise is used as the error variable; in the update stage, take the map matching results and wheel speed data as observations, update the mean and covariance of the error state, and these two values ​​are used to describe the change of the error state; integrate the updated error state into the nominal state variable and reset the error state to zero, completing a cycle;

[0080] The error state variable motion equation is linearized as follows:

[0081] δx(t+Δt)=Jδx+ω

[0082] Among them, δx=[δp, δv, δθ, δb g ,δb a ,g] T is the error state variable, p is the translation, v is the velocity, b g and b a is the zero bias of the gyroscope and accelerometer, g is the acceleration of gravity, ω=[0,-η v ,-η θ ,η g ,η a ,0] T is the noise; t is the absolute time of the current moment, Δt is the time difference between two consecutive moments; η v is the error speed; η g is the measurement noise of the gyroscope; η θ is the rotation error; η a is the measurement noise of the accelerometer;

[0083] J is the Jacobian matrix:

[0084]

[0085] Where I is the identity matrix, and Acceleration and angular velocity readings for inertial navigation;

[0086] In the prediction stage, the predicted value of the noise covariance between the nominal state variables and the motion equation is

[0087] δx pred =Jδx,

[0088] P pred =JPJ T +Q,

[0089] Among them, δx pred is the predicted value of the nominal state variable, Q is the noise covariance during the sampling time;

[0090] In the update phase, the error state update process is:

[0091] K=P pred H T (HP pred H T +V) -1 ,

[0092] δx=K(zh(x pred )),

[0093] x=x pred +δx,

[0094] P=(I-KH)P pred .

[0095] Where K is the Kalman gain, P pred is the predicted covariance matrix, z is the observed data, V is the covariance matrix of the observation noise, h is the observation equation, H is the Jacobian matrix of the relative error state of the observation equation, and P is the optimal covariance matrix;

[0096] The matching result between the current frame point cloud and the map is the observation of translation and rotation. The observation value at a certain moment is p icp With R icp , the observation equation is

[0097] p icp =p+δp,

[0098] R icp =RExp(δθ).

[0099] Among them, δp is the error state of the translation transformation, Exp(δθ) is the Lie algebraic representation corresponding to the nominal state rotation R;

[0100] R icp is the observation of the error state δθ, and the update amount zh(x) in the update process is expressed as:

[0101] zh(x)=[p icp -p,Log(RT R icp )] T .

[0102] The Jacobian matrices of the translation part and the rotation part are both unit matrices;

[0103] The wheel speed meter data is a direct observation of the vehicle's speed in the forward direction. The Jacobian matrix of the nominal state speed is the unit matrix. The Kalman gain is calculated and the covariance is updated;

[0104] Module M3.5: Compare the RFID coordinates in the data queue that are closest to the current frame point cloud time. If the position information deviation is greater than the preset value or is lost, repositioning is performed;

[0105] Module M3.6: Use ROS communication to publish global positioning coordinates.

[0106] Compared with the prior art, the present invention has the following beneficial effects:

[0107] 1. The present invention adopts a series of offline mapping and online positioning methods to filter and fuse the matching results of the lidar point cloud and the map with the sensor information, thereby realizing the point cloud fusion positioning problem of unmanned transportation in industrial scenarios.

[0108] 2. Compared with the positioning algorithm of simple sensor data fusion, the positioning method of the present invention, which combines high-precision point cloud map data and integrates other sensor information, has better adaptability and accuracy.

[0109] 3. By fusing lidar and sensor data, the present invention can achieve higher positioning accuracy and provide more accurate rail vehicle position and attitude information.

[0110] 4. The present invention provides a rail vehicle real-time positioning method, which can instantly obtain the position and posture of the rail vehicle. The real-time positioning result can be used to control the rail vehicle to improve production efficiency and work efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0111] Other features, objects and advantages of the present invention will become more apparent from the detailed description of non-limiting embodiments made with reference to the following drawings:

[0112] Figure 1 It is a schematic diagram of the process of the present invention. DETAILED DESCRIPTION

[0113] The present invention is described in detail below in conjunction with specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention, but are not intended to limit the present invention in any form. It should be noted that, for those of ordinary skill in the art, several changes and improvements can also be made without departing from the concept of the present invention. These all belong to the protection scope of the present invention.

[0114] Embodiment 1:

[0115] The present invention realizes a real-time positioning solution for a rail vehicle by matching laser radar data and performing filter fusion with sensor information on the basis of establishing a point cloud map.

[0116] The present invention provides a method, system and steps for rail vehicle positioning based on the fusion of laser radar and sensor information. The present invention includes two operations: offline mapping and online positioning. Offline mapping includes: establishing vehicle posture estimation based on point cloud features according to laser radar point cloud and inertial navigation data; solving graph optimization problems according to RFID coordinate observations, and constructing a globally optimized point cloud map. Online positioning includes: matching maps with real-time laser radar data to obtain vehicle positioning results; using error state Kalman filtering methods, fusing inertial navigation data for posture optimization, and obtaining vehicle precise positioning results. The present invention integrates multiple sensor information to achieve unmanned vehicle positioning in open outdoor environments. Based on the present invention, the high requirements for positioning accuracy in the field of equipment manufacturing can be met, providing a more accurate, reliable and efficient solution for the automated control of rail vehicles.

[0117] According to a rail vehicle positioning method based on laser radar and sensor information fusion provided by the present invention, Figure 1 As shown, including:

[0118] Step S1: perform offline calibration of laser radar and inertial navigation;

[0119] Specifically, in step S1:

[0120] Step S1.1: External parameter calibration of LiDAR and inertial navigation: When the vehicle is stationary, obtain the point cloud data collected by the LiDAR and the inertial measurement data collected by the IMU, use open source tools to perform external parameter calibration, and calculate the three-axis coordinates and three-axis rotation quaternion of the IMU when the LiDAR is used as the reference coordinate system to form the translation vector and rotation matrix from the radar to the IMU;

[0121] Step S1.2: Inertial navigation internal parameter calibration: When the vehicle is stationary, obtain the inertial measurement data collected by the IMU, and calculate the noise and zero bias through the readings of the accelerometer and gyroscope during the stationary time.

[0122] Step S2: Establishing a vehicle pose estimation based on point cloud features according to the lidar point cloud and inertial navigation data; solving the graph optimization problem according to the RFID coordinate observation and constructing a globally optimized point cloud map;

[0123] Specifically, in step S2:

[0124] Step S2.1: Receiving and preprocessing the laser point cloud, using inertial navigation data to dedistort the laser point cloud, and rotating each data point of a frame scan to the end of the frame:

[0125]

[0126] in, is the coordinate of each point in the radar coordinate system at the end of the frame, p t It is the coordinate of a point at any time in a scan time. The subscript of the rotation matrix from right to left represents the transformation relationship T LI , T IL They are the transformation matrices from inertial navigation to lidar and from lidar to inertial navigation, T(t end ) IW and T(t) WI They are the position and posture of the inertial navigation at the end of the frame and at any time in one scan time;

[0127] Step S2.2: Calculate the line beam of each point in the laser radar scanning frame through the field of view of the laser radar and classify it, calculate the difference between each point in the line beam and other adjacent points, that is, the curvature, judge the corner point and the plane point through the curvature of the line beam, and extract the features of the laser point cloud;

[0128] Step S2.3: Execute the matching of the laser point cloud to the local map, search for the nearest neighboring points of the corner point to be matched to fit the straight line, solve the distance from the corner point to be matched to the straight line and the components of the vertical vector in the three axis directions, and extract the stable corner point features; search for the nearest neighboring points of the plane point to be matched to fit the plane, judge the reliability of the plane by the distance from the point to the fitted plane, and extract the stable plane point features; solve the least squares problem based on the point-to-line and point-to-plane distances to obtain the radar odometer;

[0129] Step S2.4: Using the parallel mechanism, loop detection is performed when performing matching; iterative closest point matching is performed by searching historical key frames to identify scenes that have been reached;

[0130] Step S2.5: Use the graph optimization library GTSAM to add the radar odometer and RFID observation factors to the factor graph for optimization; if loop detection is performed, loop information is detected, and then the loop detection factor is added to obtain the globally optimized key frame pose, and the key frame laser point cloud is saved to the map.

[0131] Step S3: Based on the point cloud map, match the map with the real-time lidar data to obtain the vehicle positioning result; use the error state Kalman filter method to integrate the inertial navigation data for posture optimization to obtain the vehicle's precise positioning result.

[0132] Specifically, in step S3:

[0133] Step S3.1: Receive the lidar point cloud and sensor data, synchronize the data, and perform positioning initialization according to the RFID data;

[0134] Step S3.2: Dedistorting the laser point cloud using inertial navigation data;

[0135] Step S3.3: Use the occupancy grid map for map management and match the current scan with the map; implement the point-to-point ICP method based on the Gauss-Newton method, calculate the nearest neighbors of points in each iteration, and obtain the increment of posture by solving the least squares problem; the least squares problem with the vehicle posture as the state variable is:

[0136]

[0137] Where D(x) is the distance and function of the corresponding point pair, x is the two-dimensional position of the current vehicle, is the scan point in the current scan frame in the world coordinate system, yes The nearest neighbor point in the map, i is the index of the current point pair, and n is the number of all points in this frame of point cloud.

[0138] Specifically, in step S3:

[0139] Step S3.4: Adopt the error state Kalman filter method, fuse the vehicle wheel speed data, inertial navigation data and map matching results, and perform vehicle track estimation. The specific process is as follows: in the prediction stage, integrate the inertial navigation data to obtain the nominal state variable, and the error caused by noise is used as the error variable; in the update stage, take the map matching results and wheel speed data as observations, update the mean and covariance of the error state, and these two values ​​are used to describe the change of the error state; integrate the updated error state into the nominal state variable and reset the error state to zero, completing a cycle;

[0140] The error state variable motion equation is linearized as follows:

[0141] δx(t+Δt)=Jδx+ω

[0142] Among them, δx = [δp, δv, δθ, δb g ,δb a ,g] T is the error state variable, p is the translation, v is the velocity, bg and b a is the zero bias of the gyroscope and accelerometer, g is the acceleration of gravity, ω=[0,-η v ,-η θ ,η g ,η a ,0] T is the noise; t is the absolute time of the current moment, Δt is the time difference between two consecutive moments; η v is the error speed; η g is the measurement noise of the gyroscope; η θ is the rotation error; η a is the measurement noise of the accelerometer;

[0143] J is the Jacobian matrix:

[0144]

[0145] Where I is the identity matrix, and Acceleration and angular velocity readings for inertial navigation;

[0146] In the prediction stage, the predicted value of the noise covariance between the nominal state variables and the motion equation is

[0147] δx pred =Jδx,

[0148] P pred =JPJ T +Q,

[0149] Among them, δx pred is the predicted value of the nominal state variable, Q is the noise covariance during the sampling time;

[0150] In the update phase, the error state update process is:

[0151] K=P pred H T (HP pred H T +V) -1 ,

[0152] δx=K(zh(x pred )),

[0153] x=x pred +δx,

[0154] P=(I-KH)P pred .

[0155] Where K is the Kalman gain, P predis the predicted covariance matrix, z is the observed data, V is the covariance matrix of the observation noise, h is the observation equation, H is the Jacobian matrix of the relative error state of the observation equation, and P is the optimal covariance matrix;

[0156] The matching result between the current frame point cloud and the map is the observation of translation and rotation. The observation value at a certain moment is p icp With R icp , the observation equation is

[0157] p icp =p+δp,

[0158] R icp =RExp(δθ).

[0159] Among them, δp is the error state of the translation transformation, Exp(δθ) is the Lie algebraic representation corresponding to the nominal state rotation R;

[0160] R icp is the observation of the error state δθ, and the update amount zh(x) in the update process is expressed as:

[0161] zh(x)=[p icp -p,Log(R T R icp )] T .

[0162] The Jacobian matrices of the translation part and the rotation part are both unit matrices;

[0163] The wheel speed meter data is a direct observation of the vehicle's speed in the forward direction. The Jacobian matrix of the nominal state speed is the unit matrix. The Kalman gain is calculated and the covariance is updated;

[0164] Step S3.5: Compare the RFID coordinates in the data queue that are closest to the current frame point cloud time. If the position information deviation is greater than the preset value or is lost, repositioning is performed;

[0165] Step S3.6: Use ROS communication to publish global positioning coordinates.

[0166] Embodiment 2:

[0167] Embodiment 2 is a preferred example of Embodiment 1, and is used to illustrate the present invention in more detail.

[0168] The present invention also provides a rail vehicle positioning system based on the fusion of laser radar and sensor information. The rail vehicle positioning system based on the fusion of laser radar and sensor information can be realized by executing the process steps of the rail vehicle positioning method based on the fusion of laser radar and sensor information, that is, those skilled in the art can understand the rail vehicle positioning method based on the fusion of laser radar and sensor information as a preferred implementation of the rail vehicle positioning system based on the fusion of laser radar and sensor information.

[0169] According to the present invention, a rail vehicle positioning system based on laser radar and sensor information fusion is provided, comprising:

[0170] Module M1: Offline calibration of LiDAR and inertial navigation;

[0171] Specifically, in the module M1:

[0172] Module M1.1: External parameter calibration of LiDAR and inertial navigation: When the vehicle is stationary, obtain the point cloud data collected by the LiDAR and the inertial measurement data collected by the IMU, use open source tools to perform external parameter calibration, and calculate the three-axis coordinates and three-axis rotation quaternions of the IMU when the LiDAR is used as the reference coordinate system to form the translation vector and rotation matrix from the radar to the IMU;

[0173] Module M1.2: Inertial navigation internal parameter calibration: When the vehicle is stationary, obtain the inertial measurement data collected by the IMU, and calculate the noise and zero bias through the readings of the accelerometer and gyroscope during the stationary time.

[0174] Module M2: Establish vehicle posture estimation based on point cloud features according to LiDAR point cloud and inertial navigation data; solve graph optimization problems according to RFID coordinate observations and construct a globally optimized point cloud map;

[0175] Specifically, in the module M2:

[0176] Module M2.1: Receiving and preprocessing the laser point cloud, using inertial navigation data to dedistort the laser point cloud, and rotating each data point of a frame scan to the end of the frame:

[0177]

[0178] in, is the coordinate of each point in the radar coordinate system at the end of the frame, p t It is the coordinate of a point at any time in a scan time. The subscript of the rotation matrix from right to left represents the transformation relationship T LI , T IL They are the transformation matrices from inertial navigation to lidar and from lidar to inertial navigation, T(t end ) IWand T(t) WI They are the position and posture of the inertial navigation at the end of the frame and at any time in one scan time;

[0179] Module M2.2: Calculate the line beam of each point in the laser radar scanning frame through the laser radar's field of view and classify it, calculate the difference between each point in the line beam and other adjacent points, that is, the curvature, determine the corner points and plane points through the curvature of the line beam, and extract features from the laser point cloud;

[0180] Module M2.3: Execute the matching of laser point cloud to local map, search for the nearest neighboring points of the corner point to be matched to fit the straight line, solve the distance from the corner point to be matched to the straight line and the components of the vertical vector in the three axis directions, and extract the stable corner point features; search for the nearest neighboring points of the plane point to be matched to fit the plane, judge the reliability of the plane by the distance from the point to the fitted plane, and extract the stable plane point features; solve the least squares problem based on the point-to-line and point-to-plane distances to obtain the radar odometer;

[0181] Module M2.4: Use parallel mechanism to perform loop detection when performing matching; perform iterative closest point matching by searching historical keyframes to identify scenes that have been reached;

[0182] Module M2.5: Use the graph optimization library GTSAM to add the radar odometer and RFID observation factors to the factor graph for optimization; if loop detection is performed, loop information is detected, and then the loop detection factor is added to obtain the globally optimized keyframe pose, and the keyframe laser point cloud is saved to the map.

[0183] Module M3: Based on the point cloud map, match the map with real-time lidar data to obtain the vehicle positioning result; use the error state Kalman filter method to integrate the inertial navigation data for posture optimization to obtain the vehicle's precise positioning result.

[0184] Specifically, in the module M3:

[0185] Module M3.1: Receives lidar point cloud and sensor data, performs data synchronization, and performs positioning initialization based on RFID data;

[0186] Module M3.2: Dedistortion of laser point cloud using inertial navigation data;

[0187] Module M3.3: Use the occupancy grid map for map management and match the current scan with the map; implement the point-to-point ICP method based on the Gauss-Newton method, calculate the nearest neighbors of points in each iteration, and obtain the increment of posture by solving the least squares problem; the least squares problem with vehicle posture as the state variable is:

[0188]

[0189] Where D(x) is the distance and function of the corresponding point pair, x is the two-dimensional position of the current vehicle, is the scan point in the current scan frame in the world coordinate system, yes The nearest neighbor point in the map, i is the index of the current point pair, and n is the number of all points in this frame of point cloud.

[0190] Specifically, in the module M3:

[0191] Module M3.4: Adopt the error state Kalman filter method, integrate the vehicle wheel speed data, inertial navigation data and map matching results, and perform vehicle track calculation. The specific process is as follows: in the prediction stage, integrate the inertial navigation data to obtain the nominal state variable, and the error caused by noise is used as the error variable; in the update stage, take the map matching results and wheel speed data as observations, update the mean and covariance of the error state, and these two values ​​are used to describe the change of the error state; integrate the updated error state into the nominal state variable and reset the error state to zero, completing a cycle;

[0192] The error state variable motion equation is linearized as follows:

[0193] δx(t+Δt)=Jδx+ω

[0194] Among them, δx=[δp, δv, δθ, δb g ,δb a ,g] T is the error state variable, p is the translation, v is the velocity, b g and b a is the zero bias of the gyroscope and accelerometer, g is the acceleration of gravity, ω=[0,-η v ,-η θ ,η g ,η a ,0] T is the noise; t is the absolute time of the current moment, Δt is the time difference between two consecutive moments; η v is the error speed; η g is the measurement noise of the gyroscope; η θ is the rotation error; η a is the measurement noise of the accelerometer;

[0195] J is the Jacobian matrix:

[0196]

[0197] Where I is the identity matrix, and Acceleration and angular velocity readings for inertial navigation;

[0198] In the prediction stage, the predicted value of the noise covariance between the nominal state variables and the motion equation is

[0199] δx pred =Jδx,

[0200] P pred =JPJ T +Q,

[0201] Among them, δx pred is the predicted value of the nominal state variable, Q is the noise covariance during the sampling time;

[0202] In the update phase, the error state update process is:

[0203] K=P pred H T (HP pred H T +V) -1 ,

[0204] δx=K(zh(x pred )),

[0205] x=x pred +δx,

[0206] P=(I-KH)P pred .

[0207] Where K is the Kalman gain, P pred is the predicted covariance matrix, z is the observed data, V is the covariance matrix of the observation noise, h is the observation equation, H is the Jacobian matrix of the relative error state of the observation equation, and P is the optimal covariance matrix;

[0208] The matching result between the current frame point cloud and the map is the observation of translation and rotation. The observation value at a certain moment is p icp With R icp , the observation equation is

[0209] p icp =p+δp,

[0210] R icp =RExp(δθ).

[0211] Among them, δp is the error state of the translation transformation, Exp(δθ) is the Lie algebraic representation corresponding to the nominal state rotation R;

[0212] R icp is the observation of the error state δθ, and the update amount zh(x) in the update process is expressed as:

[0213] zh(x)=[p icp -p,Log(RT R icp )] T .

[0214] The Jacobian matrices of the translation part and the rotation part are both unit matrices;

[0215] The wheel speed meter data is a direct observation of the vehicle's speed in the forward direction. The Jacobian matrix of the nominal state speed is the unit matrix. The Kalman gain is calculated and the covariance is updated;

[0216] Module M3.5: Compare the RFID coordinates in the data queue that are closest to the current frame point cloud time. If the position information deviation is greater than the preset value or is lost, repositioning is performed;

[0217] Module M3.6: Use ROS communication to publish global positioning coordinates.

[0218] Embodiment 3:

[0219] Embodiment 3 is a preferred example of Embodiment 1, and is used to illustrate the present invention in more detail.

[0220] Step S1: Offline calibration of laser radar and inertial navigation;

[0221] Step S2: Offline construction of point cloud map using lidar point cloud, inertial navigation and RFID data;

[0222] Step S3: Based on the saved point cloud map, the vehicle is positioned online using the lidar point cloud data, inertial navigation data, wheel speed data and RFID data.

[0223] The step S1 comprises the following steps:

[0224] Step S1.1: External parameter calibration of LiDAR and inertial navigation. When the vehicle is stationary, obtain the point cloud data collected by the LiDAR and the inertial measurement data collected by the IMU, use open source tools to perform external parameter calibration, and calculate the three-axis coordinates and three-axis rotation quaternions of the IMU when the LiDAR is used as the reference coordinate system to form the translation vector and rotation matrix from the radar to the IMU.

[0225] Step S1.2: Inertial navigation internal parameter calibration: When the vehicle is stationary, obtain the inertial measurement data collected by the IMU, and calculate the noise and zero bias through the readings of the accelerometer and gyroscope during the stationary time.

[0226] The step S2 comprises the following steps:

[0227] Step S2.1: Receiving and preprocessing the laser point cloud, using inertial navigation data to dedistort the laser point cloud, and rotating each data point of a frame scan to the end of the frame.

[0228]

[0229] in, is the coordinate of each point in the radar coordinate system at the end of the frame, p t is the coordinate of a point at any time in a scan time. The subscript of the rotation matrix represents the transformation relationship from right to left, that is, T LI , T IL They are the transformation matrices from inertial navigation to lidar and from lidar to inertial navigation, T(t end ) IW , T(t) WI , are the positions of the inertial navigation at the end of the frame and at any time in a scan time.

[0230] Step S2.2: Calculate the line beam of each point in the laser radar scanning frame through the field of view of the laser radar and classify it, calculate the difference between each point in the line beam and other adjacent points, that is, the curvature, judge the corner point and the plane point through the curvature of the line beam, and extract the features of the laser point cloud;

[0231] Step S2.3: Execute the matching of laser point cloud to local map. Search for the nearest neighboring points of the corner point to be matched to fit the straight line, solve the distance from the corner point to be matched to the straight line and the components of the perpendicular vector in the three axis directions, and extract the stable corner point features. Search for the nearest neighboring points of the plane point to be matched to fit the plane, judge the reliability of the plane by the distance from the point to the fitted plane, and extract the stable plane point features. Solve the least squares problem based on the point-to-line and point-to-plane distances to obtain a more accurate radar odometer;

[0232] Step S2.4: Using the parallel mechanism, loop detection is performed when performing matching; iterative closest point matching is performed by searching historical key frames to identify scenes that have been reached;

[0233] Step S2.5: Use the graph optimization library GTSAM to add the radar odometer and RFID observation factors to the factor graph for optimization. If loop detection is performed, loop information is detected, and then the loop detection factor is added to obtain the globally optimized keyframe pose, and the keyframe laser point cloud is saved to the map.

[0234] The step S3 comprises the following steps:

[0235] Step S3.1: Receive the lidar point cloud and sensor data, synchronize the data, and perform positioning initialization according to the RFID data;

[0236] Step S3.2: Dedistorting the laser point cloud using inertial navigation data;

[0237] Step S3.3: Use the occupancy grid map for map management and match the current scan with the map. The point-to-point ICP method is implemented based on the Gauss-Newton method. The nearest neighbors of points are calculated in each iteration, and the increment of the posture is obtained by solving the least squares problem. The least squares problem with the vehicle posture as the state variable is

[0238]

[0239] Where D(x) is the distance and function of the corresponding point pair, x is the two-dimensional position of the current vehicle, is the scan point in the current scan frame in the world coordinate system, yes The nearest neighbor point in the map.

[0240] Step S3.4: Use the error state Kalman filter method to fuse the vehicle wheel speed data, inertial navigation data and map matching results to perform vehicle track estimation. The specific process is as follows: In the prediction stage, the inertial navigation data is integrated to obtain the nominal state variable. At this time, the error caused by noise is used as the error variable. In the update stage, the map matching results and wheel speed data are used as observations to update the mean and covariance of the error state. These two values ​​are used to describe the change of the error state. The updated error state is integrated into the nominal state variable and the error state is reset to zero to complete a cycle.

[0241] The error state variable motion equation is linearized as follows:

[0242] δx(t+Δt)=Jδx+ω,

[0243] Among them, δx=[δp, δv, δθ, δb g ,δb a ,g] T is the error state variable p is translation, v is velocity, b g and b a is the zero bias of the gyroscope and accelerometer, g is the acceleration of gravity, ω=[0,-η g ,-η θ ,η g ,η a ,0] T is the noise. J is the Jacobian matrix,

[0244]

[0245] Where I is the identity matrix, and It is the acceleration and angular velocity readings of inertial navigation.

[0246] In the prediction stage, the predicted value of the noise covariance between the nominal state variables and the motion equation is

[0247] δx pred =Jδx,

[0248] P pred =JPJ T +Q,

[0249] In the update phase, the error state update process is:

[0250] K=P pred H T (HP pred H T +V) -1 ,

[0251] δx=K(zh(x pred )),

[0252] x=x pred +δx,

[0253] P=(I-KH)P pred .

[0254] Where K is the Kalman gain, P pred is the predicted covariance matrix, z is the observed data, V is the covariance matrix of the observation noise, h is the observation equation, H is the Jacobian matrix of the observation equation relative error state, and P is the optimal covariance matrix.

[0255] The matching result between the current frame point cloud and the map can be directly regarded as the observation of translation and rotation. The observation value at a certain moment is recorded as p icp With R icp , the observation equation is

[0256] p icp =p+δp,

[0257] R icp =RExp(δθ).

[0258] R icp is regarded as the observation of the error state δθ, Exp(δθ) is the Lie algebraic representation corresponding to the nominal state rotation R, and the update amount zh(x) in the update process can be expressed as

[0259] zh(x)=[p icp -p,Log(R T R icp )] T .

[0260] The Jacobian matrices of the translation part and the rotation part are both unit matrices.

[0261] The wheel speed meter data can be regarded as a direct observation of the vehicle's speed in the forward direction, and the Jacobian matrix of the nominal state speed is the unit matrix. Similarly, the Kalman gain can be calculated and the covariance can be updated.

[0262] Step S3.5: Compare the RFID coordinates in the data queue that are closest to the current frame point cloud time. If the position information deviates too much or is lost, reposition it;

[0263] Step S3.6: Use ROS communication to publish global positioning coordinates.

[0264] Those skilled in the art know that, in addition to realizing the system and its various devices, modules, and units provided by the present invention in a purely computer-readable program code, it is entirely possible to realize the same functions in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers by logically programming the method steps. Therefore, the system and its various devices, modules, and units provided by the present invention can be considered as a hardware component, and the devices, modules, and units included therein for realizing various functions can also be regarded as structures within the hardware component; the devices, modules, and units for realizing various functions can also be regarded as both software modules for realizing the method and structures within the hardware component.

[0265] The above describes the specific embodiments of the present invention. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art can make various changes or modifications within the scope of the claims, which does not affect the essence of the present invention. In the absence of conflict, the embodiments of the present application and the features in the embodiments can be combined with each other arbitrarily.

Claims

1. A rail vehicle positioning method based on laser radar and sensor information fusion, characterized in that: include: Step S1: perform offline calibration of laser radar and inertial navigation; Step S2: Establishing a vehicle pose estimation based on point cloud features according to the lidar point cloud and inertial navigation data; solving the graph optimization problem according to the RFID coordinate observation and constructing a globally optimized point cloud map; Step S3: Based on the point cloud map, match the map with the real-time lidar data to obtain the vehicle positioning result; use the error state Kalman filter method to integrate the inertial navigation data for posture optimization to obtain the vehicle's precise positioning result.

2. The rail vehicle positioning method based on laser radar and sensor information fusion according to claim 1 is characterized in that: In step S1: Step S1.1: External parameter calibration of LiDAR and inertial navigation: When the vehicle is stationary, obtain the point cloud data collected by the LiDAR and the inertial measurement data collected by the IMU, use open source tools to perform external parameter calibration, and calculate the three-axis coordinates and three-axis rotation quaternion of the IMU when the LiDAR is used as the reference coordinate system to form the translation vector and rotation matrix from the radar to the IMU; Step S1.2: Inertial navigation internal parameter calibration: When the vehicle is stationary, obtain the inertial measurement data collected by the IMU, and calculate the noise and zero bias through the readings of the accelerometer and gyroscope during the stationary time.

3. The rail vehicle positioning method based on laser radar and sensor information fusion according to claim 1 is characterized in that: In step S2: Step S2.1: Receiving and preprocessing the laser point cloud, using inertial navigation data to dedistort the laser point cloud, and rotating each data point of a frame scan to the end of the frame: in, is the coordinate of each point in the radar coordinate system at the end of the frame, p t It is the coordinate of a point at any time in a scan time. The subscript of the rotation matrix from right to left represents the transformation relationship T LI , T IL They are the transformation matrices from inertial navigation to lidar and from lidar to inertial navigation, T(t end ) IW and T(t) WI They are the position and posture of the inertial navigation at the end of the frame and at any time in one scan time; Step S2.2: Calculate the line beam of each point in the laser radar scanning frame through the field of view of the laser radar and classify it, calculate the difference between each point in the line beam and other adjacent points, that is, the curvature, judge the corner point and the plane point through the curvature of the line beam, and extract the features of the laser point cloud; Step S2.3: Execute the matching of the laser point cloud to the local map, search for the nearest neighboring points of the corner point to be matched to fit the straight line, solve the distance from the corner point to be matched to the straight line and the components of the vertical vector in the three axis directions, and extract the stable corner point features; search for the nearest neighboring points of the plane point to be matched to fit the plane, judge the reliability of the plane by the distance from the point to the fitted plane, and extract the stable plane point features; solve the least squares problem based on the point-to-line and point-to-plane distances to obtain the radar odometer; Step S2.4: Using the parallel mechanism, loop detection is performed when performing matching; iterative closest point matching is performed by searching historical key frames to identify scenes that have been reached; Step S2.5: Use the graph optimization library GTSAM to add the radar odometer and RFID observation factors to the factor graph for optimization; if loop detection is performed, loop information is detected, and then the loop detection factor is added to obtain the globally optimized key frame pose, and the key frame laser point cloud is saved to the map.

4. The rail vehicle positioning method based on laser radar and sensor information fusion according to claim 1 is characterized in that: In step S3: Step S3.1: Receive the lidar point cloud and sensor data, synchronize the data, and perform positioning initialization according to the RFID data; Step S3.2: Dedistorting the laser point cloud using inertial navigation data; Step S3.3: Use the occupancy grid map for map management and match the current scan with the map; implement the point-to-point ICP method based on the Gauss-Newton method, calculate the nearest neighbors of points in each iteration, and obtain the increment of the pose by solving the least squares problem; The least squares problem with vehicle posture as state variable is: Where D(x) is the distance and function of the corresponding point pair, x is the two-dimensional position of the current vehicle, is the scan point in the current scan frame in the world coordinate system, yes The nearest neighbor point in the map, i is the index of the current point pair, and n is the number of all points in this frame of point cloud.

5. The rail vehicle positioning method based on laser radar and sensor information fusion according to claim 4 is characterized in that: In step S3: Step S3.4: Using the error state Kalman filter method, the vehicle wheel speed data, inertial navigation data and map matching results are integrated to perform vehicle track calculation. The specific process is as follows: in the prediction stage, the inertial navigation data is integrated to obtain the nominal state variable, and the error caused by noise is used as the error variable; In the update phase, the map matching results and wheel speed data are used as observations to update the mean and covariance of the error state. The two values ​​are used to describe the change of the error state. The updated error state is integrated into the nominal state variable and the error state is reset to zero to complete a cycle. The error state variable motion equation is linearized as δx(t+Δt)=Jδx+ω Among them, δx=[δp, δv, δθ, δb g ,δb a ,g] T is the error state variable, p is the translation, v is the velocity, b g and b a is the zero bias of the gyroscope and accelerometer, g is the acceleration of gravity, ω=[0,-η v ,-η θ ,η g ,η a ,0] T is the noise; t is the absolute time of the current moment, Δt is the time difference between two consecutive moments; η v is the error speed; η g is the measurement noise of the gyroscope; η θ is the rotation error; η a is the measurement noise of the accelerometer; J is the Jacobian matrix: Where I is the identity matrix, and Acceleration and angular velocity readings for inertial navigation; In the prediction stage, the predicted value of the noise covariance between the nominal state variables and the motion equation is Among them, δx pred is the predicted value of the nominal state variable, Q is the noise covariance during the sampling time; In the update phase, the error state update process is: K=P pred H T (HP pred H T +V) -1 , δx=K(z-h(x pred )), x=x pred +δx, P=(I-KH)P pred . Where K is the Kalman gain, P pred is the predicted covariance matrix, z is the observed data, V is the covariance matrix of the observation noise, h is the observation equation, H is the Jacobian matrix of the relative error state of the observation equation, and P is the optimal covariance matrix; The matching result between the current frame point cloud and the map is the observation of translation and rotation. The observation value at a certain moment is p icp With R icp , the observation equation is p icp =p+δp, R icp =RExp(δθ). Among them, δp is the error state of the translation transformation, Exp(δθ) is the Lie algebraic representation corresponding to the nominal state rotation R; R icp is the observation of the error state δθ, and the update amount zh(x) in the update process is expressed as: z-h(x)=[p icp -p,Log(R T R icp )] T . The Jacobian matrices of the translation part and the rotation part are both unit matrices; The wheel speed meter data is a direct observation of the vehicle's speed in the forward direction. The Jacobian matrix of the nominal state speed is the unit matrix. The Kalman gain is calculated and the covariance is updated; Step S3.5: Compare the RFID coordinates in the data queue that are closest to the current frame point cloud time. If the position information deviation is greater than the preset value or is lost, repositioning is performed; Step S3.6: Use ROS communication to publish global positioning coordinates.

6. A rail vehicle positioning system based on laser radar and sensor information fusion, characterized in that: include: Module M1: Offline calibration of LiDAR and inertial navigation; Module M2: Establish vehicle posture estimation based on point cloud features according to LiDAR point cloud and inertial navigation data; solve graph optimization problems according to RFID coordinate observations and construct a globally optimized point cloud map; Module M3: Based on the point cloud map, match the map with real-time lidar data to obtain the vehicle positioning result; use the error state Kalman filter method to integrate the inertial navigation data for posture optimization to obtain the vehicle's precise positioning result.

7. The rail vehicle positioning system based on laser radar and sensor information fusion according to claim 6 is characterized in that: In the module M1: Module M1.1: External parameter calibration of LiDAR and inertial navigation: When the vehicle is stationary, obtain the point cloud data collected by the LiDAR and the inertial measurement data collected by the IMU, use open source tools to perform external parameter calibration, and calculate the three-axis coordinates and three-axis rotation quaternions of the IMU when the LiDAR is used as the reference coordinate system to form the translation vector and rotation matrix from the radar to the IMU; Module M1.2: Inertial navigation internal parameter calibration: When the vehicle is stationary, obtain the inertial measurement data collected by the IMU, and calculate the noise and zero bias through the readings of the accelerometer and gyroscope during the stationary time.

8. The rail vehicle positioning system based on laser radar and sensor information fusion according to claim 6, characterized in that: In the module M2: Module M2.1: Receiving and preprocessing the laser point cloud, using inertial navigation data to dedistort the laser point cloud, and rotating each data point of a frame scan to the end of the frame: in, is the coordinate of each point in the radar coordinate system at the end of the frame, p t It is the coordinate of a point at any time in a scan time. The subscript of the rotation matrix from right to left represents the transformation relationship T LI , T IL They are the transformation matrices from inertial navigation to lidar and from lidar to inertial navigation, T(t end ) IW and T(t) WI They are the position and posture of the inertial navigation at the end of the frame and at any time in one scan time; Module M2.2: Calculate the line beam of each point in the laser radar scanning frame through the laser radar's field of view and classify it, calculate the difference between each point in the line beam and other adjacent points, that is, the curvature, determine the corner points and plane points through the curvature of the line beam, and extract features from the laser point cloud; Module M2.3: Execute the matching of laser point cloud to local map, search for the nearest neighboring points of the corner point to be matched to fit the straight line, solve the distance from the corner point to be matched to the straight line and the components of the vertical vector in the three axis directions, and extract the stable corner point features; search for the nearest neighboring points of the plane point to be matched to fit the plane, judge the reliability of the plane by the distance from the point to the fitted plane, and extract the stable plane point features; solve the least squares problem based on the point-to-line and point-to-plane distances to obtain the radar odometer; Module M2.4: Use parallel mechanism to perform loop detection when performing matching; perform iterative closest point matching by searching historical keyframes to identify scenes that have been reached; Module M2.5: Use the graph optimization library GTSAM to add the radar odometer and RFID observation factors to the factor graph for optimization; if loop detection is performed, loop information is detected, and then the loop detection factor is added to obtain the globally optimized keyframe pose, and the keyframe laser point cloud is saved to the map.

9. The rail vehicle positioning system based on laser radar and sensor information fusion according to claim 6, characterized in that: In the module M3: Module M3.1: Receives lidar point cloud and sensor data, performs data synchronization, and performs positioning initialization based on RFID data; Module M3.2: Dedistortion of laser point cloud using inertial navigation data; Module M3.3: Use the occupancy grid map for map management and match the current scan with the map; implement the point-to-point ICP method based on the Gauss-Newton method, calculate the nearest neighbors of points in each iteration, and obtain the increment of the pose by solving the least squares problem; The least squares problem with vehicle posture as state variable is: Where D(x) is the distance and function of the corresponding point pair, x is the two-dimensional position of the current vehicle, is the scan point in the current scan frame in the world coordinate system, yes The nearest neighbor point in the map, i is the index of the current point pair, and n is the number of all points in this frame of point cloud.

10. The rail vehicle positioning system based on laser radar and sensor information fusion according to claim 9, characterized in that: In the module M3: Module M3.4: Adopt the error state Kalman filter method to fuse the vehicle wheel speed data, inertial navigation data and map matching results to perform vehicle track calculation. The specific process is as follows: In the prediction stage, the inertial navigation data is integrated to obtain the nominal state variable, and the error caused by noise is used as the error variable; In the update phase, the map matching results and wheel speed data are used as observations to update the mean and covariance of the error state. The two values ​​are used to describe the change of the error state. The updated error state is integrated into the nominal state variable and the error state is reset to zero to complete a cycle. The error state variable motion equation is linearized as δx(t+Δt)=Jδx+ω Among them, δx=[δp, δv, δθ, δb g ,δb a ,g] T is the error state variable, p is the translation, v is the velocity, b g and b a is the zero bias of the gyroscope and accelerometer, g is the acceleration of gravity, ω=[0,-η v ,-η θ ,η g ,η a ,0] T is the noise; t is the absolute time of the current moment, Δt is the time difference between two consecutive moments; η v is the error speed; η g is the measurement noise of the gyroscope; η θ is the rotation error; η a is the measurement noise of the accelerometer; J is the Jacobian matrix: Where I is the identity matrix, and Acceleration and angular velocity readings for inertial navigation; In the prediction stage, the predicted value of the noise covariance between the nominal state variables and the motion equation is δx pred =Jδx, P pred =JPJ T +Q, Among them, δx pred is the predicted value of the nominal state variable, Q is the noise covariance during the sampling time; In the update phase, the error state update process is: K=P pred H T (HP pred H T +V) -1 , δx=K(z-h(x pred )), x=x pred +δx, P=(I-KH)P pred . Where K is the Kalman gain, P pred is the predicted covariance matrix, z is the observed data, V is the covariance matrix of the observation noise, h is the observation equation, H is the Jacobian matrix of the relative error state of the observation equation, and P is the optimal covariance matrix; The matching result between the current frame point cloud and the map is the observation of translation and rotation. The observation value at a certain moment is p icp With R icp , the observation equation is p icp =p+δp, R icp =RExp(δθ). Among them, δp is the error state of the translation transformation, Exp(δθ) is the Lie algebraic representation corresponding to the nominal state rotation R; R icp is the observation of the error state δθ, and the update amount zh(x) in the update process is expressed as: z-h(x)=[p icp -p,Log(R T R icp )] T . The Jacobian matrices of the translation part and the rotation part are both unit matrices; The wheel speed meter data is a direct observation of the vehicle's speed in the forward direction. The Jacobian matrix of the nominal state speed is the unit matrix. The Kalman gain is calculated and the covariance is updated; Module M3.5: Compare the RFID coordinates in the data queue that are closest to the current frame point cloud time. If the position information deviation is greater than the preset value or is lost, repositioning is performed; Module M3.6: Use ROS communication to publish global positioning coordinates.

Images