Initialized positioning system and method based on cross-country scene point cloud feature matching

Abstract

The invention provides an initialized positioning method based on cross-country scene point cloud feature matching, and the method comprises the steps: 1, obtaining global positioning, and judging the deviation of a vehicle position; step 2, when the vehicle position deviation is smaller than a threshold value T1, turning to step 6, and when the vehicle position deviation is larger than the threshold value T1 and smaller than a threshold value T2, obtaining radar point cloud, removing obstacle point cloud in the radar point cloud, and then converting the point cloud into a global coordinate system; step 3, loading a point cloud map near the vehicle; 4, matching the reference point cloud in the in-situ initialization file with the to-be-registered point cloud in the loaded point cloud map, and judging whether the matched pose meets the initialization requirement or not; 5, if not, turning to the step 4 and continuing matching, and if yes, obtaining current global positioning and turning to the step 6; and step 6, segmenting the laser point cloud map into a plurality of sub-maps, performing feature extraction on the sub-maps, and matching the sub-maps with the global map to complete laser feature matching positioning.

Description

Technical Field

[0001] The present application relates to the field of autonomous driving technology, and in particular to a system and method for initializing positioning based on off-road scene point cloud feature matching. Background Art

[0002] In the field of autonomous driving, precise positioning is a key technology for safe vehicle operation. This is particularly true in off-road scenarios, where complex terrain and changing environments pose even greater challenges for positioning. Traditional positioning methods often suffer from insufficient accuracy and long initialization times in off-road scenarios, making them difficult to meet real-time and accuracy requirements. Currently used point cloud registration methods, such as ICP and its improved algorithms, achieve point cloud alignment by extracting and matching feature points (such as edges and planes) within the point cloud. However, this method has high requirements for initial positioning and is prone to falling into local optima when the initial positions of the point clouds differ significantly. Furthermore, the iteration time is long, the feature extraction and matching process is complex, and the computational efficiency is low, making it unable to meet real-time requirements. Another alternative is the point field feature (PRF) registration method. Compared to the PPF algorithm, this algorithm uses two additional descriptors, which increases the algorithm time. Furthermore, in off-road scenarios, it may face challenges such as complex terrain and obstruction caused by vegetation, which affects its applicability and performance. Summary of the Invention

[0003] In view of this, the present invention provides a system and method for initializing positioning based on off-road scene point cloud feature matching to solve the technical problem that the existing technology is insufficiently robust in complex off-road scenes and cannot effectively adapt to the positioning requirements of off-road scenes.

[0004] The present invention provides an initialization positioning method based on off-road scene point cloud feature matching, which is applied to intelligent driving vehicles. The method includes: step 1, obtaining global positioning by combining navigation and on-site initialization files, and judging the deviation of vehicle position according to the confidence of global positioning; step 2, when the vehicle position deviation is less than threshold T1, going to step 6, when the vehicle position deviation is greater than threshold T1 and less than threshold T2, obtaining radar point cloud by laser radar, removing obstacle point cloud therein, and then converting point cloud to global coordinate system; step 3, using parallel mechanism to load point cloud map near vehicle according to global positioning; step 4, using multi-resolution Matching algorithm, match the reference point cloud in the original initialization file with the point cloud to be registered in the loaded point cloud map, and judge whether the matched pose meets the initialization requirements based on the matching score; Step 5, if not, go to Step 4 and continue matching, if so, obtain the current global positioning and go to Step 6; Step 6, obtain the initial position of the initial vehicle based on the global positioning, divide the laser point cloud map into multiple sub-maps, extract features from the sub-maps, match them with the global map, eliminate incorrect matches, perform filtering and fusion, and complete laser feature matching positioning, where c is the curvature, S is the sub-image, ri, rj are the points in the sub-image.

[0005] Furthermore, step 2 also includes: when the vehicle position deviation is greater than T2, initial positioning cannot be performed, a fault code is issued, and the vehicle-side system is reported.

[0006] Furthermore, T1=1m, T2=5m.

