An unmanned vehicle relocation method based on LiDAR / GPS / IMU fusion

Abstract

The application discloses an unmanned vehicle repositioning method based on LiDAR / GPS / IMU fusion, first, laser radar and IMU sensor information are utilized to complete the construction of the surrounding environment point cloud map of the driving area of the unmanned vehicle; a GPS module is adopted to perform coarse positioning, the approximate position of the unmanned vehicle in the built prior map is determined, then an NDT algorithm is further used for positioning to obtain the global initial pose; high-frequency IMU information is utilized to pre-process the point cloud, the screened point cloud is used for feature extraction, and the laser odometry result is obtained by matching the prior map, the system state and the covariance matrix are updated, and finally the relatively accurate local pose is output. Compared with the traditional NDT positioning method, the positioning robustness and accuracy are greatly improved.

Description

TECHNICAL FIELD

[0001] The application mainly relates to a LiDAR / GPS / IMU fusion-based unmanned vehicle repositioning method and belongs to the technical field of multi-sensor fusion positioning. BACKGROUND

[0002] The positioning scheme based on the matching technology is widely applied in the automatic driving field and is famous for high matching precision and accurate attitude estimation results, however, the scheme is quite strict to the initial value, only when the initial position and attitude are good, the algorithm can ensure accuracy and timeliness. In the case of no initial value, direct matching between the current frame data and the global map will cause the search range to be too large and the platform computing load to be too large, thereby causing the algorithm to fail.

[0003] In an open environment, pure satellite positioning performs well. However, in a complex environment, such as a building-dense area and a tall tree area, a multipath effect is prone to occur. Similarly, in a closed area such as a tunnel and an underground area, signal loss becomes a problem. In addition, when the GNSS signal jumps, the attitude solution may also produce errors. Therefore, for the above reasons, the pure GNSS scheme is gradually abandoned, and instead, a multi-sensor fusion positioning scheme is used.

[0004] When three-dimensional laser radar data is used to construct a large-range environment map, a problem inevitably occurs, that is, whether the robot has moved to a place that has been mapped before. Due to the existence of cumulative error, the maps constructed by the robot when passing through the same place twice are often not closed, and therefore, in order to ensure the accuracy and precision of the map, it is necessary to detect whether the robot has run in a certain environment, so the research on repositioning is of great significance. SUMMARY

[0005] The technical problems to be solved by the present application are that the positioning robustness needs to be improved in the prior art and the repositioning technology in a large scene point cloud map cannot run on a platform with limited computing power, and a LiDAR / GPS / IMU fusion-based unmanned vehicle repositioning method is provided, which first divides the positioning problem into two parts, namely global initial pose determination and local pose determination; the global initial pose is determined by using GPS to assist in determining a rough position, and then combined with the traditional matching method DNT for fine matching. Then a fusion positioning scheme based on LiDAR / IMU / Prior Map is designed, the original point cloud is preprocessed through high-frequency IMU information, the edge features and plane features of the processed point cloud observation are extracted, and matched with the prior map (Prior Map) to obtain the laser odometry result. The system state and covariance matrix are updated in a filtering-based manner to estimate the real-time pose of the vehicle, thereby realizing position positioning. Compared with the traditional positioning method, the positioning robustness and accuracy are greatly improved. To solve the above technical problems, the technical scheme adopted by the present application is a LiDAR / GPS / IMU fusion-based unmanned vehicle repositioning method, which comprises the following steps:

[0006] S1: global initial pose determination: first, the GPS module is used for rough positioning to determine the approximate position of the unmanned vehicle in the prior map; when collecting the laser radar point cloud data, the GPS data is received synchronously, and the latitude and longitude of the GPS are labeled on the point cloud track according to the time stamps of the laser radar and the GPS; when the initial state of the unmanned vehicle is at a random position in the prior map, the labeled latitude and longitude information is queried in the prior map and compared, the deviation is calculated, and the nearest labeled position to the position is taken as the preliminary position of the unmanned vehicle in the prior map, thereby completing the rough positioning of the vehicle,

