Method and device for constructing high-precision laser point cloud map of ports

Abstract

The present application discloses a method and device for constructing a high-precision laser point cloud map of a port, which constructs an IMU pre-integration factor, a laser odometry factor, and a closed-loop factor. The global consistent posture of the robot is obtained by optimizing the factor graph while regularly correcting the laser odometry factor with the INS factor. The advantage of the present application is that initial value data is constructed between the IMU pre-integration and the INS value to form the laser odometry factor, which can effectively avoid the risk of system failure; combined with deep learning, dynamic objects are eliminated, which can effectively reduce the interference of dynamic objects and improve matching stability and accuracy; the elimination of unstable near-ground feature points can effectively improve the matching stability in a long corridor environment with few features, thereby constructing a high-precision laser point cloud map that can adapt to port characteristics and robustly deal with scenes such as feature loss, long corridor environment, GPS multipath effect, and environmental changes.

Description

Technical Field

[0001] The present application relates to the field of laser map construction technology, and in particular to a method and device for constructing a high-precision laser point cloud map of a port. Background Art

[0002] Currently, there are many open source solutions for building laser point cloud maps, such as the more representative cartographer, loam, lego-loam, lio-sam, etc. In large outdoor scenes such as ports, lio-sam has relatively better overall performance. Figure 1 It is the lio-sam flowchart, which adopts a tightly coupled laser-inertial odometry method to obtain the globally consistent posture of the robot by optimizing the lidar odometry factor, IMU pre-integration factor, GPS factor and loopback factor.

[0003] a) IMU pre-integration factor, obtained by integrating the IMU measurements between two adjacent keyframes.

[0004] b) Laser odometry factor, obtained by the frame-graph matching results between each keyframe and the previous n keyframes.

[0005] c) GPS factor, obtained from the GPS value at the key frame moment.

[0006] d) The loop closure factor is obtained by matching the frame graph between the key frame and the 2m+1 key frames that are temporally adjacent to the candidate loop closure key frame.

[0007] However, due to the unique characteristics of ports, such as the unfavorable environment of long corridors neatly stacked with cargo containers in some areas, the laser odometry factor can be erroneous. Metal objects such as yard cranes, quay cranes, and overly tall containers can obstruct GPS multipathing, leading to significant GPS factor errors. The large number of operating vehicles leads to a highly dynamic environment, which can also cause large errors in the laser odometry factor, or even errors. All of these factors make lio-sam less suitable for port environments, causing drift and jumps in these areas, and failing to meet accuracy requirements.

[0008] The main reasons why the accuracy cannot be achieved here are:

[0009] 1. The laser odometry factor and the IMU factor are interdependent. If the laser matching error is too large due to missing or unfavorable features in a certain area, the IMU pre-integration error will also increase. Since the IMU pre-integration is the initial value for laser matching, an excessively large initial error will lead to laser matching errors. An error in one will cause both to fail, leading to system drift and a lack of robustness.

[0010] 2. The judgment of GPS quality is not rigorous enough, and the GPS multi-pass effect cannot be determined, which will lead to the addition of GPS factors with larger errors, and never reduce the accuracy of the map.

[0011] 3. It is not very adaptable to changing environments. When there are many changing objects around, the laser odometry factor is likely to fail. Summary of the Invention

[0012] In order to solve the above technical problems, the embodiments of the present application provide a method and device for constructing a high-precision laser point cloud map of a port, so as to overcome the problem of low accuracy in the prior art.

[0013] A first aspect of an embodiment of the present application provides a method for constructing a high-precision laser point cloud map of a port, comprising the following steps:

[0014] S1: Real-time acquisition of the IMU attitude and INS value of the current frame to form initial value data, and the extraction of the required feature data after acquiring the lidar data of the current frame;

[0015] S2: Based on the initial value data and feature data of the current frame obtained in step S1), a key frame is selected and combined with the feature data of the previous n frames to obtain the pose of the current key frame as the laser odometry factor of the key frame;

[0016] S3: Obtain two adjacent key frame data and use pre-integration to construct the IMU pre-integration factor, and optimize the current IMU deviation in combination with the laser odometry factor;

