An autonomous positioning method for unmanned vehicles based on multi-sensor fusion

Abstract

The present invention relates to an autonomous positioning method for an unmanned vehicle based on multi-sensor fusion. The GPS and IMU sensors are used to achieve rough positioning in the point cloud map, and the precise initial positioning is optimized according to the matching degree of laser point cloud registration. Then, a method of fusing the IMU and lidar is used to achieve high-performance real-time positioning. The mutual fusion of multi-sensor data in the present invention effectively improves the accuracy and real-time performance of autonomous positioning.

Description

Technical Field

[0001] The present invention relates to the technical field of measurement and testing, belongs to the field of autonomous driving, and particularly relates to an autonomous positioning method for an unmanned vehicle based on multi-sensor fusion under a point cloud map. Background Art

[0002] An unmanned vehicle is an autonomous mobile platform that uses an on-vehicle sensing system to perceive the environment around the vehicle, plan the route by itself, and control the vehicle to successfully reach the destination. It is usually equipped with on-vehicle sensors such as a global positioning system (GPS), an inertial sensor (IMU), and a multi-line lidar. An unmanned vehicle has three functional modules: perception, decision-making, and control. Among them, the positioning module is the uppermost sub-module in the perception module and is the basis of the entire unmanned driving system.

[0003] The invention patent "A positioning initialization method and device, a positioning method and device, and a mobile device" with the application number 201911299726.9 proposes to determine search points around an initial point, use multi-threading to implement the registration of lidar point cloud and global point cloud map, and sort the matching result scores to obtain the optimal pose. However, in actual applications, if the pose of the lidar at startup is far from the given initial point, correct registration cannot be obtained.

[0004] The invention patent "A method for optimizing lidar positioning using radius search" with the application number 201911015902.1 proposes a multi-threaded programming scheme. Centered on the input GPS coordinates, multiple pose parameters are generated within a range with a radius of R and a height of H, and continuous attempts are made to match until the set matching accuracy threshold S is met and then it ends. However, the search and registration method of this invention has relatively large randomness, poor search and registration convergence, and cannot guarantee rapid convergence of registration. Summary of the Invention

[0005] The present invention solves the problems existing in the prior art and provides an optimized autonomous positioning method for an unmanned vehicle based on multi-sensor fusion. The mutual fusion of multi-sensor data effectively improves the accuracy and real-time performance of autonomous positioning.

[0006] The technical solution adopted by the present invention is an autonomous positioning method for an unmanned vehicle based on multi-sensor fusion. The method uses GPS and IMU sensors to achieve rough positioning of the unmanned vehicle in the point cloud map, optimizes to obtain the accurate initial positioning of the unmanned vehicle according to the matching degree of lidar point cloud registration, and uses the fusion of IMU sensors and lidar to complete the real-time positioning of the unmanned vehicle.

[0007] Preferably, the method includes the following steps:

[0008] Step 1: Based on GPS, utilize the advantage of GPS global positioning and use the GPS sensor data as the global positioning when the unmanned vehicle starts.

[0009] Step 2: Based on the IMU sensor, obtain the yaw angle of the unmanned vehicle.

[0010] In the present invention, since the GPS sensor can only obtain position information and cannot obtain attitude information, but the attitude positioning of the driverless vehicle is equally important, the magnetometer in the IMU sensor is used to calculate the yaw angle of the driverless vehicle.

[0011] Step 3: Calibrate the global positioning and the point cloud map to obtain the rotation matrix R and the translation vector t.

[0012] In the present invention, since the poses of the unmanned vehicle obtained by the GPS and IMU sensors and the coordinate system of the point cloud map are different, it is necessary to calibrate the rotation matrix R and the translation vector t of the GPS and IMU positioning.

[0013] Step 4: Obtain the fine positioning from the global positioning to the point cloud map.

[0014] In the present invention, considering that the errors of the GPS and IMU sensors are both large, they cannot be directly used as positioning data; set the GPS translation error to be 10m to 20m, and the IMU yaw angle error to be π / 4 to π / 2, and then use the matching degree of NDT registration as the optimization object, and use intelligent optimization algorithms such as the CRS algorithm that do not require gradients to find the optimal pose of the point cloud registration.

