Multi-line laser radar positioning method and system based on dynamic obstacle removal

Abstract

The present invention discloses a multi-line laser radar positioning method based on dynamic obstacle removal, comprising: collecting a global laser point cloud; detecting and filtering out dynamic point clouds; extracting feature points and obtaining odometer pose; correcting pose based on loop detection; establishing a global high-precision point cloud map that filters out dynamic point clouds; re-driving to collect point clouds; extracting feature points of the current frame and establishing a sliding window local map; solving the initial pose of the current frame relative to the local map; projecting the feature points of the current frame onto the local map; eliminating the dynamic point cloud of the local map; and matching the global map to correct the initial pose to obtain precise positioning. The present invention has the following beneficial effects: through a dual matching strategy of the feature points of the current frame with the local map that has removed the dynamic point cloud and the high-precision global map, the laser positioning accuracy and robustness can be improved, and multi-line laser radar positioning in a dynamic environment can be achieved.

Description

Technical Field

[0001] The present invention relates to the technical field of synchronous positioning and map construction, and in particular to a multi-line laser radar positioning method and system based on dynamic obstacle removal. Background Art

[0002] Autonomous driving has become a hot research topic. Simultaneous localization and mapping (SLAM), a key foundational technology for autonomous driving, has made significant progress in the past decade. However, most mainstream algorithms currently operate stably only in static environments. However, the real world is dynamic, with moving cars and pedestrians, for example. This dynamic environment poses new challenges to SLAM modules such as pose estimation, feature extraction and tracking, and map construction. Summary of the Invention

[0003] The present invention discloses a multi-line laser radar positioning method and system based on dynamic obstacle removal, which can effectively solve the technical problems involved in the background technology.

[0004] To achieve the above object, the technical solution of the present invention is:

[0005] A multi-line laser radar positioning method based on dynamic obstacle removal includes the following steps:

[0006] Step S1: collecting global point cloud data of the running scene at low speed;

[0007] Step S2: Detect the global point cloud data through the 3D object detection network and filter out dynamic point clouds;

[0008] Step S3: Obtain the feature points of each point cloud frame after filtering out the dynamic point cloud based on curvature calculation, and solve the position and posture of the lidar through point cloud association matching;

[0009] Step S4: Correct the posture based on loop closure detection;

[0010] Step S5: establishing a global high-precision point cloud map composed of feature points based on the corrected posture;

[0011] Step S6: Re-driving the vehicle to collect laser point cloud data for real-time positioning;

[0012] Step S7: Obtain feature points of the current frame and establish a sliding window local map;

[0013] Step S8: solving the relative pose of the current frame and the local map of the sliding window;

[0014] Step S9: Project the feature points of the current frame onto the local map of the sliding window according to their relative poses, and calculate the distance between each feature point and the corresponding nearest point;

[0015] Step S10: judging and removing the dynamic point cloud in the local map of the sliding window according to the distance value;

[0016] Step S11: Match the feature points of the current frame with the global high-precision point cloud map through relative posture to obtain accurate positioning.

[0017] As a preferred improvement of the present invention, the 3D object detection network is PointPillars.

[0018] As a preferred improvement of the present invention, the feature points include line feature points and surface feature points. Feature points with a curvature greater than a set threshold are line feature points, and feature points with a curvature less than the set threshold are surface feature points.

[0019] As a preferred improvement of the present invention, in step S4, the distance between the posture of each point cloud frame and the posture of all previous point cloud frames is detected. If the distance is less than 3m and the interval time is greater than 15s, it is a loop frame.

[0020] As a preferred improvement of the present invention, the sliding window local map is composed of feature points of key frames.

[0021] As a preferred improvement of the present invention, the key frame is determined by the current frame. If the displacement distance between the current frame and the previous point cloud key frame is greater than a set threshold or the angle distance is greater than a set threshold, the current frame is determined to be a key frame.

[0022] As a preferred improvement of the present invention, the distance value described in step S9 includes: the distance from a line feature point to a line segment formed by its two nearest feature points, and the distance from a surface feature point to a plane formed by its three nearest non-collinear feature points.

[0023] As a preferred improvement of the present invention, in step S10, if the distance from a line feature point to a line segment or the distance from a surface feature point to a plane is greater than a set threshold, it is determined to be a dynamic point cloud.

[0024] A multi-line laser radar positioning system based on dynamic obstacle removal for executing the method includes a global high-precision point cloud map building module and a laser positioning module; the global high-precision point cloud map building module includes:

[0025] Point cloud acquisition module, used to collect global point cloud data of running scenes at low speed;

[0026] Dynamic object filtering module, used to detect global point cloud data and filter out dynamic point clouds;

[0027] Feature point extraction and pose solution module, used to obtain the feature points of each point cloud frame and obtain the pose of the lidar;

[0028] A posture correction module, which corrects the posture based on loop closure detection; and

[0029] Global map building module, used to build a global high-precision point cloud map composed of feature points;

[0030] The laser positioning module includes:

[0031] Point cloud acquisition module, used to collect laser point cloud data from re-driving vehicles for real-time positioning;

[0032] A local map building module is used to obtain feature points of the current frame to build a sliding window local map;

[0033] The pose solving module is used to solve the relative pose of the current frame and the local map of the sliding window;

[0034] The local map matching module is used to project the feature points of the current frame onto the local map of the sliding window according to the relative pose and calculate the distance between each feature point and the corresponding nearest point;

[0035] A dynamic point cloud removal module is used to determine and remove dynamic point clouds in the local map of the sliding window based on the distance value; and

[0036] The precise positioning module is used to project the feature points of the current frame onto the global high-precision point cloud map for correlation matching based on the initial relative pose value to solve the pose and obtain precise positioning.

[0037] The beneficial effects of the present invention are as follows:

[0038] 1. Dynamic point clouds are removed through a 3D object detection network to construct a global high-precision point cloud map composed of feature points without dynamic point clouds, effectively improving the accuracy of point cloud matching and the robustness of laser positioning in dynamic environments;

[0039] 2. The relative pose is solved by matching the current frame with the local map of the sliding window, and the dynamic point cloud in the local map is eliminated based on the relative pose relationship, effectively eliminating the influence of the dynamic point cloud on the initial pose solution;

[0040] 3. Through the dual matching strategy of the current frame feature points, the local map without dynamic point cloud, and the high-precision global map, the cumulative error can be effectively eliminated, the accuracy and robustness of laser positioning can be improved, and multi-line laser radar positioning in dynamic environments can be realized. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 This is a flow chart of the PointPillars network architecture in the present invention;

[0042] Figure 2 This is the Pillar feature extraction network flow chart in the present invention;

[0043] Figure 3 Schematic diagram of the 2D convolutional neural network in the present invention;

[0044] Figure 4 Schematic diagram of loop detection in the present invention;

[0045] Figure 5 This is a system framework diagram of the present invention. DETAILED DESCRIPTION

[0046] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the embodiments of the present invention. Obviously, the embodiments described are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0047] It should be noted that all directional indications (such as up, down, left, right, front, back, etc.) in the embodiments of the present invention are only used to explain the relative position relationship, movement status, etc. between the various components under a certain specific posture (as shown in the accompanying drawings). If the specific posture changes, the directional indication will also change accordingly.

[0048] In addition, the terms "first," "second," and so on, used in this disclosure are for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of the technical features being referenced. Thus, a feature specified as "first" or "second" may explicitly or implicitly include at least one such feature. In the description of this disclosure, "plurality" means at least two, such as two or three, unless otherwise specifically defined.

[0049] In the present invention, unless otherwise specified or limited, the terms "connection" and "fixation" should be understood in a broad sense. For example, "fixation" can mean fixed connection, detachable connection, or integration; mechanical connection or electrical connection; direct connection or indirect connection through an intermediate medium; internal communication between two elements or interaction between two elements, unless otherwise specified. Those skilled in the art will be able to understand the specific meanings of the above terms in the present invention based on specific circumstances.

[0050] In addition, the technical solutions between the various embodiments of the present invention can be combined with each other, but it must be based on the fact that ordinary technicians in this field can implement it. When the combination of technical solutions is mutually contradictory or cannot be implemented, it should be deemed that such a combination of technical solutions does not exist and is not within the scope of protection required by the present invention.

[0051] The present invention proposes a multi-line laser radar positioning method based on dynamic obstacle removal, which specifically includes the following steps:

[0052] Step S1: The vehicle travels at a low speed in the operating scene and collects global point cloud data through the lidar.

[0053] Step S2: Detect the global point cloud data through the 3D object detection network PointPillars and filter out dynamic point clouds.

[0054] See Figure 1 As shown in the figure, PointPillars is mainly divided into three parts: Pillar feature extraction network, backbone network and SSD detection network.