[0017] S4: Constructing a closed-loop factor. Whenever the autonomous vehicle passes a previously passed location again, all keyframes in the history of that location are extracted as candidate loop keyframes. The closed-loop factor is formed by matching the frame graphs between the current keyframe and the 2m+1 keyframes that are temporally adjacent to the candidate loop keyframes.

[0018] S5: Continuously construct the IMU pre-integration factor and the closed-loop factor in step S4, and correct the pose of the current key frame in combination with the laser odometry factor, and use factor graph optimization to obtain globally consistent pose data.

[0019] Furthermore, after obtaining the INS value, the initial value data in S1) uses the IMU pre-integration as the initial value if the INS status meets the requirements; otherwise, uses the INS value as the initial value.

[0020] Furthermore, the feature data in S1) includes plane point features and corner point features.

[0021] Furthermore, the feature data is only obtained from the static object point cloud in the laser point cloud.

[0022] Furthermore, the static object point cloud is obtained by obtaining the laser point cloud and using the moving objects in the deep learning environment to derive their occupied spatial positions, eliminating these moving objects from the laser point cloud and removing the interference features of these moving objects.

[0023] Furthermore, after removing the interference features of the moving object from the static object point cloud, all corner point data within a preset range near the ground obtained by the fitting method are removed again.

[0024] Furthermore, step S5 also includes constructing an INS factor; the INS factor is constructed based on the extended Kalman filter fusing the sensor data of GPS, IMU, and vehicle odometer. Whenever the unmanned vehicle travels a preset distance, the INS factor is used instead of the laser odometer factor for correction.

[0025] A second aspect of an embodiment of the present application provides a device comprising components for executing any of the methods described above.

[0026] The map constructed by the method of this application can adapt to the characteristics of the port and robustly deal with laser mapping challenges such as feature loss, long corridor environment, GPS multipath effect, environmental changes, etc., to form a high-precision laser point cloud map. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without any creative work.

[0028] Figure 1 It is a flow chart of the lio-sam solution of the prior art;

[0029] Figure 2(a) is a flow chart of the method of the present application;

[0030] FIG2( b ) is a modular schematic diagram of the method flow of the present application;

[0031] Figure 3 This is a schematic diagram of the plane point and corner point features of the embodiment provided in this application;

[0032] Figure 4 Schematic diagram of the operation of the initial value decision module and the feature screening module of the embodiment provided in this application;

[0033] Figure 5 (a) is a rendering of the unstable corner points before removal in the embodiment provided by the present application;

[0034] Figure 5(b) is a diagram showing the effect of removing unstable corner points in the embodiment provided by the present application;

[0035] Figure 6 This is an effect diagram of the point cloud map of the embodiment provided in this application;

[0036] Figure 7 This is an effect diagram of the point cloud map of the existing lio-sam solution;

[0037] Figure 8 This is another rendering of the point cloud map of the prior art lio-sam solution;

[0038] Figure 9 This is the second rendering of the point cloud map of the embodiment provided in this application. DETAILED DESCRIPTION

[0039] In order to make the purpose, features, and advantages of this application more obvious and easy to understand, the technical solutions in the embodiments of this application will be clearly and completely described below in conjunction with the drawings in the embodiments of this application. Obviously, the embodiments described below are only part of the embodiments of this application, not all of the embodiments. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of this application.

[0040] The present invention will be further explained below with reference to the accompanying drawings and specific embodiments.

[0041] In the description of this application, it should be understood that the terms "upper", "lower", "top", "bottom", "inside", "outside", etc., indicating orientations or positional relationships, are based on the orientations or positional relationships shown in the accompanying drawings, and are only for the convenience of describing this application and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be understood as a limitation on this application.

[0042] The method for constructing high-precision laser point cloud maps of ports provided in this application is a new method for constructing high-precision laser point cloud maps of ports. Figure 1 Improvements such as Figure 2(a) and 2(b) As shown, the following steps are included:

[0043] S1: Obtain the IMU attitude and INS value of the current frame in real time to form the initial value data, obtain the lidar data of the current frame and extract the required feature data.