[0007] Furthermore, the step 4 includes: step 41, stratifying the laser point cloud according to the resolution, voxelizing the reference point cloud and the point cloud to be registered at different resolutions, and obtaining voxel grid representations at different resolutions, with resolutions of r1, r2...rn, and the corresponding voxelized reference point clouds are C(1), C(2),..., C(n), and the point clouds to be registered are Q(1), Q(2),...Q(n); step 42, obtaining the point cloud to be matched that falls in the voxels of the reference point cloud at the same resolution, and obtaining the point cloud density in each voxel of the reference point cloud, and the matching score at this resolution is the sum of the point cloud densities of the voxels of each reference point cloud; step 43, at the lowest resolution r1, calculating the change matrix T(1)=arg at the resolution r1 according to the matching score at this resolution. maxS(1)(T), where S(1)(T) is the matching score at the lowest resolution r1, and T(1) is the rotation matrix when S(1)(T) is the maximum. Step 44: use T(1) as the initial change matrix at the next layer resolution r2, and calculate the change matrix T(2)=arg maxS(2)(T│T(1)) according to the matching score at resolution r2, where S(2)(T│T(1)) is the matching score at resolution r2 with T(1) as the initial value, and T(1) is the rotation matrix when S(2)(T│T(1)) is the maximum. Repeat this step until the matching score at the highest resolution Rn is calculated. Step 45: compare the matching score at the highest resolution Rn with the preset parameters. If it is less than the preset parameters, the initialization requirement is met.

[0008] Furthermore, the feature extraction includes: step 61, dividing the point cloud map horizontally into a number of equal sub-images; step 62, calculating the curvature of the sub-images; step 63, sorting the points in each row of each sub-image according to the curvature size; step 64, selecting the points with the largest and smallest curvature in each row as feature points.

[0009] The present invention also provides a system for initializing positioning based on off-road scene point cloud feature matching, characterized in that the system includes: a judgment module for obtaining global positioning through combined navigation and on-site initialization files, judging the deviation of the vehicle position according to the confidence of the global positioning, and sending the global positioning to the laser positioning matching module when the vehicle position deviation is less than the threshold value T1, and sending the global positioning to the initialization module when the vehicle position deviation is greater than the threshold value T1 and less than the threshold value T2; the initialization module is connected to the judgment module, and is used to remove the obstacle point cloud in the radar point cloud obtained by the laser radar, and then convert the remaining point cloud into the global coordinate system, and use a parallel mechanism to load the vehicle's attached points according to the global position. The nearest point cloud map adopts a multi-resolution matching algorithm to match the reference point cloud in the original initialization file with the point cloud to be registered in the loaded point cloud map. The matching score is used to determine whether the matched posture meets the initialization requirements. If not, the matching continues until the initialization requirements are met. If so, the current global positioning is sent to the laser positioning matching module; the laser positioning matching module is connected to the judgment module and the initialization module respectively, and is used to obtain the initial position of the initial vehicle according to the global positioning, divide the laser point cloud map into multiple sub-maps, extract features from the sub-maps, match them with the global map, eliminate false matches, filter and fuse them, and complete the laser feature matching positioning.

[0010] Furthermore, the initialization module is also used to: when the vehicle position deviation is greater than T2, initialization positioning cannot be performed, a fault code is issued, and a report is made to the vehicle-side system.

[0011] Furthermore, T1=1m, T2=5m.

[0012] Furthermore, the integrated navigation includes an inertial navigation system and a global satellite navigation system.

[0013] The present invention provides an initialization positioning system and method based on off-road scene point cloud feature matching, which is mainly used to solve the problem that the center position of the point cloud grid and the actual center of gravity position in the existing technology deviate from each other, resulting in the matching trajectory not matching the actual trajectory and insufficient robustness in complex off-road scenes; in off-road scenes, vehicles face complex and changeable terrain and environment. Traditional positioning methods are difficult to complete initialization positioning quickly and accurately, and cannot effectively adapt to the positioning needs of off-road scenes. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] Figure 1 This is a flow chart of a method for initializing positioning based on off-road scene point cloud feature matching provided by the present invention; Figure 2 This is a flow chart of a matching judgment method provided by the present invention; Figure 3 It is a flow chart of the feature extraction method provided by the present invention. DETAILED DESCRIPTION

[0015] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments 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.

[0016] Method Example: The present invention provides an initialization positioning method based on off-road scene point cloud feature matching, such as Figure 1 As shown, the method includes the following steps.