[0055] The role of the Pillar feature extraction network is to convert the point cloud into a pseudo image. Figure 2 As shown, first, the input point cloud is divided into multiple Pillar (cylindrical) units. Each Pillar divides the point cloud into a small 3D unit with a certain step size on the XY plane (Cartesian coordinate system). Then each point cloud in each Pillar is encoded into a 9-dimensional vector D: , where x, y, z, and r represent the three real coordinates and reflection intensity of the point cloud in three-dimensional space respectively. Represents the geometric center of all points in the Pillar, They are xx c ,yy c , indicating the relative position of the point to the geometric center of the Pillar.

[0056] Due to the sparsity of point cloud data, many Pillars contain no point clouds or only a small number of point clouds. Considering the computational complexity, the number of Pillars is limited to a maximum of P non-empty Pillars. Each Pillar contains a maximum of N point cloud feature vectors. If the number of point clouds is greater than N, random sampling is used to select N from them. Conversely, if the number of point clouds is less than N, zero padding is used to fill the number to N. Through the above method, a frame of point cloud data is encoded into a dense tensor of dimension (D, P, N).

[0057] After achieving tensor quantization, the PointNet network first processes each point containing D-dimensional features through a linear layer, BatchNorm, and the ReLU activation function, generating a tensor of dimension (C, P, N). Max pooling is then performed on each Pillar unit, resulting in a tensor of dimension (C, P). Finally, the (C, P) tensor generated in the previous step is converted back to its original Pillar coordinates using the Pillar index value of each point to create a pseudo image of size (C, H, W).

[0058] The backbone network consists of a 2D convolutional neural network, such as Figure 3 As shown in the figure, its function is to extract high-dimensional features from the output pseudo image. The backbone network is divided into two sub-networks: a top-down sub-network is used to extract features from feature maps of increasingly smaller spatial resolutions, and the other sub-network is responsible for upsampling the features extracted from feature maps of different resolutions to the same dimensional size through deconvolution operations and then concatenating them.

[0059] PointPillars uses the Single Shot Detection (SSD) algorithm's detection head to perform 3D object detection. SSD acts as a two-dimensional detector, matching a pre-set bounding box with the ground truth bounding box using the 2D Intersection over Union (IoU) algorithm. The box height and the center point height serve as additional regression targets. Similar to SSD, PointPillars performs object detection within a 2D network, while the z-axis height of the final output 3D bounding box is calculated through a separate regression.

[0060] Step S3: Based on curvature calculation, the feature points of each point cloud frame after filtering out the dynamic point cloud are obtained, as shown in formula (1). The feature points include line feature points and surface feature points, and then the position and posture of the lidar are solved through point cloud association matching.

[0061] (1)

[0062] Among them, S j Represents the curvature of the point cloud in the local range, Indicates the measurement value of the current point cloud position, Indicates the measurement coordinates of other point clouds within the local range except the current point cloud position. Represents a collection of points within the range of a laser point. When the curvature is greater than a certain threshold, the point cloud frame is selected as a line feature point; when the curvature is less than a certain threshold, the point cloud is selected as a surface feature point.

[0063] Step S4: Correct the posture based on loop closure detection.

[0064] like Figure 4 As shown in the figure, the loop detection process is as follows: for each laser frame's pose, the distance between it and the poses of all previous frames is measured. If the distance is less than 3m and the interval is greater than 15s, the frame is considered a loop frame. ICP registration is then performed on the two point clouds. The reliability of the loop constraint is determined by the degree of ICP registration. If the loop is reliable, it is added to the loop candidate pair. The best-matched pose pair is selected based on the degree of ICP registration to establish the loop constraint.

[0065] Step S5: Based on the corrected posture, a global high-precision point cloud map consisting of the feature points in step S3 is established.

[0066] The global high-precision point cloud map establishment process is as follows: for each frame of the laser point cloud, the line feature points and surface feature points are transformed into the map coordinate system through the pose conversion after loop closure constraint in each frame, and a global high-precision point cloud map composed of line feature points and surface feature points is constructed.

[0067] Step S6: After constructing the global high-precision point cloud map, the vehicle drives again to collect laser point cloud data for real-time positioning.

[0068] Step S7: Obtain feature points of the current frame and establish a sliding window local map.

[0069] The sliding window is composed of feature point clouds of fixed keyframes. When a new keyframe is added, the oldest historical frame is removed to maintain the window size. The keyframe is determined by the current frame. If the displacement distance or angle distance between the current frame and the previous point cloud keyframe is greater than a set threshold, the current frame is determined to be a keyframe.

[0070] Step S8: Calculate the relative pose between the current frame and the local map of the sliding window.

