Unmanned vehicle high-precision map construction method for large outdoor scene

Abstract

The invention discloses an unmanned vehicle high-precision map construction method for a large outdoor scene, an unmanned vehicle at least carries a laser radar, a wheel type odometer and an inertial measurement unit, and the map construction method comprises the following steps: acquiring 3D point cloud information of the unmanned vehicle at a current position by using the laser radar; 3D point cloud preprocessing is carried out through direct filtering and a gradient negative feedback voxel downsampling method; the current pose of the unmanned vehicle is calculated based on wheel type odometer information, inertial measurement unit information and the optimized point cloud registration algorithm, and the layered space screening method for eliminating isolated laser points is an optimization method of the point cloud registration algorithm; selecting key frames according to displacement information and turning information of the unmanned vehicle, and splicing the key frames into a global point cloud map; and the unmanned vehicle performs loopback detection in the area where loopback may exist, and if loopback is detected, loopback constraint is added and graph optimization is executed. According to the invention, the performance expenditure can be reduced, the unmanned system cost is reduced, and the stability of high-precision map construction is improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous driving and high-precision map construction, and specifically relates to a method for constructing high-precision maps for unmanned vehicles in large outdoor scenes. Background Art

[0002] In recent years, LiDAR-based mapping and positioning technologies have been widely used in the fields of autonomous mobile robots and self-driving cars. As an important application scenario in the field of autonomous mobile robots, unmanned vehicles need to build high-precision maps of the working environment before they are put into use to achieve path planning and positioning.

[0003] When building high-precision maps, in order to generate a more complete environmental map and reduce scanning blind spots, laser radars with a field of view of 360° are generally used; however, in actual engineering applications, due to the fixed installation position of the laser radar of unmanned vehicles, the field of view is often limited, and all-round unobstructed scanning cannot be achieved, which increases the difficulty of building high-precision maps.

[0004] At present, there are some high-precision map construction schemes, such as the LOAM algorithm (Zhang J, Singh S. LOAM: Lidarodometry and mapping in real-time [C] / / Robotics: Science and systems. 2014, 2 (9): 1-9.), the LIO-SAM algorithm (Shan T, Englot B, Meyers D, et al. Lio-sam: Tightly-coupled lidar inertial odometry via smoothing and mapping [C] / / 2020 IEEE / RSJ international conference on intelligent robots and systems (IROS). IEEE, 2020: 5135-5142.), etc. These methods have good performance in indoor scenes and small outdoor scenes under the condition of sufficient computing resources, but there are still some limitations and problems in the construction of maps in large outdoor scenes:

[0005] 1) Both use a laser radar with a field of view of 360°, and do not consider the mapping scenario under the condition of limited field of view of the laser radar (such as the requirements of patent CN 111578932 B);

[0006] 2) The performance cost is large, and the onboard computer of the unmanned vehicle is difficult to achieve the performance required by the algorithm, and it is impossible to achieve a balance between performance cost and mapping quality (such as the requirements of patent CN 112116656 B);

[0007] 3) The performance is unstable in large outdoor scenes. Map drift accumulates with distance and running time, making it difficult to obtain high-quality HD maps.

[0008] 4) The high-precision map construction solution requires the support of a variety of expensive sensors, which is contrary to the economic requirements of unmanned vehicles (such as the requirements of patent CN 103901774 B).

[0009] Therefore, it is necessary to design a high-precision map construction method for unmanned vehicles in large outdoor scenes to solve the above technical problems. Summary of the invention

[0010] In order to solve the shortcomings of the prior art, reduce the dependence of the high-precision map construction algorithm on the laser radar field of view, improve the stability of high-precision map construction, and reduce the performance cost of high-precision map construction and the cost of unmanned systems, the present invention adopts the following technical solutions:

[0011] A method for constructing a high-precision map for an unmanned vehicle in a large outdoor scene comprises the following steps:

[0012] Step S1: Obtain the point cloud information at the current position of the unmanned vehicle;

[0013] Step S2: The initial positioning result P of the unmanned vehicle t As the initial estimate of point cloud registration, before point cloud registration, the hierarchical spatial screening method is used to complete the preprocessing of the 3D point cloud frame F at the current position. t2 Remove the isolated laser points in the 3D point cloud frame F after removing the isolated laser points t3 As the source point cloud, the global point cloud map Map is used as the target point cloud. The source point cloud F is aligned with the point cloud through the point cloud registration algorithm. t3 It is aligned with the target point cloud Map to obtain the real-time pose P of the unmanned vehicle. t1 and the coordinate transformation matrix T true ;