[0015] Step 5: Use IMU integrated positioning between lidar data frames as the initial value for the next frame of point cloud registration. After each successful point cloud registration, update the attitude and speed of the unmanned vehicle, and then accumulate and update the pose through the angular velocity and linear acceleration data of the IMU sensor between lidar frames.

[0016] In the present invention, when the driverless vehicle starts, it is necessary to obtain the initial pose of the point cloud registration according to Step 1, Step 2, and Step 4. After that, only the IMU and multiple lidars can largely ensure the requirements of real-time positioning.

[0017] In the present invention, the IMU sensor, which has gyroscope and accelerometer sensors, has the advantage of high frequency and can obtain the real-time pose through integration, but it has cumulative errors and cannot be eliminated; the multi-line lidar can obtain accurate positioning through the method of point cloud registration, but the frame rate is low. If there is a relatively large pose change of the driverless vehicle between point cloud frames, it may cause positioning loss; therefore, use IMU integrated positioning between lidar data frames to provide the initial value for the next frame of point cloud registration, and update the attitude and speed after successful point cloud registration for use in IMU integrated positioning.

[0018] Step 6: Dynamically load the point cloud map to achieve autonomous positioning of the driverless vehicle.

[0019] In the present invention, in most cases, the file of the point cloud map is large. If the entire map is directly used as the target point cloud for NDT registration, it will occupy a large amount of memory and even cause memory overflow. Based on this, dynamically loading and constructing the currently required point cloud map can theoretically support scenarios with an infinitely large area.

[0020] In the present invention, dynamically loading the point cloud map can not only reduce memory occupancy but also reduce the time for loading the point cloud map.

[0021] Preferably, in step 1, the longitude, latitude, and altitude data are converted into Cartesian coordinate data x and y.

[0022]

[0023] Among them, the longitude, latitude, and altitude of the origin of the GPS coordinate system are OrgLong, OrgLat, and OrgAlt, the longitude, latitude, and altitude of the driverless vehicle corresponding to the current GPS sensor data are long, lat, and alt, the radius of the earth is r, the eccentricity of the earth is e, Re0 and Re1 are the curvature radii of the origin and the current earth ellipsoid respectively, and the coordinates of the origin and the current earth center are (x0, y0, z0) and (x1, y1, z1) respectively.

[0024] Preferably, in step 2, the yaw angle y of the driverless vehicle is calculated by formula (2).

[0025]

[0026] Among them, the pitch angle in the IMU sensor data is p, the roll angle is r, and the three-axis magnetometers are mag_x, mag_y, and mag_z respectively.

[0027] Preferably, in step 3, the initial pose is set manually, and the pose obtained by NDT registration is used as the actual value. The rotation matrix R and the translation vector t are obtained by solving the least squares using formula (3).

[0028]

[0029] Among them, x i ’, y i ’ and θi’ are the poses calculated by the GPS and magnetometers of the i-th frame; x i , y i and θi are the actual poses obtained by NDT registration of the i-th frame.

[0030] Preferably, in step 4, set the GPS translation error and the IMU yaw angle error, take the matching degree of NDT registration as the optimization object, and use the CRS algorithm to find the optimal pose of point cloud registration; finally, calculate the least squares according to the residual equation of formula (3) to update R and t.

[0031] Preferably, step 5 includes the following steps:

[0032] Step 5.1: Integrate to obtain the pose according to formula (4),

[0033]

[0034] where a is the linear acceleration of the accelerometer in the IMU, dt is the time interval, v1 and w1 are the linear velocity of the previous frame and the angular velocity of the gyroscope in the IMU, v2 and w2 are the linear velocity of the current frame and the angular velocity of the gyroscope in the IMU, x is the position, and yaw is the yaw angle;

[0035] Step 5.2: Use the point cloud to construct a normal distribution of multi-dimensional variables, use a three-dimensional voxel network to divide the point cloud, and calculate the probability density; input the initial search pose, start the NDT matching search, judge the fitting degree of the two point clouds, and iteratively find the optimal solution of the three-dimensional transformation, which is the pose of the current lidar point cloud in the point cloud map;