[0071] Step S9: Project the feature points of the current frame onto the local map of the sliding window according to the relative pose, and calculate the distance between each feature point and the corresponding nearest point.

[0072] 1. Current laser point cloud The i-th line feature point X (k,i) Projected to the local map, point cloud on the local map Find the two nearest feature points n and m, whose coordinates are 、 , feature points n and m form a line segment, then the distance from the feature point to the line segment is as shown in formula (2):

[0073] (2)

[0074] 2. Current laser point cloud The jth face feature point in Projected to the local map, point cloud on the local map Find the three nearest non-collinear feature points a, b, c, whose coordinates are 、 、 , three non-collinear feature points form a plane, then the distance from the feature point to the plane is as shown in formula (3):

[0075] (3)

[0076] Step S10: Determine and remove the dynamic point cloud in the local map of the sliding window according to the distance value.

[0077] If the distance value from the line feature point to the line segment Or the distance value from the surface feature point to the plane If the value is greater than the set threshold, the corresponding point cloud in the local map is determined to be a dynamic point cloud and is removed.

[0078] Step S11: Project the feature points of the current frame onto the global high-precision point cloud map through the initial pose value for correlation matching to solve the precise pose and obtain precise positioning.

[0079] Construct the error function of line feature points, as shown in formula (4):

[0080] (4)

[0081] in, is the distance from the point to the line, R is the rotation matrix of the current frame relative to the global map, t is the translation vector of the current frame relative to the global map, is a line point in the global map, is the line point of the current frame, is the unit vector corresponding to the straight line formed by the target's neighboring line points.

[0082] The error function of the constructed surface feature points is shown in formula (5):

[0083] (5)

[0084] in, is the distance from the point to the plane, is a plane point in the global map, is the plane point of the current frame, It is the unit normal vector corresponding to the plane composed of the target's neighboring plane points.

[0085] Using the Gauss-Newton method, and The total residual is solved iteratively to eliminate the accumulated error and obtain accurate positioning.

[0086] The present invention also provides a multi-line laser radar positioning system based on dynamic obstacle removal for executing the method. Figure 5 As shown, the system includes a global high-precision point cloud map establishment module 1 and a laser positioning module 2. The global high-precision point cloud map establishment module 1 is used to obtain a global high-precision point cloud map, and the laser positioning module 2 is used to solve the precise posture to obtain precise positioning.

[0087] Specifically, the global high-precision point cloud map establishment module 1 includes a point cloud acquisition module 11 , a dynamic object filtering module 12 , a feature point extraction and posture solution module 13 , a posture correction module 14 and a global map establishment module 15 .

[0088] Among them, the point cloud acquisition module 11 is used to collect global point cloud data of the running scene at low speed, the dynamic object filtering module 12 is used to detect the global point cloud data and filter out dynamic point clouds, the feature point extraction and posture solution module 13 is used to obtain the feature points of each point cloud frame and obtain the posture of the lidar, the posture correction module 14 corrects the posture based on loop detection, and the global map establishment module 15 is used to establish a global high-precision point cloud map composed of feature points.

[0089] Specifically, the laser positioning module 2 includes a point cloud acquisition module 21 , a local map building module 22 , a pose solving module 23 , a local map matching module 24 , a dynamic point cloud elimination module 25 and a precise positioning module 26 .

[0090] Among them, the point cloud acquisition module 21 is used to re-drive the vehicle to collect laser point cloud data for real-time positioning, the local map establishment module 22 is used to obtain the feature points of the current frame to establish a sliding window local map, the posture solving module 23 is used to solve the relative posture of the current frame and the sliding window local map, the local map matching module 24 is used to project the feature points of the current frame to the sliding window local map according to the relative posture and calculate the distance value between each feature point and the corresponding nearest point, the dynamic point cloud elimination module 25 is used to judge and remove the dynamic point cloud in the sliding window local map according to the distance value, and the precise positioning module 26 is used to project the feature points of the current frame to the global high-precision point cloud map according to the initial value of the relative posture for association matching to solve the precise posture and obtain precise positioning.

[0091] The beneficial effects of the present invention are as follows:

[0092] 1. Dynamic point clouds are removed through a 3D object detection network to construct a global high-precision point cloud map composed of feature points without dynamic point clouds, effectively improving the accuracy of point cloud matching and the robustness of laser positioning in dynamic environments;