[0014] Step S3: In order to reduce the performance cost of high-precision map construction, not all point cloud frames are spliced ​​into the global point cloud map Map. Instead, key frames F are selected based on the displacement information and turning information of the unmanned vehicle. key , through the coordinate transformation matrix T true Transform to the real-time pose P of the unmanned vehicle t1 and set the key frame F key Stitched into the global point cloud map Map;

[0015] Step S4: Based on the current and historical key frames, determine the area where loops may exist in space and time for loop detection. If a loop is detected, add loop constraints and perform map optimization.

[0016] Furthermore, in step S1, in order to reduce performance costs as much as possible without affecting the quality of high-precision map construction, the point cloud information is preprocessed by direct filtering, and the 3D point cloud frame F at the current position is removed based on the distance information and height information. t The laser points with medium height higher than the height threshold and the laser points with distance from the unmanned vehicle greater than the distance threshold are used to obtain the 3D point cloud frame F after direct filtering. t1 , further improving the priority of laser points on the ground and its vicinity, and also improving the priority of laser points that are close to car-free sanitation vehicles, reducing the difficulty of road mapping.

[0017] Furthermore, the straight-through filtering will convert the point cloud frame F at the current position collected by the laser radar t The laser points that satisfy the following formula are retained:

[0018]

[0019] Among them, x i ,y i , z i Respectively represent the 3D point cloud frame F at the current position t The real-time position P of the i-th laser point relative to the unmanned vehicle t1 The x, y, z axis coordinates, h max Indicates the set height threshold, R max Indicates the set distance threshold.

[0020] Furthermore, in step S1, the 3D point cloud frame F after the straight-through filtering t1 The point cloud range is reduced, but the point cloud is still too dense. In order to further reduce the performance cost, it is necessary to filter the 3D point cloud frame F after the direct filtering. t1 Perform gradient negative feedback voxel downsampling to obtain a 3D point cloud frame F t2 , including the following steps:

[0021] Step S1.1: According to the performance of the unmanned vehicle system, set the total number of laser points S that the computing device can process, set the number of laser point errors allowed ∈, set the initial value of the downsampling parameter V, and set the initial value of the upward gradient of the downsampling parameter V ΔV up and its replica ΔV up1 , set the initial value ΔV of the downward gradient of the downsampling parameter V down and its replica is ΔV down1 ;

[0022] Step S1.2: Obtain the 3D point cloud frame F after direct filtering t1 and the number of laser points Adjust the initial value V of the downsampling parameter according to the following formula:

[0023]

[0024] Step S1.3: Downsample according to the adjusted downsampling parameter V to obtain the preprocessed 3D point cloud frame F t2 .

[0025] Furthermore, in step S2, the ground plane speed of the unmanned vehicle is obtained through the wheel odometer information, the three-axis acceleration and three-axis angular velocity of the unmanned vehicle are obtained through the inertial measurement unit information, and based on the posture P of the unmanned vehicle at the previous moment t-1 The time difference Δt and the position change ΔP from the current moment are used to calculate the initial positioning result P of the unmanned vehicle. t =P t-1 +ΔP, ΔP is calculated as follows:

[0026]

[0027] Among them, Δx, Δy, and Δz represent the position changes in the x-axis, y-axis, and z-axis directions, respectively; Δφ, Δθ, and Δψ represent the angle changes around the x-axis, y-axis, and z-axis, respectively. They represent the speed of the unmanned vehicle in the x-axis and y-axis directions at the previous moment, They represent the acceleration of the unmanned vehicle in the x-axis, y-axis, and z-axis directions at the previous moment, respectively. They respectively represent the angular velocity of the unmanned vehicle around the x-axis, y-axis, and z-axis at the previous moment.

[0028] Furthermore, in step S2, in order to reduce the performance cost of the NDT point cloud registration algorithm and speed up the point cloud registration, based on the preliminary positioning result P of the unmanned vehicle t As the initial estimate of point cloud registration, the coordinate transformation matrix T is constructed based on this pre , through the coordinate transformation matrix T pre The preprocessed 3D point cloud frame F t2 Pre-transform to the ground coordinate system. When the source point cloud contains isolated laser points, it will affect the statistical characteristics of the region. In order to obtain better point cloud registration results, the point cloud frame F needs to be transformed before point cloud registration. t2 Isolated laser points are removed in the 3D point cloud frame F transformed to the ground coordinate system through hierarchical spatial screening. t2 Construct the first data structure OcTree to complete the preprocessing of the 3D point cloud frame F at the current position t2 Perform spatial division to distribute the point cloud into spatial cube nodes, calculate the laser point density ρ of each node in the first data structure OcTree, and for the laser point density ρ lower than the density threshold ρ threshThe nodes of the second data structure KD-Tree are used to perform the nearest neighbor search to remove isolated laser points, and the 3D point cloud frame F as the source point cloud is obtained after the isolated laser points are removed. t3 .