[0017] Step 1: Obtain global positioning by combining navigation and local initialization files, and determine the deviation of vehicle position based on the confidence of global positioning; Step 2: When the vehicle position deviation is less than threshold T1, go to step 6. When the vehicle position deviation is greater than threshold T1 and less than threshold T2, obtain the radar point cloud through the lidar, remove the obstacle point cloud, and then convert the point cloud to the global coordinate system; Because off-road scenarios are complex and ever-changing, and the combined navigation signal is easily obstructed, making a fixed solution difficult, it is necessary to judge and process the position based on the accuracy of global positioning, that is, relying on the confidence level of global positioning. Based on practical experience, T1=1m and T2=5m. When the vehicle position deviation is less than 1m, laser feature matching positioning can be directly performed. When the vehicle position deviation is greater than 1m and less than 5m, coarse positioning initialization is performed. This means that the radar point cloud is acquired through the lidar, the obstacle point cloud is removed, and the point cloud is converted to the global coordinate system. At the same time, a parallel mechanism is used to load the point cloud map near the vehicle based on the global position. The NDT matching algorithm, also known as the multi-resolution matching algorithm, is used to continuously match the initial position until the matching effect meets the initialization requirement. The NDT matching score is used to determine whether the matched pose meets the initialization requirement. When the vehicle position deviation is greater than 5m, initial positioning cannot be performed, a fault code is issued, and the vehicle-side system is reported.

[0018] Step 3: Load the point cloud map near the vehicle based on global positioning; Step 4: Use a multi-resolution matching algorithm to match the reference point cloud in the original initialization file with the point cloud to be registered in the loaded point cloud map, and judge whether the matched pose meets the initialization requirements based on the matching score; like Figure 2 As shown, step 4 includes the following.

[0019] Step 41, the laser point cloud is layered according to the resolution, and the reference point cloud and the point cloud to be registered are voxelized at different resolutions to obtain voxel grid representations at different resolutions, with the resolutions being r1, r2...rn. The corresponding voxelized reference point clouds are C(1), C(2),..., C(n), and the point clouds to be registered are Q(1), Q(2),...Q(n). First, the points are layered according to the resolution, i.e., resolution r1 is the first layer, which is also the lowest layer, r2 is the second layer, and so on, the highest layer is rn. Voxelization is performed at the resolution of each layer to obtain a voxel grid representation of the point cloud.

[0020] Step 42: At the same resolution, obtain the point cloud to be matched that falls within the voxels of the reference point cloud, and obtain the point cloud density in each voxel of the reference point cloud. The matching score at this resolution is the sum of the point cloud densities of each voxel of the reference point cloud. Step 43, at the lowest resolution r1, based on the matching score at the resolution, calculate the change matrix T(1)=arg maxS(1)(T) at the resolution r1, where S(1)(T) is the matching score at the lowest resolution r1, and T(1) is the rotation matrix when S(1)(T) is the maximum. The arg max function is a mathematical function that represents the value of the independent variable x that maximizes a given function f(x). In other words, arg max(f(x)) is the variable x that maximizes f(x). In mathematical optimization and machine learning, the arg max function is often used to find the optimal values ​​of model parameters to maximize the objective function.

[0021] Step 44: Use T(1) as the initial change matrix at the next resolution r2, and calculate the change matrix T(2)=arg maxS(2)(T│T(1)) based on the matching score at resolution r2, where S(2)(T│T(1)) is the matching score at resolution r2 with T(1) as the initial value, and T(2) is the rotation matrix when S(2)(T│T(1)) is the maximum. Repeat this step until the matching score at the highest resolution Rn is calculated. Since T(1) is the initial change matrix at the next layer resolution r2, the reference point cloud C(2) and the point cloud to be registered Q(2) are matched through the change matrix T(1) at resolution r2, thereby obtaining the matching score at resolution r2. That is, at resolution r2, the point cloud to be matched that falls within the voxels of the reference point cloud is obtained, and the point cloud density in each reference point cloud voxel is obtained. The matching score at resolution r2 is the sum of the point cloud densities of each reference point cloud voxel. This completes the hierarchical matching and parameter transfer.

[0022] In step 45 , the matching score at the highest resolution Rn is compared with the preset parameters. If the matching score is less than the preset parameters, the initialization requirement is met.

[0023] Compared to traditional point cloud registration algorithms, the multi-resolution matching algorithm used in this paper has lower requirements for initial position accuracy, shorter iteration time, and higher computational efficiency. The preset parameters are set based on empirical values, and the smaller the matching score, the better the positioning status.

[0024] Step 5: If no, go to step 4 and continue matching. If yes, get the current global positioning and go to step 6. Step 6: Obtain the initial position of the initial vehicle based on global positioning, divide the laser point cloud map into multiple sub-maps, extract features from the sub-maps, match them with the global map, eliminate false matches, perform filtering and fusion, and complete laser feature matching and positioning.

[0025] like Figure 3 As shown, the feature extraction includes the following steps.

[0026] Step 61: Divide the point cloud map horizontally into a number of equal sub-images; Step 62, calculating the sub-image curvature; Step 63, sorting the points in each row of each sub-image according to the curvature; Step 64 : Select the points with the maximum and minimum curvature in each row as feature points.