[0036] Step 5.3: Update the pose and speed of the driverless vehicle.

[0037] Preferably, in step 6, pre-divide the file of the point cloud map into several point clouds according to the square range, and at the same time downsample and denoise the segmented point clouds, save all the segmented point clouds in multiple threads, and record their lists and ranges in the file.

[0038] Preferably, read the file list of all point clouds, screen out the point clouds with a plane side length of 150m around the current driverless vehicle according to the real-time position, judge the point cloud files that need to be reloaded, remove the point clouds that do not need to be loaded, and then load the remaining point cloud files in multiple threads and splice them into a dynamically loaded point cloud map.

[0039] The present invention relates to an optimized autonomous positioning method for driverless vehicles based on multi-sensor fusion, which uses GPS and IMU sensors to achieve rough positioning in the point cloud map, optimizes to obtain an accurate initial positioning according to the matching degree of laser point cloud registration, and then uses the method of fusing IMU and lidar to achieve high-performance real-time positioning.

[0040] The beneficial effect of the present invention is that the mutual fusion of multi-sensor data effectively improves the accuracy and real-time performance of autonomous positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 This is the flowchart of the present invention.

[0042] Figure 2 This is the implementation flowchart of the present invention.

[0043] Figure 3 This is the flowchart for obtaining the poses of GPS and IMU sensors.

[0044] Figure 4 This is the flowchart for calibrating the poses of GPS and point cloud map.

[0045] Figure 5 This is the flowchart for precise positioning from global positioning to point cloud map.

[0046] Figure 6 This is the flowchart for dynamic loading of point cloud map. Specific implementation manners

[0047] The following further describes the present invention in detail with reference to embodiments, but the protection scope of the present invention is not limited thereto.

[0048] The present invention relates to a positioning method for an autonomous vehicle based on multi-sensor fusion under a point cloud map, which is implemented based on the Robot Operating System (ROS) platform; the lidar adopts the RSHELIOS model of RoboSense, with a scanning frequency of 10Hz / 20Hz, a horizontal angular resolution of 0.1° / 0.4°, and a vertical scanning angle of 70° (-55° to +15°); the IMU adopts the ART-IMU-02A nine-axis IMU, which has three-axis gyroscopes, three-axis accelerometers, and three-axis magnetometers, and this IMU can collect data at 100hz. The GPS sensor adopts the TOP102 high-precision sub-meter receiver of TOPGNSS and uses the NMEA data protocol to output longitude, latitude, and altitude data at a frequency of 1Hz by default; the main control operating system of the autonomous vehicle is Ubuntu18.04 + ROS Melodic.

[0049] The present invention entirely adopts a right-handed coordinate system, and the placement positions of the IMU sensor and the lidar only have translation and no rotation transformation; all three sensors read the sampled data through ROS node programs and publish the collected data using the standard data format of the ROS operating system.

[0050] The present invention also needs to obtain a global point cloud map in advance using SC-LIO-SAM or SC-LeGO-LOAM.

[0051] As Figure 1 shown, a positioning method for an autonomous vehicle based on multi-sensor fusion under a point cloud map mainly includes the following steps:

[0052] (1) Global positioning based on GPS sensor data;

[0053] (2) Yaw angle acquisition based on IMU sensor data;

[0054] (3) Global positioning and point cloud map calibration;

[0055] (4) Fine positioning from global positioning to point cloud map;

[0056] (5) Positioning based on IMU integration and multi-line lidar registration;

[0057] (6) Dynamic loading of point cloud map.

[0058] Step (1) specifically includes:

[0059] What the GPS sensor directly outputs is the earth longitude and latitude data. However, the unmanned vehicle positioning system uses rectangular coordinate data. Therefore, it is necessary to convert the longitude and latitude data into rectangular coordinate data. Set the origin longitude OrgLong = 2.09509, the origin latitude OrgLat = 0.52735, and the origin altitude OrgAlt = 20.0m. The current GPS longitude, latitude and altitude values are long, lat and alt, the earth radius is r = 6378137m, and the earth eccentricity e = 0.0818191908425. Then calculate the rectangular coordinate data x and y according to Equation (1):