[0029] Furthermore, the determination formula of the isolated laser point is as follows:

[0030] ||Point i -Point i-nearest ||>D thresh

[0031] Among them, Point i Indicates the pre-processed 3D point cloud frame F at the current position t2 The i-th laser point in Point i-nearest represents the laser point closest to the i-th laser point, D thresh Indicates the distance threshold for determining isolated laser points.

[0032] Furthermore, the key frame F in step S3 key The selection is done by setting the displacement change threshold ΔX max , whenever the displacement change ΔX of the wheel odometer is greater than or equal to the displacement change threshold ΔX max When the current frame is selected as the key frame F key ;

[0033] In order to select additional key frames when the unmanned vehicle turns to improve the stability of high-precision map construction and filter the noise of the inertial measurement unit information, the turning angle threshold ψ of the unmanned vehicle is set. max , construct a queue Q of the yaw angle ψ in the unmanned vehicle inertial measurement unit information to match the data frequency of different inertial measurement units. When there is data with different signs in the queue Q, clear the queue Q. When the queue Q is full, calculate the mean value Q of the data in the queue ave , if the mean value of the data in the queue is Q ave Greater than or equal to ψ max When the unmanned vehicle is considered to be in a turning state, the current frame is selected as the key frame F key And clear the queue Q.

[0034] Furthermore, in step S4, in order to reduce performance overhead as much as possible and avoid useless calculations, the unmanned vehicle performs loop detection only in areas that meet the requirements in both space and time, and forms a set of triples of historical key frame poses, point cloud frames corresponding to the historical key frame poses, and the time of the historical key frame poses:

[0035] A={(Pose1,Frame1,t1),(Pose2,Frame2,t2),…,(Pose n,Frame n ,t n )}

[0036] Among them, Pose i Represents the i-th key frame pose, Frame i represents the point cloud frame corresponding to the i-th key frame, t i represents the time corresponding to the i-th key frame, and the spatial search radius is recorded as R search , the time search threshold is recorded as t search , the area where the loop may exist satisfies the following formula:

[0037]

[0038] Among them, Pose cur and Pose i Represent the current key frame pose and the i-th key frame pose, t cur Indicates the time corresponding to the current keyframe.

[0039] Furthermore, in step S4, the loop detection method includes a key frame search strategy biased towards the historical data direction, extracting submaps for registration, and using overlapping laser point registration strategy;

[0040] The strategy of searching for key frames in the direction of historical data is to pre-set a maximum key frame search range k, set a historical data direction search range k1, and a new data direction search range k2, satisfying k1>k2;

[0041] In order to reduce memory performance overhead, it is not advisable to complete the preprocessing of the 3D point cloud frame F at the current position. t2 Directly perform point cloud registration with the global point cloud map Map, and extract submaps in areas where loops may exist. The extraction of submaps for registration is to record the key frame pose where the loop may exist as the mth key frame pose, and the extracted submap is represented as a set:

[0042]

[0043] Among them, Map sub Represents the extracted sub-map, Frame i Represents the point cloud corresponding to the i-th key frame;

[0044] Due to the limited field of view of the laser radar, the overlapped area of ​​the laser radar field of view obtained before and after the unmanned vehicle turns may be small, making it difficult to detect potential loops. In order to find potential loops as much as possible, only the laser points in the overlapping area can be used to complete the registration; the overlapping laser point registration strategy is to complete the preprocessed 3D point cloud frame F at the current position t2As the source point cloud, the submap Map sub As the target point cloud, the threshold for overlapping laser points is D, and the KD-Tree algorithm is used to perform the nearest neighbor search. The formula for screening overlapping laser points is as follows:

[0045] ||Target j -Source k ||≤D

[0046] Among them, Target j and Source k They represent the jth laser point of the target point cloud and the kth laser point of the source point cloud respectively. After filtering out the overlapping laser points, the preprocessed 3D point cloud frame F at the current position is t2 The overlapping laser points extracted from t2 ′, sub-map Map sub As the target point cloud, the ICP point cloud registration algorithm is used to align the source point cloud F t2 ′ and target point cloud Map sub Perform registration, detect loop closures, and if loop closures are detected, add loop constraints and perform graph optimization.

[0047] The advantages and beneficial effects of the present invention are:

[0048] The present invention proposes a method for constructing high-precision maps for unmanned vehicles in large outdoor scenes, which is suitable for mapping tasks under conditions where the laser radar field of view is limited; by improving the point cloud preprocessing method, a balance between performance cost and mapping quality is achieved; a key frame selection strategy is provided through wheel odometer information and inertial measurement unit information, which can effectively improve the mapping stability of unmanned vehicles in large outdoor scenes; the present invention also provides a loop detection method with low performance cost, which effectively improves the loop detection rate and reduces the map drift accumulated over a long period of operation; in addition, the sensors used in the present invention have fewer installation location restrictions, fewer types, and lower costs, which can significantly reduce the cost of the unmanned vehicle and meet the economic requirements. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 is a flow chart of a method in an embodiment of the present invention.

[0050] Figure 2a This is a schematic diagram of the laser radar installation position of the conventional mapping algorithm in the prior art.

[0051] Figure 2b It is a schematic diagram of the installation position of the laser radar in an embodiment of the present invention.

[0052] Figure 3a It is a 360° field of view point cloud top view obtained by conventional mapping algorithms in the prior art.

[0053] Figure 3b It is a top view of a point cloud with a field of view angle of about 240° obtained in an embodiment of the present invention.

[0054] Figure 4 4 is a flow chart of point cloud preprocessing in an embodiment of the present invention.

[0055] Figure 5 It is a flow chart of unmanned vehicle positioning in an embodiment of the present invention.

[0056] Figure 6 It is a flowchart of key frame selection and map construction in an embodiment of the present invention.

[0057] Figure 7 4 is a flow chart of loop detection in an embodiment of the present invention.

[0058] Figure 8 1 is a schematic top view of a U-turn scene of an unmanned vehicle in an embodiment of the present invention. DETAILED DESCRIPTION

[0059] The specific implementation of the present invention is described in detail below in conjunction with the accompanying drawings. It should be understood that the specific implementation described here is only used to illustrate and explain the present invention, and is not used to limit the present invention.

[0060] like Figure 1 , Figure 2a , Figure 2b , Figure 3a , Figure 3b As shown in the example, this example proposes a method for constructing high-precision maps for unmanned vehicles in large outdoor scenes. The traditional unmanned vehicle laser radar is installed as follows Figure 2a As shown, the laser radar of the unmanned vehicle of the embodiment of the present invention is installed as follows Figure 2b As shown, the map construction method specifically includes the following steps:

[0061] Step S1: Use the laser radar to obtain the 3D point cloud information at the current position of the unmanned vehicle, and perform 3D point cloud preprocessing through the straight-through filtering and gradient negative feedback voxel downsampling method. The point cloud image obtained by the traditional mapping algorithm is as follows: Figure 3a As shown, the point cloud image obtained in the embodiment of the present invention is as follows Figure 3b As shown, the preprocessing of 3D point cloud frames is as follows Figure 4 shown.

[0062] In order to reduce performance costs as much as possible without affecting the quality of high-precision map construction, it is necessary to first perform a straight-through filtering on the 3D point cloud frame. Specifically, the 3D point cloud frame at the current position collected by the LiDAR is recorded as F t , perform straight-through filtering based on the distance information and height information to obtain the 3D point cloud frame F after straight-through filtering t1, the direct filter will collect the 3D point cloud frame F at the current position collected by the laser radar t The laser points that satisfy the following formula are retained:

[0063]

[0064] Among them, x i ,y i , z i Respectively represent the 3D point cloud frame F at the current position t The real-time position P of the i-th laser point relative to the unmanned vehicle t1 The x, y, z axis coordinates, h max Indicates the set height threshold, R max Indicates the set distance threshold.

[0065] 3D point cloud frame F after straight-through filtering t1 , removing the 3D point cloud frame F at the current position t Laser points with medium heights above the height threshold and laser points with distances from the unmanned vehicle greater than the distance threshold. This step can reduce the performance overhead of all subsequent steps, increase the priority of laser points on the ground and nearby, and also increase the priority of laser points that are close to the unmanned sanitation vehicle, reducing the difficulty of road mapping.

[0066] 3D point cloud frame F after straight-through filtering t1 The point cloud range is reduced, but the point cloud is still too dense. In order to further reduce the performance cost, it is necessary to filter the 3D point cloud frame F after the direct filtering. t1 Perform gradient negative feedback voxel downsampling to obtain the preprocessed 3D point cloud frame F t2 .

[0067] In order to ensure the quality of high-precision map construction while making the most of the on-board computer performance of the unmanned vehicle, the downsampled 3D point cloud frame F t2 It should not be too sparse or too dense. It should retain as much laser point information as possible without causing the on-board computer of the unmanned vehicle to freeze. The gradient negative feedback voxel downsampling process includes the following steps:

[0068] Step S1.1: According to the performance of the unmanned vehicle system, set the total number of laser points that the computing device can process S, set the number of laser point errors that can be allowed to be ∈, set the initial value of the downsampling parameter to V, and set the initial value of the upward gradient of the downsampling parameter V to ΔV up , set the initial value of the upward gradient copy to ΔV up1 , set the initial value of the downward gradient of the downsampling parameter V to ΔV down , set the initial value copy of the downward gradient to ΔV down1 ;

[0069] Step S1.2: Receive the 3D point cloud frame F after direct filtering t1 , the 3D point cloud frame F after direct filtering t1 The number of laser points in is recorded as Adjust the initial value V of the downsampling parameter according to the following formula:

[0070]

[0071] Step S1.3: Downsampling is performed according to the adjusted downsampling parameter V, thereby obtaining a pre-processed 3D point cloud frame F t2 .

[0072] Step S2: Calculate the current position and posture of the unmanned vehicle based on the wheel odometer information, inertial measurement unit information and the optimized point cloud registration algorithm, where the layered spatial screening method removes isolated laser points as an optimization method for the point cloud registration algorithm. The process of calculating the current position and posture of the unmanned vehicle is as follows: Figure 5 shown.

[0073] Specifically, the initial positioning result P of the unmanned vehicle is obtained. t As the initial estimate of point cloud registration, before point cloud registration, the hierarchical spatial screening method is used to complete the preprocessing of the 3D point cloud frame F at the current position. t2 Remove the isolated laser points in the image, and get the 3D point cloud frame F after removing the isolated laser points. t3 .

[0074] Specifically, the ground plane speed of the unmanned vehicle can be obtained through the wheel odometer information, and the three-axis acceleration and three-axis angular velocity of the unmanned vehicle can be obtained through the inertial measurement unit information. The position of the unmanned vehicle at the previous moment is recorded as P t-1 , the time difference between the previous moment and the current moment is recorded as Δt, and the posture change is recorded as ΔP, then the preliminary positioning result P t =P t-1 +ΔP, ΔP is calculated as follows:

[0075]

[0076] Among them, Δx, Δy, and Δz represent the position changes in the x-axis, y-axis, and z-axis directions, respectively; Δφ, Δθ, and Δψ represent the angle changes around the x-axis, y-axis, and z-axis, respectively. They represent the speed of the unmanned vehicle in the x-axis and y-axis directions at the previous moment, They represent the acceleration of the unmanned vehicle in the x-axis, y-axis, and z-axis directions at the previous moment, respectively. They respectively represent the angular velocity of the unmanned vehicle around the x-axis, y-axis, and z-axis at the previous moment.

[0077] The present invention uses an NDT point cloud registration algorithm to complete point cloud registration. The NDT algorithm realizes point cloud registration by dividing the point cloud into a plurality of normally distributed small areas and minimizing the distances from the source point cloud to these distributions.

[0078] The performance of the NDT algorithm depends largely on a good initial estimate. If the initial estimate differs greatly from the true transformation, the algorithm may converge slowly. In order to reduce the performance cost of the NDT point cloud registration algorithm and speed up the point cloud registration, the present invention uses the preliminary positioning result P t As an initial estimate of point cloud registration, use the preliminary positioning result P t Construct the coordinate transformation matrix T pre , through the coordinate transformation matrix T pre The preprocessed 3D point cloud frame F t2 Pre-transform to the ground coordinate system.

[0079] The NDT algorithm mainly considers the statistical distribution of point clouds. When the source point cloud contains isolated laser points, it will affect the statistical characteristics of the region. In order to obtain better point cloud registration results, the point cloud frame F needs to be t2 Isolated laser point removal in .

[0080] The hierarchical spatial screening method first transforms the 3D point cloud frame F into the ground coordinate system t2 Construct an OcTree data structure to store the preprocessed 3D point cloud frame F at the current position t2 Perform spatial division to distribute the point cloud into cubic nodes with a side length of a, calculate the laser point density ρ of each node in OcTree, and for laser point density ρ below the density threshold ρ thresh The nodes of the KD-Tree data structure are constructed to perform the nearest neighbor search, and the laser points that satisfy the following formula are regarded as isolated laser points:

[0081] ||Point i -Point i-nearest ||>D thresh

[0082] Among them, Point i Indicates the pre-processed 3D point cloud frame F at the current position t2 The i-th laser point in Point i-nearest represents the laser point closest to the i-th laser point, D thresh To determine the distance threshold of isolated laser points; the 3D point cloud frame F t2 After the isolated laser points in are removed, the 3D point cloud frame F after the isolated laser points are removed is obtained. t3 .