[0027] After sorting by curvature, the point with the largest curvature is determined as a non-ground point, and the point with the smallest curvature is determined as a ground point. Therefore, the maximum and minimum curvature points of each row can be used as feature points.

[0028] The present invention provides an initialization positioning method based on off-road scene point cloud feature matching, which is mainly used to solve the problem that the center position of the point cloud grid and the actual center of gravity position in the existing technology deviate from each other, resulting in the matching trajectory not matching the actual trajectory and insufficient robustness in complex off-road scenes; in off-road scenes, vehicles face complex and changeable terrain and environment. Traditional positioning methods are difficult to complete initialization positioning quickly and accurately, and cannot effectively adapt to the positioning needs of off-road scenes.

[0029] Device Item Example: The present invention provides a system based on an off-road scene point cloud feature matching initialization positioning method, the system comprising a judgment module, an initialization module and a laser positioning matching module; The judgment module is used to obtain global positioning through combined navigation and on-site initialization files, and to judge the deviation of the vehicle position based on the confidence of the global positioning. When the vehicle position deviation is less than the threshold value T1, the global positioning is sent to the laser positioning matching module. When the vehicle position deviation is greater than the threshold value T1 and less than the threshold value T2, the global positioning is sent to the initialization module. The judgment module is also used to: when the vehicle position deviation is greater than T2, initialization positioning cannot be performed, a fault code is issued, and the vehicle-side system is reported. Here, T1 and T2 can be adjusted according to actual conditions. Under normal circumstances, T1 = 1m and T2 = 5m. The combined navigation includes an inertial navigation system and a global satellite navigation system.

[0030] The initialization module is connected to the judgment module and is used to remove obstacle point clouds from the radar point cloud obtained by the lidar, and then convert the remaining point clouds into the global coordinate system. A parallel mechanism is used to load the point cloud map near the vehicle according to the global position. A multi-resolution matching algorithm is used to match the reference point cloud in the original initialization file with the point cloud to be registered in the loaded point cloud map. The matching score is used to determine whether the matched posture meets the initialization requirements. If not, the matching continues until the initialization requirements are met. If so, the current global positioning is sent to the laser positioning matching module.

[0031] The laser positioning and matching module is connected to the judgment module and the initialization module respectively. It is used to obtain the initial position of the initial vehicle based on the global positioning, divide the laser point cloud map into multiple sub-maps, extract features from the sub-maps, match them with the global map, eliminate false matches, perform filtering and fusion, and complete laser feature matching and positioning.

[0032] The present invention provides an initialization positioning system based on off-road scene point cloud feature matching, which is mainly used to solve the problem that the center position of the point cloud grid and the actual center of gravity position in the existing technology deviate from each other, resulting in the matching trajectory not matching the actual trajectory and insufficient robustness in complex off-road scenes; in off-road scenes, vehicles face complex and changeable terrain and environment, and traditional positioning methods are difficult to complete initialization positioning quickly and accurately, and cannot effectively adapt to the positioning needs of off-road scenes.

[0033] In summary, the embodiments of the present invention provide a system and method for initializing positioning based on off-road scene point cloud feature matching. This technical solution relies on combined navigation and on-site initialization files to obtain the global position, and relies on the confidence of the global positioning for judgment. It is divided into fine positioning initialization and coarse positioning initialization. That is, when the initialized global position is less than 1m, laser feature matching positioning can be performed directly. When the initialized global position is greater than 1m and less than 5m, the NDT (pcl_omp) matching algorithm is used to continuously match the initialized position until the matching effect meets the initialization requirement, so as to solve the problems existing in the existing technology and achieve efficient and accurate initialization positioning.

[0034] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A method for initializing and positioning off-road scene point cloud feature matching, applied to intelligent driving vehicles, characterized by: The method comprises: Step 1: Obtain global positioning by combining navigation and local initialization files, and determine the deviation of vehicle position based on the confidence of global positioning; Step 2: When the vehicle position deviation is less than threshold T1, go to step 6. When the vehicle position deviation is greater than threshold T1 and less than threshold T2, obtain the radar point cloud through the lidar, remove the obstacle point cloud, and then convert the point cloud to the global coordinate system; Step 3: Use a parallel mechanism to load the point cloud map near the vehicle based on global positioning; Step 4: Use a multi-resolution matching algorithm to match the reference point cloud in the original initialization file with the point cloud to be registered in the loaded point cloud map, and judge whether the matched pose meets the initialization requirements based on the matching score; Step 5: If no, go to step 4 and continue matching. If yes, get the current global positioning and go to step 6. Step 6: Obtain the initial position of the initial vehicle based on global positioning, divide the laser point cloud map into multiple sub-maps, calculate the curvature of each point in the sub-image, take the points with the largest and smallest curvatures as feature points, perform feature extraction, match them with the global map, eliminate false matches, perform filtering and fusion, and complete laser feature matching positioning.