[0044] Because the IMU pre-integration error and radar matching error are interdependent, a large error in one will also amplify the error in the other, creating a vicious cycle and making the system unstable. For example, in areas with missing features or long corridors, radar matching is prone to failure, causing the IMU pre-integration that relies on the radar matching results to fail. Conversely, the radar matching that relies on IMU pre-integration as an initial value will further fail, further creating a vicious cycle. Considering the above issues, in areas where features are missing, radar matching is prone to failure. However, since the environment is relatively open and there is no obstruction around the GPS antenna in these areas, the INS (Inertial Navigation System) value will be better, and the INS value can be used as the initial value for laser matching. In areas with obstruction, the INS value will be relatively poor, but in these areas with abundant features, the laser matching value will be better, and thus the IMU pre-integration value will be very good, so the IMU pre-integration value can be used as the initial value for laser matching.

[0045] Therefore, in this step, after obtaining the INS value, if the INS status meets the requirements, the IMU pre-integration is used as the initial value; if the INS status does not meet the requirements, the INS value is used as the initial value.

[0046] As a specific embodiment, the feature data in this step includes planar point features and corner point features, and these feature data are obtained only from the static object point cloud in the laser point cloud. The static object point cloud here is obtained by obtaining the laser point cloud, using the deep learning environment to determine the spatial position of moving objects, removing these moving objects from the laser point cloud, and removing the interference features of these moving objects. After removing the interference features of the moving objects, the static object point cloud is further removed from all corner point data within a preset range near the ground obtained by the fitting method.

[0047] As a specific example, when new radar data arrives, we first perform feature extraction on it. We extract corner points and plane points by the degree of concavity and convexity of points in the local area. For example, Figure 3 , the edges of the black container are corner points, and the gray ground and container surface are plane points.

[0048] Since there are many trucks in the working environment, these dynamic objects will have an adverse effect on laser matching. When extracting feature data in this step, the 3D target detection neural network pointpilars is first used to identify the moving objects in the environment and obtain the spatial position they occupy. Then these moving objects are removed from the laser data, thereby removing the interference features of the dynamic objects. Since the radar's incident angle on the ground is small, it is easy to extract less stable corner features. These unstable features are very unfavorable for laser matching, especially in a long corridor environment with few effective features. The impact is even worse. Therefore, the method used here is to cut the perigee cloud and then use the ransac method to fit the ground to eliminate feature points within 0.2m from the ground to reduce the impact of unstable features. Figure 5 (a) shows the situation where there are unstable black corner features on the ground, such as Figure 5 (b) is the situation after elimination.

[0049] S2: Based on the initial value data and feature data of the current frame obtained in step S1), a key frame is selected and combined with the feature data of the previous n frames to obtain the pose of the current key frame as the laser odometry factor of the key frame.

[0050] As a specific embodiment, after obtaining the feature data of the current frame, the feature map is matched with the features of the previous n frames so that the corner points are close together and the plane points are close together, thereby obtaining the vehicle posture in the current frame.

[0051] As a specific embodiment, the key frames here may be set at intervals of 0.5 to 1.0 m.

[0052] S3: Obtain two adjacent key frame data and use the pre-integration IMU measurement value between the two key frames to construct the IMU pre-integration factor. The posture formed by the current frame posture acquisition module is used as the laser odometry factor. The IMU pre-integration factor and the laser odometry factor use the factor graph to optimize the deviation of the current IMU.

[0053] S4: Construct a closed-loop factor. Whenever the autonomous vehicle passes a previously passed location again, all keyframes in the history of that location are extracted as candidate loop keyframes. The closed-loop factor is formed by matching the frame graphs between the current keyframe and the 2m+1 keyframes that are temporally adjacent to the candidate loop keyframe. Here, m is preset based on an empirical value.

[0054] S5: Continuously construct the IMU pre-integration factor and the closed-loop factor in step S4, and correct the pose of the current key frame in combination with the laser odometry factor, and use factor graph optimization to obtain globally consistent pose data.