[0083] Among them, the node side length a of the OcTree data structure cube in the hierarchical spatial screening method, the node density threshold ρ thresh And the distance threshold D for determining isolated laser points thresh Determined by user input or automatic algorithm to adapt to different application scenarios and accuracy requirements.

[0084] Use the 3D point cloud frame F after removing isolated laser points t3 As the source point cloud, the global point cloud map Map is used as the target point cloud, and the source point cloud F is registered by the NDT point cloud registration algorithm. t3 And the target point cloud Map, complete the point cloud registration, and get the real-time position P of the unmanned vehicle t1 and the coordinate transformation matrix T true .

[0085] Step S3: Select key frames based on the displacement information and turning information of the unmanned vehicle, and splice the key frames into the global point cloud map. The key frame selection and map construction process is as follows: Figure 6 shown.

[0086] In order to reduce the performance cost of high-precision map construction, not all point cloud frames are spliced ​​into the global point cloud map Map, but only some key frames F key Through the coordinate transformation matrix T true Transform to the real-time pose P of the unmanned vehicle t1 and set the key frame F key Stitched into the global point cloud map Map.

[0087] Keyframe F key The wheel odometer information and inertial measurement unit information are used to set the displacement change threshold ΔX max , whenever the displacement change ΔX of the wheel odometer is greater than the displacement change threshold ΔX max When the current frame is selected as the key frame F key .

[0088] In order to select additional key frames when the unmanned vehicle turns to improve the stability of high-precision map construction and filter the noise of the inertial measurement unit information, the turning angle threshold of the unmanned vehicle is set to ψ max , create a queue Q to store the value of the yaw angle ψ in the inertial measurement unit information. The queue length is set to 10 by default and can also be determined by user input to match the data frequency of different inertial measurement units.

[0089] When there are data with different signs in queue Q, clear queue Q. When queue Q is full, calculate the mean value Q of the data in the queue. ave , if the mean value of the data in the queue is Q ave Greater than ψ maxWhen the unmanned vehicle is considered to be in a turning state, the current frame is selected as the key frame F key And clear the queue Q.

[0090] Specifically, the unmanned vehicle selects the pre-processed 3D point cloud frame F at the current position t2 As a key frame F key , if and only if the following formula is satisfied:

[0091]

[0092] Where ΔX represents the displacement change of the wheel odometer, Q ave Indicates the average value of data in the queue when the queue Q is full. When the queue Q is full, the queue Q is cleared.

[0093] Step S4: The unmanned vehicle performs loop detection in areas where loops may exist. If a loop is detected, loop constraints are added and graph optimization is performed. Figure 7 shown.

[0094] Specifically, areas that meet the requirements in both space and time are areas where loops may exist. The loop detection methods include searching for key frames in the direction of historical data, extracting submaps for registration, and using overlapping laser point registration strategies.

[0095] In order to minimize performance overhead and avoid useless calculations, the unmanned vehicle only performs loop detection in areas that meet the requirements in both space and time.

[0096] The historical key frame pose, the point cloud frame corresponding to the historical key frame pose, and the time of the historical key frame pose are stored in the form of triplets. All triplets can be represented as a set:

[0097] A={(Pose1,Frame1,t1),(Pose2,Frame2,t2),…,(Pose n ,Frame n ,t n )}

[0098] Among them, Pose i Represents the i-th key frame pose, Frame i represents the point cloud frame corresponding to the i-th key frame, t i represents the time corresponding to the i-th key frame, and the spatial search radius is recorded as R search , the time search threshold is recorded as t search , where the loop may exist, satisfies the following formula:

[0099]

[0100] Among them, Posecur and Pose i Represent the current key frame pose and the i-th key frame pose, t cur Indicates the time corresponding to the current keyframe.

[0101] In the scenario where the unmanned vehicle turns around, since the driving direction of the unmanned vehicle changes, the key frames in the direction of historical data and their corresponding point cloud information are more valuable for loop detection. The historical data here is relative to the key frames searched through the above spatial and temporal information. In order to improve the loop detection rate under the condition of limited field of view of the lidar, some offsets should be introduced in the searched areas where loops may exist, and the search should be biased towards the direction of historical data when searching for historical key frames, such as Figure 8 As shown, the dots represent a series of key frame poses.

[0102] Specifically, the strategy of searching for key frames in the direction of historical data is to pre-set a maximum key frame search range k, set a search range k1 in the direction of historical data, and a search range k2 in the direction of new data, satisfying k1>k2.