[0007] S2: laser radar point cloud matching with local map based on NDT: the straight-through filter intercepts the point cloud near the GPS rough positioning, and the point cloud graph intercepted by the straight-through filter is taken as the matched point cloud P={p1, p2, …p3}, and the real-time measurement data of the laser radar is taken as the to-be-matched point cloud Q={q1, q2, …q3}, the two point clouds are taken as the input of the NDT algorithm, and the precise positioning on the map is realized,

[0008] S3: matching optimization: after determining the global initial pose, the high-frequency IMU information is used to pre-process the subsequent radar frame point cloud to remove distortion and improve the quality of the point cloud; then feature extraction is performed, and the curvature feature index is used here, and all points are divided into edge points, plane points and general points according to the curvature of each point; the feature points of the kth frame scanning are matched with the prior map, and the pose transformation relationship of the current frame point cloud relative to the prior map is obtained, thereby providing the output of the odometry, and therefore the residual equation is constructed and optimized,

[0009] S4: Filter fusion mode determines local pose: the IEKF filter takes the pose obtained by the IMU pre-integration module as the prediction, and takes the pose estimation result obtained by the feature-based laser odometry as the observation, fuses the prediction and the observation data to update the system state, and outputs the final local positioning result, thereby constructing an IMU and laser radar tightly coupled local positioning method based on the iterative extended Kalman filter.

[0010] In the step S1, the rough initial pose of the robot relative to the prior map is determined by using the GPS information as follows:

[0011] S1.1 When the unmanned vehicle is in motion, a SLAM system is applied to the collected 3D laser radar data to construct a prior point cloud map, and the trajectory of the unmanned vehicle in the map is retained; according to the time stamps of the laser radar and the GPS, the latitude and longitude of the point cloud trajectory are labeled; the GPS module data update frequency is 1 Hz, and the latitude and longitude information is labeled once every 1 s; through the above operation, the coordinates of the corresponding points in the existing prior map are one-to-one corresponding to the latitude and longitude,

[0012] S1.2 When the initial state of the unmanned vehicle is at a random position in the prior map, the latitude and longitude information of the position is obtained through the GPS module, the labeled latitude and longitude information in the prior map is sequentially queried and compared, the deviation is calculated, and the labeled position with the shortest Euclidean straight line distance from the position is taken as the preliminary position of the unmanned vehicle in the prior map, thereby completing the coarse positioning of the vehicle.

[0013] In the step S2, the laser radar point cloud and local map matching method based on NDT is as follows:

[0014] S2.1 The real-time point cloud is taken as the source point cloud The local prior map is taken as the target point cloud The estimated real-time pose is a rotation and translation of the source point cloud:

[0015]

[0016] wherein, is a transformation matrix, R and t are rotation and translation components, and s is a parameter to be solved q′ i is the transformed source point cloud.

[0017] S2.2 The objective function constructed when performing NDT matching is:

[0018]

[0019] wherein, N q represents the number of point clouds, μ represents the mean, and Σ represents the covariance.

[0020] S2.3 The residual function for associating real-time point cloud with prior map is:

[0021] f i (p)=q′ i -μ

[0022] where f(·) represents the probability density function;

[0023] The Jacobian matrix J(p) of the residual function with respect to the parameters to be solved s is calculated, and the incremental equation H△x k =g, where H=J(p)·J(p) T ,g=-J(p)·f(p), is solved, and the parameters to be solved s, i.e. the translation and rotation components, can be iteratively optimized. The matching optimization mode in step S3 is as follows:

[0024] S3.1 The curvature is referenced as an index to extract feature points from the radar frame point cloud, project them into the prior map coordinate system, and find n points corresponding to the target points in the global map. The point-to-line and point-to-plane distances are calculated, and the corner point residual equation is defined as follows:

[0025]

[0026] wherein, represents the feature point coordinate of the i-th feature point in the world coordinate system converted by the estimated radar pose at the k+1 time, and respectively represent two coordinate points of the i-th corner feature point on the edge feature corresponding to the map, S3.2 for the face feature point, first construct an overdetermined equation using the five feature points closest to the face feature point in the face point map, then calculate the face feature pa·x+pb·y+pc·z+pd=0 corresponding to the face feature point through QR decomposition, and the face feature point residual equation constructed based on this is as follows:

[0027]