[0055] As a specific embodiment, an INS factor is additionally added in step S5. The INS factor is constructed based on the extended Kalman filter, which fuses sensor data from GPS, IMU, and vehicle odometer. Since the laser odometer gradually accumulates drift as the distance increases, an absolute observation value is required to correct the error. The reliability of the INS value is checked at intervals. If it is reliable, the INS factor is added to reduce the cumulative error. The INS factor is used here to replace the original GPS factor. Due to the multipath effect of GPS, judging GPS reliability purely from the variance is not very reliable. Since the INS value incorporates information from more sensors such as the IMU and vehicle odometer, a more accurate value can be obtained. The accuracy can also be better judged by comprehensively considering conditions such as whether the variance of the INS value exceeds a predetermined value and whether the trajectory is smooth. Therefore, in this step, every time the unmanned vehicle travels a preset distance, the INS factor is used to replace the laser odometer factor for correction.

[0056] According to the above map construction method, the present invention also provides a device for executing the above method.

[0057] Compared to the drawbacks of existing technologies, the advantage of this application is that the initial value decision module is constructed between the IMU pre-integration and the INS value, which can effectively avoid the risk of system failure. Combined with deep learning, the elimination of dynamic objects can effectively reduce the interference of dynamic objects and improve matching stability and accuracy. The elimination of unstable near-ground feature points can greatly improve the stability of matching in long corridor environments with limited special features.

[0058] The preferred embodiments of the present invention are described in detail above, but the present invention is not limited to the specific details in the above embodiments. Within the technical concept of the present invention, various equivalent transformations (such as quantity, shape, position, etc.) can be made to the technical solution of the present invention, and these equivalent transformations are all protected by the present invention.

Claims

1. A method for constructing a high-precision laser point cloud map of a port, characterized in that: The steps include: S1: Real-time acquisition of the IMU attitude and INS value of the current frame to form initial value data, and the extraction of the required feature data after acquiring the lidar data of the current frame; Among them, in places where features are missing, INS values ​​are used as the initial values ​​for laser matching; in places where there are occlusions, IMU pre-integration is used as the initial values ​​for laser matching; S2: Based on the initial value data and feature data of the current frame obtained in step S1), a key frame is selected and combined with the feature data of the previous n frames to obtain the pose of the current key frame as the laser odometry factor of the key frame; S3: Obtain two adjacent key frame data and use pre-integration to construct the IMU pre-integration factor, and optimize the current IMU deviation in combination with the laser odometry factor; S4: Constructing a closed-loop factor. Whenever the autonomous vehicle passes a previously passed location again, all keyframes in the history of that location are extracted as candidate loop keyframes. The closed-loop factor is formed by matching the frame graphs between the current keyframe and the 2m+1 keyframes that are temporally adjacent to the candidate loop keyframes. S5: Continuously construct the IMU pre-integration factor and the closed-loop factor in step S4, and correct the pose of the current key frame in combination with the laser odometry factor, and use factor graph optimization to obtain globally consistent pose data.

2. The method for constructing a high-precision laser point cloud map of a port according to claim 1, characterized in that: After obtaining the INS value, the initial value data in S1) uses the IMU pre-integration as the initial value if the INS status meets the requirements; otherwise, uses the INS value as the initial value.

3. The method for constructing a high-precision laser point cloud map of a port according to claim 2, characterized in that: The feature data in S1) includes plane point features and corner point features.

4. The method for constructing a high-precision laser point cloud map of a port according to claim 3, characterized in that: The feature data is only obtained from the static object point cloud in the laser point cloud.

5. The method for constructing a high-precision laser point cloud map of a port according to claim 4, characterized in that: The static object point cloud is obtained by obtaining the laser point cloud and using the deep learning environment to obtain the moving objects and derive the spatial positions occupied by them, eliminating these moving objects from the laser point cloud and removing the interference features of these moving objects.

6. The method for constructing a high-precision laser point cloud map of a port according to claim 5, characterized in that: After removing the interference features of the moving object from the static object point cloud, all corner point data within a preset range near the ground obtained by the fitting method are removed again.

7. The method for constructing a high-precision laser point cloud map of a port according to claim 1, characterized in that: The step S5 also includes constructing an INS factor; the INS factor is constructed based on extended Kalman filtering and fusion of GPS, IMU, and vehicle odometer sensor data. Whenever the unmanned vehicle travels a preset distance, the INS factor is used to replace the laser odometer factor for correction.

8. A device for constructing high-precision laser point cloud maps of ports, characterized in that: Comprising means for performing the method according to any one of claims 1 to 7.

Images