[0103] In order to reduce memory performance overhead, it is not advisable to complete the preprocessing of the 3D point cloud frame F at the current position. t2 By directly registering the point cloud with the global point cloud map, sub-maps can be extracted from areas where loops may exist.

[0104] To extract the submap for registration, the key frame pose where the loop may exist is recorded as the mth key frame pose. The extracted submap can be expressed as a set:

[0105]

[0106] Among them, Map sub Represents the extracted sub-map, Frame i Represents the point cloud corresponding to the i-th key frame.

[0107] Due to the limited field of view of the laser radar, the overlapped area of ​​the laser radar field of view obtained by the unmanned vehicle before and after turning may be small, making it difficult to detect potential loops. In order to find potential loops as much as possible, only the laser points in the overlapping area can be used for registration. The overlapping laser point registration strategy is to complete the preprocessed 3D point cloud frame F at the current position. t2 As the source point cloud, the submap Map sub As the target point cloud, the overlapping laser point determination threshold is recorded as D, and the KD-Tree algorithm is used to perform the nearest neighbor search. The formula for screening overlapping laser points is as follows:

[0108] ||Target j -Source k ||≤D

[0109] Among them, Target j and Source k They represent the jth laser point of the target point cloud and the kth laser point of the source point cloud respectively. After filtering out the overlapping laser points, the preprocessed 3D point cloud frame F at the current position is t2 The overlapping laser points extracted from t2 ′, sub-map Map sub As the target point cloud, the ICP point cloud registration algorithm is used to align the source point cloud F t2 ′ and target point cloud Map sub Perform registration, detect loop closures, and if loop closures are detected, add loop constraints and perform graph optimization.

[0110] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some or all of the technical features thereof may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for constructing high-precision maps for unmanned vehicles in large outdoor scenes, characterized in that The steps include: Step S1: Obtain the point cloud information at the current position of the unmanned vehicle; Step S2: Using the preliminary positioning result of the unmanned vehicle as the initial estimate, using the hierarchical spatial screening method to remove isolated points in the point cloud at the current position, using the point cloud after removing the isolated points as the source point cloud, using the global point cloud map as the target point cloud, and using the point cloud registration algorithm to register the source point cloud and the target point cloud to obtain the real-time pose and coordinate transformation matrix of the unmanned vehicle; Step S3: selecting key frames according to the displacement information and turning information of the unmanned vehicle, transforming them to the real-time position of the unmanned vehicle through the coordinate transformation matrix, and splicing the key frames into the global point cloud map; Step S4: Based on the current and historical key frames, determine the area where loops may exist in space and time for loop detection. If a loop is detected, add loop constraints and perform map optimization.

2. The method for constructing a high-precision map for an unmanned vehicle in a large outdoor scene according to claim 1, characterized in that: In step S1, the point cloud information is preprocessed by straight-through filtering, and the point cloud frame F at the current position is removed according to the distance information and height information. t Points with a height higher than the height threshold and points with a distance from the unmanned vehicle greater than the distance threshold are obtained to obtain the point cloud frame F after direct filtering. t1 .

3. The method for constructing a high-precision map for an unmanned vehicle in a large outdoor scene according to claim 2, characterized in that: The straight-through filtering will transform the point cloud frame F at the current position t The points that satisfy the following formula are retained: Among them, x i ,y i , z i Respectively represent the point cloud frame F at the current position t The real-time position P of the i-th point relative to the unmanned vehicle t1 The x, y, z axis coordinates, h max Indicates the set height threshold, R max Indicates the set distance threshold.

4. The method for constructing a high-precision map for an unmanned vehicle in a large outdoor scene according to claim 1, characterized in that: In step S1, the point cloud frame is subjected to gradient negative feedback voxel downsampling to obtain a point cloud frame F t2 , including the following steps: Step S1.1: According to the performance of the unmanned vehicle system, set the total number of points S that the computing device can process, set the number of point errors allowed ∈, set the downsampling parameter V, and set the initial value ΔV of the upward gradient of the downsampling parameter V up and its replica ΔV up1 , set the initial value ΔV of the downward gradient of the downsampling parameter V down and its replica is ΔV down1 ; Step S1.2: Get the point cloud frame and its point count Adjust the downsampling parameter V according to the following formula: Step S1.3: Downsample according to the adjusted downsampling parameter V to obtain the preprocessed point cloud frame F t2 .