[0028] where x=[x,y,z] T represents the face feature point in the radar coordinate system converted to the world coordinate system by the estimated radar pose.

[0029] S3.4 The optimal global pose estimation is obtained by minimizing the point-to-line and point-to-plane distances

[0030] wherein ε is a corner point, p is a face point, f(*) is a robust kernel function, and N(ε) and N(p) represent the number of corner points and face points.

[0031] Compared with existing technologies, the advantages of this invention are: it proposes a LiDAR / IMU / GPS fusion positioning method. First, the Global Positioning System (GPS) provides absolute positioning data with small errors, offering relatively accurate initial pose estimation to reduce search time on the map and resource consumption. Next, based on coarse GPS positioning, the NDT algorithm is used for further matching to achieve precise positioning on the prior map. High-frequency IMU information is used to process the point cloud before frame-to-prior map matching. Finally, a filter is used to fuse observation and prediction data to estimate local pose information, further improving the accuracy and stability of positioning on a known map. This invention solves the resource consumption and coarse accuracy problems of traditional relocation techniques, and its effect is more robust than direct matching. Attached Figure Description

[0032] Figure 1 This is a flowchart of the steps of the present invention;

[0033] Figure 2 This is a flowchart of the global initial pose determination process of the present invention;

[0034] Figure 3 This is a framework diagram of the filtering and fusion algorithm of the present invention;

[0035] Figure 4 This is a flowchart of the IEKF algorithm. Detailed Implementation

[0036] The present invention will be further illustrated below with reference to the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are for illustrative purposes only and are not intended to limit the scope of the present invention.

[0037] Example 1: A method for relocalizing unmanned vehicles based on LiDAR / GPS / IMU fusion, such as... Figure 1 As shown, the specific steps are as follows: Step S1: Determining the global initial pose. The main idea of ​​this step is to first use GPS for rough positioning, which can speed up the search efficiency during repositioning. The latitude and longitude information of the current location of the unmanned vehicle is viewed and saved in real time on the computer through a visual interface software. Based on the timestamps of the LiDAR and GPS, GPS latitude and longitude are marked on the point cloud trajectory, so that the coordinates of the corresponding points in the existing prior map correspond one-to-one with the latitude and longitude.

[0038] When the autonomous vehicle initially appears at a random location on the prior map, it obtains the latitude and longitude information of that location via GPS. It then sequentially queries and compares the latitude and longitude information marked on the prior map, calculating the deviation. The marked location with the shortest Euclidean straight-line distance from this initial location is taken as the vehicle's preliminary position on the prior map, completing the vehicle's coarse localization.

[0039] Step S2: NDT-based matching of laser radar point cloud and local map

[0040] The point cloud of 15*15*15m around the initial position is intercepted as the matched point cloud P, and the laser radar real-time measurement data is rotated as the to-be-matched point cloud Q, as shown in formula (1):

[0041]

[0042] Further positioning is performed in the intercepted point cloud using the NDT algorithm, and the general idea is shown in FIG. 2. Figure 2 The accurate position of the unmanned vehicle in the prior map is obtained, and the maximum joint probability is taken as the objective function, as shown in formula (2):

[0043]

[0044] f(·) represents a probability density function, is a six-dimensional parameter to be solved. If the segmented grid contains five or more points, the mean and covariance of the grid are calculated, as shown in formula (3) and formula (4):

[0045]

[0046]

[0047] wherein μ represents the mean, Σ represents the covariance, represents the points in the grid, N x represents the number of points in the grid. According to the predicted pose, the source points are rotated and translated:

[0048]

[0049] In formula (1), T represents the conversion from the radar coordinate system to the world coordinate system.

[0050] At this time, all points are converted to the world coordinate system. The joint probability can be obtained as:

[0051]

[0052] By taking the logarithm and the like, the objective function and the residual function are obtained:

[0053]

[0054] f i (p)=q′ i -μ (8)

[0055] Finally, the Jacobian matrix J(p) of the residual function with respect to the to-be-solved parameter s is calculated, and the incremental equation H△x is solved​k = g, where H = J(p) · J(p) T , g = -J(p) · f(p), the parameters to be solved s, i.e. translation and rotation components, can be iteratively optimized. Step S3: matching optimization,