2. According to the off-road scene point cloud feature matching initialization positioning method of claim 1, step 2 further includes: When the vehicle position deviation is greater than T2, initial positioning cannot be performed, a fault code is issued, and the vehicle-side system is reported.

3. According to the off-road scene point cloud feature matching initialization positioning method described in claim 1 or 2, T1=1m, T2=5m.

4. According to the off-road scene point cloud feature matching initialization positioning method of claim 1, step 4 comprises: Step 41, the laser point cloud is layered according to the resolution, and the reference point cloud and the point cloud to be registered are voxelized at different resolutions to obtain voxel grid representations at different resolutions, with the resolutions being r1, r2...rn. The corresponding voxelized reference point clouds are C(1), C(2),..., C(n), and the point clouds to be registered are Q(1), Q(2),...Q(n). Step 42: At the same resolution, obtain the point cloud to be matched that falls within the voxels of the reference point cloud, and obtain the point cloud density in each voxel of the reference point cloud. The matching score at this resolution is the sum of the point cloud densities of each voxel of the reference point cloud. Step 43, at the lowest resolution r1, based on the matching score at the resolution, calculate the change matrix T(1)=arg maxS(1)(T) at the resolution r1, where S(1)(T) is the matching score at the lowest resolution r1, and T(1) is the rotation matrix when S(1)(T) is the maximum. Step 44: Use T(1) as the initial change matrix at the next resolution r2, and calculate the change matrix T(2)=arg maxS(2)(T│T(1)) based on the matching score at resolution r2, where S(2)(T│T(1)) is the matching score at resolution r2 with T(1) as the initial value, and T(2) is the rotation matrix when S(2)(T│T(1)) is the maximum. Repeat this step until the matching score at the highest resolution Rn is calculated. In step 45 , the matching score at the highest resolution Rn is compared with the preset parameters. If the matching score is less than the preset parameters, the initialization requirement is met.

5. According to the off-road scene point cloud feature matching initialization positioning method of claim 1, the feature extraction includes: Step 61: Divide the point cloud map horizontally into a number of equal sub-images; Step 62, calculating the curvature of each point in the sub-image; Step 63, sorting the points in each row of each sub-image according to the curvature; Step 64 : Select the points with the maximum and minimum curvature in each row as feature points.

6. A system for implementing the off-road scene point cloud feature matching initialization positioning method according to claims 1-5, characterized in that: The system comprises: The judgment module is used to obtain global positioning by combining navigation and in-situ initialization files, and judge the deviation of the vehicle position according to the confidence of the global positioning. When the vehicle position deviation is less than the threshold T1, the global positioning is sent to the laser positioning matching module. When the vehicle position deviation is greater than the threshold T1 and less than the threshold T2, the global positioning is sent to the initialization module; The initialization module is connected to the judgment module and is used to remove obstacle point clouds from the radar point cloud acquired by the lidar, and then convert the remaining point cloud into the global coordinate system. A parallel mechanism is used to load the point cloud map near the vehicle according to the global position. A multi-resolution matching algorithm is used to match the reference point cloud in the original initialization file with the point cloud to be registered in the loaded point cloud map. The matching score is used to determine whether the matched pose meets the initialization requirements. If not, the matching continues until the initialization requirements are met. If so, the current global positioning is sent to the laser positioning matching module. The laser positioning and matching module is connected to the judgment module and the initialization module respectively. It is used to obtain the initial position of the initial vehicle according to the global positioning, divide the laser point cloud map into multiple sub-maps, extract features from the sub-maps, match them with the global map, eliminate false matches, perform filtering and fusion, and complete the laser feature matching and positioning. Where c is the curvature, S is the sub-image, and r is the image. i ,r j is a point in the sub-image.

7. According to the off-road scene point cloud feature matching initialization positioning system described in claim 6, the judgment module is also used to: when the vehicle position deviation is greater than T2, initialization positioning cannot be performed, a fault code is issued, and a report is sent to the vehicle-side system.

8. The off-road scene point cloud feature matching initialization positioning system according to claim 6 or 7, wherein T1 = 1m and T2 = 5m.

9. The off-road scene point cloud feature matching-based initialization positioning system according to claim 6, wherein the integrated navigation comprises an inertial navigation system and a global satellite navigation system.

Images