5. The method for constructing a high-precision map for an unmanned vehicle in a large outdoor scene according to claim 1, characterized in that: In step S2, the ground plane velocity, acceleration and angular velocity of the unmanned vehicle are obtained, and based on the posture P of the unmanned vehicle at the previous moment t-1 The time difference Δt and the position change ΔP from the current moment are used to calculate the initial positioning result P of the unmanned vehicle. t =P t-1 +ΔP, ΔP is calculated as follows: Among them, Δx, Δy, and Δz represent the position changes in the x-axis, y-axis, and z-axis directions, respectively; Δφ, Δθ, and Δψ represent the angular changes around the x-axis, y-axis, and z-axis, respectively. They represent the speed of the unmanned vehicle in the x-axis and y-axis directions at the previous moment, They represent the acceleration of the unmanned vehicle in the x-axis, y-axis, and z-axis directions at the previous moment, respectively. They respectively represent the angular velocity of the unmanned vehicle around the x-axis, y-axis, and z-axis at the previous moment.

6. The method for constructing a high-precision map for an unmanned vehicle in a large outdoor scene according to claim 1, characterized in that: In the step S2, the preliminary positioning result of the unmanned vehicle is used as the initial estimate of the point cloud registration, and a coordinate transformation matrix is ​​constructed based on this. The point cloud is pre-transformed to the ground coordinate system through the coordinate transformation matrix. Through hierarchical spatial screening, a first data structure is constructed for the point cloud transformed to the ground coordinate system, and the point cloud at the current position is spatially divided so that the point cloud is distributed to spatial nodes. The point density of each node in the first data structure is calculated, and a second data structure is constructed for nodes below the density threshold to perform nearest neighbor search to eliminate isolated points and obtain the source point cloud.

7. The method for constructing a high-precision map for an unmanned vehicle in a large outdoor scene according to claim 6, characterized in that: The determination formula of the isolated point is as follows: ||Point i -Point i-nearest ||>D thresh Among them, Point i Indicates the i-th point in the point cloud at the current position, Point i-nearest represents the point closest to the i-th point, D thresh Indicates the distance threshold for determining isolated points.

8. The method for constructing a high-precision map for an unmanned vehicle in a large outdoor scene according to claim 1, characterized in that: The key frame selection in step S3 is to set a displacement change threshold, and whenever the displacement change is greater than or equal to the displacement change threshold, select the current frame as the key frame F. key ; By setting the turning angle threshold of the unmanned vehicle, a queue of the yaw angle of the unmanned vehicle is constructed. When there is data with different signs in the queue, the queue is cleared. When the queue is full, the mean of the data in the queue is calculated. If the mean of the data in the queue is greater than or equal to , the current frame is selected as the key frame and the queue is cleared.

9. The method for constructing a high-precision map for an unmanned vehicle in a large outdoor scene according to claim 1, characterized in that: In step S4, the unmanned vehicle performs loop detection only in the area that meets the requirements in both space and time, and forms a set of three tuples including the historical key frame pose, the point cloud frame corresponding to the historical key frame pose, and the time of the historical key frame pose: A={(Pose1,Frame1,t1),(Pose2,Frame2,t2),…,(Pose n ,Frame n ,t n )} Among them, Pose i Represents the i-th key frame pose, Frame i represents the point cloud frame corresponding to the i-th key frame, t i represents the time corresponding to the i-th key frame, and the spatial search radius is recorded as R search , the time search threshold is recorded as t search , the area where the loop may exist satisfies the following formula: Among them, Pose cur and Pose i Represent the current key frame pose and the i-th key frame pose, t cur Indicates the time corresponding to the current keyframe.

10. The method for constructing a high-precision map for an unmanned vehicle in a large outdoor scene according to claim 1, characterized in that: In step S4, the loop detection method includes searching for key frames in the direction of historical data, extracting submaps for registration, and using overlapping point registration strategy; The strategy of searching for key frames in the direction of historical data is to pre-set a maximum key frame search range k, set a historical data direction search range k1, and a new data direction search range k2, satisfying k1>k2; The extraction of submaps for registration is to record the key frame pose where there may be a loop as the mth key frame pose, and the extracted submap is represented as a set: Among them, Map sub Represents the extracted sub-map, Frame i Represents the point cloud corresponding to the i-th key frame; The overlapping point registration strategy is to use the point cloud at the current position as the source point cloud, the submap as the target point cloud, the overlapping point determination threshold is D, and the nearest neighbor search is used. The formula for screening overlapping points is as follows: ||Target j -Source k ||≤D Among them, Target j and Source k They represent the jth point of the target point cloud and the kth point of the source point cloud respectively. After filtering out the overlapping points, the overlapping points extracted from the point cloud at the current position are taken as the source point cloud F t2 ′ , take the sub-map as the target point cloud, and then use the point cloud registration algorithm to align the source point cloud F t2 ′ Align with the target point cloud, detect loops, and if loops are detected, add loop constraints and perform graph optimization.

Images