[0093] 2. The relative pose is solved by matching the current frame with the local map of the sliding window, and the dynamic point cloud in the local map is eliminated based on the relative pose relationship, effectively eliminating the influence of the dynamic point cloud on the initial pose solution;

[0094] 3. Through the dual matching strategy of the current frame feature points, the local map without dynamic point cloud, and the high-precision global map, the cumulative error can be effectively eliminated, the accuracy and robustness of laser positioning can be improved, and multi-line laser radar positioning in dynamic environments can be realized.

[0095] The embodiments of the present application are described above in conjunction with the accompanying drawings, but the present application is not limited to the above-mentioned specific implementation methods. The above-mentioned specific implementation methods are merely illustrative and not restrictive. Under the guidance of this application, ordinary technicians in this field can also make many forms without departing from the purpose of this application and the scope of protection of the claims, all of which are within the protection of this application.

Claims

1. A multi-line laser radar positioning method based on dynamic obstacle removal, characterized in that: The following steps are involved: Step S1: collecting global point cloud data of the running scene at low speed; Step S2: Detect the global point cloud data through the 3D object detection network and filter out dynamic point clouds; Step S3: Obtain the feature points of each point cloud frame after filtering out the dynamic point cloud based on curvature calculation, and solve the position and posture of the lidar through point cloud association matching; Step S4: Correct the posture based on loop closure detection; Step S5: establishing a global high-precision point cloud map composed of feature points based on the corrected posture; Step S6: Re-driving the vehicle to collect laser point cloud data for real-time positioning; Step S7: Obtain feature points of the current frame and establish a sliding window local map; Step S8: solving the relative pose of the current frame and the local map of the sliding window; Step S9: Project the feature points of the current frame onto the local map of the sliding window according to their relative poses, and calculate the distance between each feature point and the corresponding nearest point; Step S10: judging and removing the dynamic point cloud in the local map of the sliding window according to the distance value; Step S11: Match the feature points of the current frame with the global high-precision point cloud map through relative posture to obtain accurate positioning.

2. The method according to claim 1, wherein: The 3D object detection network is PointPillars.

3. The method according to claim 1, wherein: The feature points include line feature points and surface feature points. Feature points with a curvature greater than a set threshold are line feature points, and feature points with a curvature less than a set threshold are surface feature points.

4. The method according to claim 1, wherein: In step S4, for each point cloud frame, the distance between it and the poses of all previous point cloud frames is detected. If the distance is less than 3m and the interval time is greater than 15s, it is a loop frame.

5. The method according to claim 1, wherein: The sliding window local map is composed of feature points of key frames.

6. The method according to claim 5, characterized in that: The key frame is determined by the current frame. If the displacement distance between the current frame and the previous point cloud key frame is greater than a set threshold or the angle distance is greater than a set threshold, the current frame is determined to be a key frame.

7. The method according to claim 1, wherein: The distance values ​​in step S9 include: the distance from a line feature point to a line segment formed by its two nearest feature points, and the distance from a surface feature point to a plane formed by its three nearest non-collinear feature points.

8. The method according to claim 7, wherein: In step S10, if the distance from the line feature point to the line segment or the distance from the surface feature point to the plane is greater than a set threshold, it is determined to be a dynamic point cloud.

9. A multi-line laser radar positioning system based on dynamic obstacle removal that executes the method according to any one of claims 1 to 8, characterized in that: Including global high-precision point cloud map building module and laser positioning module; The global high-precision point cloud map building module includes: Point cloud acquisition module, used to collect global point cloud data of running scenes at low speed; Dynamic object filtering module, used to detect global point cloud data and filter out dynamic point clouds; Feature point extraction and pose solution module, used to obtain the feature points of each point cloud frame and obtain the pose of the lidar; A posture correction module, which corrects the posture based on loop closure detection; and Global map building module, used to build a global high-precision point cloud map composed of feature points; The laser positioning module includes: Point cloud acquisition module, used to collect laser point cloud data from re-driving vehicles for real-time positioning; A local map building module is used to obtain feature points of the current frame to build a sliding window local map; The pose solving module is used to solve the relative pose of the current frame and the local map of the sliding window; The local map matching module is used to project the feature points of the current frame onto the local map of the sliding window according to the relative pose and calculate the distance between each feature point and the corresponding nearest point; A dynamic point cloud removal module is used to determine and remove dynamic point clouds in the local map of the sliding window based on the distance value; and The precise positioning module is used to project the feature points of the current frame onto the global high-precision point cloud map for correlation matching based on the initial relative pose value to solve the pose and obtain precise positioning.

Images