[0060]

[0061] Step (2) specifically includes:

[0062] The IMU sensor used in the present invention will assign the initial value of the yaw angle to zero. Therefore, the magnetometer data is used to obtain the yaw angle. Set the pitch angle as p, the roll angle as r, and the three-axis magnetometers are mag_x, mag_y and mag_z. Then obtain the yaw angle y of the unmanned vehicle according to Equation (2):

[0063]

[0064] Step (3) specifically includes:

[0065] First, manually set the initial pose and use NDT registration for positioning. Then, use the Ceres library to set the rotation angle range in the rotation transformation matrix from -π to π. Subscribe to the current pose and GPS data. Since the GPS frame rate is much lower than the positioning frame rate, the data is added to the residual equation constructed by Ceres based on the GPS data callback function. The residual equation is shown in Equation (3). The poses of the sensor and DNT registration each time form a loss, and the losses accumulated many times form an overdetermined system of equations. Finally, the intermediate variables R and t can be obtained by the method of least squares, which is a common technical means in the field of robotics. Control the unmanned vehicle to move within the range of the point cloud map, covering as large a range as possible. Finally, after collecting enough data, Ceres solves the least squares to obtain the rotation matrix R and the translation vector t.

[0066]

[0067] Step (4) specifically includes:

[0068] First, obtain the rough positioning pose using the global positioning obtained in steps (1) and (2) and the rotation matrix and translation vector obtained in step (3). Then, set the Nlopt library to use the GN_CRS2_LM algorithm, set three optimization parameters, namely x, y, and yaw. The optimization parameter range is the offset of the rough positioning, with an x and y offset of 10 m and a yaw offset of π / 4. The return value of the optimization objective function is the matching degree of the NDT registration. The optimization end condition is an optimization time of 1 s, that is, after NDT registration, a result similar to the similarity is obtained, which is the matching degree. The greater the matching degree, the more reliable the position. Then, search for the position with a greater matching degree within the range through the CRS algorithm. Finally, obtain a more accurate positioning; finally, use the optimization result as the current pose and perform NDT registration again to obtain the fine positioning.

[0069] Step (5) specifically includes:

[0070] (5-1) Integrate the pose of the unmanned vehicle between laser point cloud frames using high-frequency IMU data. Due to the motion characteristics of the unmanned vehicle, the change in the z-axis of the unmanned vehicle is ignored, and only the pose integration change in the plane is considered. The pose obtained by integration according to Equation (4):

[0071]

[0072] (5-2) Use the NDT algorithm for registration to obtain the relative pose and current speed between the current frame of lidar point cloud and the point cloud map. Select the ndt-omp open-source package, which provides an OpenMP-enhanced NDT algorithm derived from PCL and uses a multi-threaded method to accelerate the NDT registration speed, which can be 10 times faster than the original version in PCL. The input point cloud of the lidar is first downsampled at a resolution of 1m. Then, assuming that the unmanned vehicle moves with uniform variable acceleration between lidar point cloud frames, the current speed of the unmanned vehicle is calculated.

[0073] Step (6) specifically includes:

[0074] (6-1) Read the pcd file of the global point cloud map through the PCL point cloud library, traverse all the point clouds to determine the x and y ranges of the point cloud map on the plane, and divide it into many square point clouds with a plane side length of 30m. Then use a resolution of 0.1 to downsample the segmented point clouds to effectively remove point cloud noise. Finally, save all the segmented point clouds through OpenMP multi-threading, and record their lists and ranges in a csv file.

[0075] (6-2) When the positioning program runs, it dynamically loads the nearby point clouds in real time according to the current pose and stitches them into an entire point cloud map. First, read the csv file list of all point clouds. Then, according to the real-time pose, filter out the point clouds with a plane side length of 150m around the current unmanned vehicle, judge the point cloud files that need to be reloaded, remove the point clouds that do not need to be loaded, and load the point cloud files through OpenMP multi-threading. Finally, stitch them into a dynamically loaded point cloud map.