[0056] After determining the global initial pose, the subsequent radar frame point cloud is preprocessed to remove distortion using high-frequency IMU information to improve point cloud quality. Then feature extraction is performed, where the curvature feature index is used, as shown in equation (9),

[0057]

[0058] where, and represent the coordinates of the i-th and j-th scanning points on the l-th laser beam in the k-th frame of data, represent the coordinates of the i-th and j-th scanning points on the l-th laser beam in the k-th frame of data,

[0059]

[0060] where, represents the feature point coordinates in the world coordinate system converted by the estimated radar pose at time k+1, and respectively represent the two coordinate points of the i-th corner feature point on the corresponding edge feature in the map. For face feature points, first construct an overdetermined equation using the five nearest feature points in the face point map, then calculate the face feature pa·x+pb·y+pc·z+pd=0 corresponding to the face feature point through QR decomposition, and then construct the face feature point residual error equation based on this as follows:

[0061]

[0062] where x = [x, y, z] T represents the coordinates of the face feature point in the radar coordinate system converted to the world coordinate system by the estimated radar pose, and the optimal global pose estimate is obtained by minimizing the distance from the point to the line and the distance from the point to the plane

[0063]

[0064] Where f(*) is the robust kernel function, and N(ε) and N(p) represent the number of corner points and face points, respectively. The laser odometry pose estimate is obtained by solving the nonlinear equation using the Gauss-Newton method. Step S4, determining the local pose through filtering and fusion: the filtering and fusion approach is as follows... Figure 3 As shown, the IEKF equations for the motion and observation model are as follows, for the operating point x op,k Linearization yields:

[0065]

[0066] At this point, the equations of motion remain unchanged, and both the observation model and its Jacobian matrix are in x. op,k Linearization is performed as follows:

[0067] y op,k =g(x op,k ,0) (14)

[0068]

[0069]

[0070] Using the linearized model above, we can obtain the five basic equations for the IEKF recursive update:

[0071] The predicted state equation is: The predicted covariance matrix is: The gain matrix is: The state equation for measurement updates is: The covariance equation for measurement updates is: In its first iteration, IEKF made Then, the Kalman gain matrix and posterior mean are repeatedly calculated, and in each iteration, the posterior mean of the previous iteration is used. Set as current working point x op,k .

[0072] when At this point, the Kalman gain and update equations have converged, and the final equation is then output. The IEKF algorithm flowchart is shown below. Figure 4 As shown;

[0073] While the application has been described with reference to particular embodiments thereof, it is to be understood that these embodiments are merely illustrative of the principles and applications of the present application. It will be apparent to those skilled in the art that numerous modifications can be made within the scope of the present application as defined by the appended claims. It is intended that all such modification fall within the spirit and scope of the present application. It will be understood that the features described in connection with one embodiment can be used in connection with another embodiment.

Claims

1. A LiDAR / GPS / IMU fusion-based unmanned vehicle relocation method, characterized in that, Comprising the following steps: S1: Global initial pose determination: First, coarse positioning is performed using a GPS module to determine the approximate position of the unmanned vehicle in the existing prior map; when collecting laser radar point cloud data, GPS data is received synchronously, and the latitude and longitude of the point cloud trajectory is labeled according to the time stamps of the laser radar and the GPS; when the initial state of the unmanned vehicle is at a random position in the prior map, the labeled latitude and longitude information in the prior map is queried and compared, the deviation is calculated, and the nearest labeled position to the position is taken as the preliminary position of the unmanned vehicle in the prior map, completing the coarse positioning of the vehicle, S2: Laser radar point cloud and local map matching based on NDT: A straight-through filter intercepts the point cloud near the GPS coarse positioning, and the point cloud graph intercepted by the straight-through filter is taken as the matched point cloud P = {p1, p2, … p3}, and the real-time measurement data of the laser radar is taken as the to-be-matched point cloud Q = {q1, q2, … q3}, and the two point clouds are taken as the input of the NDT algorithm, and the NDT algorithm is run to achieve accurate positioning on the map, S3: Matching optimization: After determining the global initial pose, the high-frequency IMU information is used to pre-process the subsequent radar frame point cloud to remove distortion and improve the quality of the point cloud; then feature extraction is performed, and in this case, the curvature feature index is used, and according to the curvature of each point, all points are divided into edge points, plane points and general points; the feature points of the kth frame scanning are registered with the prior map, and the pose transformation relationship of the current frame point cloud relative to the prior map is obtained, and the output of the odometer is provided, so the residual equation is constructed and optimized, S4: Filter fusion mode determines the local pose: The IEKF filter takes the pose obtained by the IMU pre-integration module as the prediction, and takes the pose estimation result obtained by the feature-based laser odometer as the observation, fuses the prediction and observation data to update the system state, and outputs the final local positioning result, and constructs an IMU and laser radar tightly coupled local positioning method based on the iterative extended Kalman filter.