[0076] Those skilled in the art should understand that the embodiments of the present invention can be provided as a method, a system, or a computer program product. Therefore, the present invention can take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present invention can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0077] The present invention is described with reference to the flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to the embodiments of the present invention. It should be understood that each process and / or block in the flowcharts and / or block diagrams, and the combination of processes and / or blocks in the flowcharts and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing devices generate for implementing in the process Figure 1 a process or multiple processes and / or blocksFigure 1 means for the functions specified in one or more boxes.

[0078] These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to work in a particular manner, such that the instructions stored in the computer-readable memory produce a manufacture including an instruction means that implements the functions specified in one Figure 1 one or more processes and / or boxes Figure 1 or more boxes.

[0079] These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus, such that a series of operation steps are performed on the computer or other programmable apparatus to produce a computer-implemented process, so that the instructions executed on the computer or other programmable apparatus provide steps for implementing the functions specified in one Figure 1 one or more processes and / or boxes Figure 1 or more boxes.

[0080] Although the preferred embodiments of the present invention have been described, additional changes and modifications can be made by those skilled in the art once they learn of the basic creative concept. Therefore, the appended claims are intended to be construed to include the preferred embodiments as well as all changes and modifications that fall within the scope of the present invention.

[0081] Obviously, those skilled in the art can make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalent technologies, the present invention is also intended to include these modifications and variations.

Claims

1. An autonomous positioning method for a driverless vehicle based on multi-sensor fusion, characterized in that: The method uses GPS and IMU sensors to achieve rough positioning of the unmanned vehicle in the point cloud map, optimizes to obtain the accurate initial positioning of the unmanned vehicle according to the matching degree of laser point cloud registration, and uses the fusion of IMU sensors and lidar to complete the real-time positioning of the unmanned vehicle; The method includes the following steps: Step 1: Based on GPS, use the GPS sensor data as the global positioning when the unmanned vehicle starts; Step 2: Based on the IMU sensor, obtain the yaw angle of the unmanned vehicle; Step 3: Calibrate the global positioning and the point cloud map to obtain the rotation matrix R and the translation vector t ; Step 4: Obtain the fine localization of the global positioning to the point cloud map; set the GPS translation error and the IMU yaw angle error, use the matching degree of NDT registration as the optimization object, and use the CRS algorithm to find the optimal pose of the point cloud registration and update R and t ; Step 5: Use IMU integrated positioning between lidar data frames as the initial value for the next frame of point cloud registration. After successful point cloud registration, update the attitude and speed of the unmanned vehicle for use in IMU integrated positioning; includes the following steps: Step 5.1: Integrate to obtain the pose; Step 5.2: Use the point cloud to construct a normal distribution of multi-dimensional variables, use a three-dimensional voxel network to divide the point cloud, and calculate the probability density; input the initial search pose, start the NDT matching search, judge the fitting degree of the two point clouds, and iteratively find the optimal solution of the three-dimensional transformation, which is the pose of the current lidar point cloud in the point cloud map; Step 5.3: Update the pose and speed of the driverless vehicle; Step 6: Dynamically load the point cloud map to achieve autonomous positioning of the unmanned vehicle.

2. The autonomous positioning method for an unmanned vehicle based on multi-sensor fusion according to claim 1, wherein: In the said Step 6, the file of the point cloud map is pre-divided into several point clouds according to the square range. At the same time, the segmented point clouds are downsampled and denoised, and all segmented point clouds are saved in multiple threads, and their lists and ranges are recorded in the file.

3. A method for autonomous positioning of an unmanned vehicle based on multi-sensor fusion according to claim 2, characterized in that: Read the file list of all point clouds, screen out the point clouds with a surrounding plane side length of 150m around the current unmanned vehicle according to the real-time pose, judge the point cloud files that need to be reloaded, remove the point clouds that do not need to be loaded, and then load the remaining point cloud files in multiple threads and splice them into a dynamically loaded point cloud map.

Images