2. The method of claim 1, wherein, The way of using GPS information to determine the rough initial pose of the robot relative to the prior map in step S1 is as follows: S1.1 When the unmanned vehicle is in motion, a SLAM system is applied to the collected 3D laser radar data to construct a prior point cloud map, and the trajectory of the unmanned vehicle in the map is retained; the latitude and longitude of the GPS is labeled on the point cloud trajectory according to the time stamps of the laser radar and the GPS; the data update frequency of the GPS module is 1Hz, so the latitude and longitude information is labeled once every 1s; through the above operation, the coordinates of the corresponding points in the existing prior map are one-to-one corresponding to the latitude and longitude, S1.2 When the initial state of the unmanned vehicle is at a random position in the prior map, the latitude and longitude information of the position is obtained through the GPS module, the labeled latitude and longitude information in the prior map is sequentially queried and compared, the deviation is calculated, and the nearest labeled position to the position in the Euclidean straight line distance is taken as the preliminary position of the unmanned vehicle in the prior map, completing the coarse positioning of the vehicle.

3. The method of claim 2, wherein: The way of matching laser radar point cloud and local map based on NDT in step S2 is as follows: S2.1 Take the live point cloud as the source point cloud The local prior map as the target point cloud The estimated live pose is a rotation and translation of the source point cloud: wherein, is a transformation matrix, R and t are rotation, translation components, s is the parameter to be solved q′ i is the transformed source point cloud, S2.2 The objective function constructed when performing NDT matching is: where N q denotes the number of points, μ denotes the mean, and Σ denotes the covariance. S2.3 The residual function for associating real-time point cloud with prior map is: f i (p) = q' i - μ Wherein, f(*) represents the probability density function; The Jacobian matrix J(p) of the residual function with respect to the parameters s to be solved is calculated, and the increment equation H△x is solved simultaneously k = g, where H = J(p) · J(p) T , g = -J(p) · f(p), the parameters s to be solved, i.e. the translation and rotation components, can be iteratively optimized.

4. The method of claim 1, wherein: The matching optimization mode in step S3 is as follows: S3.1 Refer to the index of curvature, extract feature points from the radar frame point cloud, project them into the prior map coordinate system, and find n points corresponding to the target point in the global prior map. The distance between the point and the line and the distance between the point and the plane are calculated. The corner point residual error equation is defined as follows: wherein, represents the feature point coordinate of the i-th feature point in the world coordinate system at time k+1 through the estimated radar pose transformation, and respectively represent the two coordinate points of the i-th corner feature point on the corresponding edge feature in the map, S3.2 For face feature points, first construct an overdetermined equation using the five closest feature points in the face point map, then calculate the face feature pa·x+pb·y+pc·z+pd=0 corresponding to the face feature point through QR decomposition, and construct the face feature point residual error equation based on this as follows: where x = [x, y, z] T represents the coordinates of the planar feature points in the world coordinate system through the estimated radar pose. S3.4 The optimal global pose estimate is found by minimizing the point-to-line and point-to-plane distances Wherein, ε is the corner point, p is the face point, f(*) is the robust kernel function, N(ε) and N(p) represent the number of corner points and face points, S3.4 obtains the laser odometry pose estimation by solving the nonlinear equation through the Gauss-Newton method.

5. The method of claim 1, wherein: Wherein the unmanned vehicle needs to carry multiple sensors including: laser radar, inertial sensor, satellite communication module.

Images