Point cloud positioning method, computer readable storage medium and intelligent device

Abstract

The application relates to the technical field of automatic driving, and particularly provides a point cloud positioning method, a computer readable storage medium and an intelligent device, and aims to solve the problem of improving point cloud positioning accuracy. To this end, the method provided by the application comprises voxelizing a current frame point cloud to form a plurality of voxels; the types of the voxels are determined respectively according to the distribution characteristics of the point cloud in each voxel; the voxels of each type are screened respectively to balance the number of voxels of each type; and the voxels reserved after screening are registered with a prior map to obtain the pose of the current frame point cloud. Through the above method, the voxelization of the current frame point cloud realizes the downsampling of the point cloud, and the computing power consumption can be greatly reduced. In addition, the above method balances the number of voxels of each type through screening, can significantly improve the registration accuracy, and thus the pose of the current frame point cloud can be accurately obtained.

Description

Technical Field

[0001] This application relates to the field of autonomous driving technology, specifically to a point cloud localization method, a computer-readable storage medium, and a smart device. Background Technology

[0002] When controlling a vehicle for autonomous driving, the point cloud acquired by radar is usually registered with a prior map to obtain an accurate point cloud pose. Then, the accurate vehicle pose is obtained based on the point cloud pose, and the vehicle driving path is planned based on the vehicle pose, and the vehicle is controlled to drive automatically according to the driving path.

[0003] Current conventional point cloud localization methods primarily involve registering all point clouds within a single frame with a prior map. Since a single frame can contain a large number of point clouds, this significantly increases the computational burden on the vehicle. Furthermore, when registering point clouds with the prior map, map points matching each point cloud are typically obtained from the prior map, and each point cloud is then registered with its respective matching map point as a pair. However, in open, unobstructed scenes, because the point clouds within a single frame are primarily ground point clouds, a large number of ground point cloud matching pairs are obtained, while matching pairs for other types of point clouds are very few. Using these matching pairs for registration significantly reduces the accuracy of point cloud localization, thereby affecting vehicle safety.

[0004] Accordingly, a new technical solution is needed in this field to solve the above problems. Summary of the Invention

[0005] In order to overcome the above-mentioned deficiencies, this application is made to provide a point cloud positioning method, a computer-readable storage medium, and a smart device that solves or at least partially solves the technical problem of how to improve the accuracy of point cloud positioning, especially the accuracy of point cloud positioning in open and unobstructed scenes.

[0006] In a first aspect, a point cloud localization method is provided, the method comprising: The point cloud of the current frame is voxelized to form multiple voxels; Based on the distribution characteristics of point clouds within each voxel, the type of each voxel is determined. Each type of voxel is screened separately to balance the number of each type of voxel; The selected and retained voxels are registered with the prior map to obtain the pose of the point cloud in the current frame.

[0007] In one technical solution of the above point cloud localization method, the distribution characteristics of the point cloud include the covariance matrix of the point cloud distribution, and the determination of the type of each voxel includes: For each voxel, the flatness and normal vector of the point cloud cluster formed by the point cloud within the voxel are obtained based on the covariance matrix of the point cloud distribution within the voxel. Obtain the angle between the normal vector and the ground normal vector; When the flatness is greater than a first set threshold, if the included angle is greater than a second set threshold, then the voxel is a vertical plane voxel; otherwise, the voxel is a ground voxel. When the flatness is less than or equal to a first set threshold, the voxel is a non-planar voxel.

[0008] In one technical solution of the above point cloud localization method, the types include ground voxels, vertical plane voxels, and non-planar voxels. The filtering of each type of voxel includes multi-level filtering through the following methods: The ground voxels and the vertical plane voxels are screened separately to remove inferior voxels from the ground voxels and the vertical plane voxels respectively; The non-planar voxels are screened based on the preset maximum number of non-ground voxels and the number of vertical planar voxels retained after removing inferior voxels, so that the sum of the number of non-planar voxels and the number of vertical planar voxels retained is less than or equal to the maximum number. The ground voxels after removing inferior voxels are screened again so that the proportion of the ground voxels in all retained voxels is less than or equal to a set ratio.

[0009] In one technical solution of the above point cloud localization method, after the ground voxels after removing inferior voxels are screened again, the multi-level screening further includes: The point cloud of the current frame after voxelization is rasterized again to form multiple grids, each grid comprising multiple voxels. Based on the grid, voxels retained by each type of screening are uniformly sampled.

[0010] In one technical solution of the above point cloud localization method, the method further includes screening ground voxels and vertical plane voxels in the following manner to remove inferior voxels: Obtain a first map voxel on the prior map that matches the ground voxel, and obtain a second map voxel on the prior map that matches the vertical plane voxel; Determine whether the ground voxel and the first map voxel satisfy the preset constraint conditions between the two grounds; if not, remove the ground voxel. Determine whether the vertical plane voxel and the second map voxel satisfy the preset constraint conditions between the two vertical planes; if not, remove the vertical plane voxel. The first map voxel and the second map voxel are formed by voxelizing the prior map.

[0011] In one technical solution of the above point cloud localization method, the distribution characteristics of the point cloud include the covariance matrix of the point cloud distribution, and the step of determining whether the ground voxel and the first map voxel satisfy the preset constraint conditions between the two grounds includes: Based on the covariance matrix of the point cloud distribution within the ground voxels, the first flatness and the first normal vector of the point cloud cluster formed by the point cloud within the ground voxels are obtained. Obtain the second planarity of the point cloud cluster formed by the point cloud within the first map voxel; If both the first flatness and the second flatness are greater than the third set threshold, and the angle between the first normal vector and the ground normal vector is less than the fourth set threshold, then it is determined that the constraint condition is met; otherwise, it is determined that the constraint condition is not met.

[0012] In one technical solution of the above point cloud localization method, the distribution characteristics of the point cloud include the covariance matrix of the point cloud distribution, and the step of determining whether the vertical plane voxel and the second map voxel satisfy the preset constraint conditions between the two vertical planes includes: Based on the covariance matrix of the point cloud distribution within the vertical plane voxels, obtain the second flatness and the second normal vector of the point cloud cluster formed by the point clouds within the vertical plane voxels. Obtain the third planarity and third normal vector of the point cloud cluster formed by the point cloud within the second map voxel; If both the second and third flatness are greater than the fifth set threshold, and the angle between the second and third normal vectors is less than the sixth set threshold, then the constraint condition is satisfied; otherwise, the constraint condition is not satisfied.

[0013] In one technical solution of the above point cloud localization method, the registration of the selected and retained voxels with the prior map includes: For each preserved voxel, obtain the map voxel on the prior map that matches the preserved voxel, wherein the map voxel is formed by voxelizing the prior map; For each preserved voxel, a residual term is constructed based on the distribution characteristics of the point cloud within the preserved voxel and the distance difference between the distribution characteristics of the point cloud within the map voxel. Registration is performed based on the residual terms of each retained voxel.

[0014] In one technical solution of the above point cloud localization method, the current frame point cloud is a point cloud acquired by radar on a smart device, and the smart device further includes a wheel speedometer and an IMU. After registering the selected and retained voxels with the prior map, the method further includes: Obtain the pose of the point cloud at the current frame using the wheel velocity meter and IMU; The pose of the current frame point cloud and the pose obtained by the wheel velocity meter and IMU are fused to obtain the final pose.

[0015] In a second aspect, a computer-readable storage medium is provided, wherein a plurality of program codes are stored therein, the program codes being adapted to be loaded and run by a processor to perform the method described in any of the above-described point cloud localization methods.

[0016] In a third aspect, a smart device is provided, the smart device including at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores a computer program, which, when executed by the at least one processor, implements the method described in any of the above-described point cloud localization methods.

[0017] The above-described technical solutions of this application have at least one or more of the following beneficial effects: In the technical solution of the point cloud localization method provided in this application, the current frame point cloud can be voxelized to form multiple voxels. Based on the distribution characteristics of the point cloud within each voxel, the type of each voxel is determined. Voxels of each type are then filtered to balance the number of voxels of each type. The filtered and retained voxels are registered with a prior map to obtain the pose of the current frame point cloud. In the above implementation scheme, voxelizing the current frame point cloud achieves downsampling of the point cloud, resulting in a significantly lower number of voxels compared to the total number of points. Compared to directly registering the point cloud with a prior map, this implementation scheme can greatly reduce computational overhead. Furthermore, the above implementation scheme also divides voxels into multiple types and balances the number of voxels of each type through filtering. Compared to existing technologies that concentrate on registering ground point clouds, this implementation scheme can significantly improve registration accuracy, thereby accurately obtaining the pose of the current frame point cloud. Attached Figure Description

[0018] The disclosure of this application will become more readily understood with reference to the accompanying drawings. It will be readily understood by those skilled in the art that these drawings are for illustrative purposes only and are not intended to limit the scope of protection of this application. Wherein: Figure 1 This is a schematic flowchart of the main steps of a point cloud localization method according to an embodiment of this application; Figure 2 This is a schematic flowchart of the main steps of a method for obtaining various voxel types according to an embodiment of this application; Figure 3 This is a schematic flowchart of the main steps of a method for multi-level screening of various types of voxels according to an embodiment of this application; Figure 4 This is a schematic flowchart of the main steps of a method for multi-level screening of various types of voxels according to another embodiment of this application; Figure 5 This is a schematic flowchart of the main steps of a method for registering voxels with a priori maps according to an embodiment of this application; Figure 6 This is a schematic flowchart of the main steps of a method for fusing the pose of a point cloud in the current frame with poses obtained from other sensors, according to an embodiment of this application. Figure 7 This is a schematic flowchart of the main steps of a point cloud localization method according to another embodiment of this application; Figure 8 This is a schematic diagram of the main structure of a smart device according to an embodiment of this application.

[0019] List of reference numerals in the attached diagram: 11: Memory; 12: Processor. Detailed Implementation

[0020] Some embodiments of this application are described below with reference to the accompanying drawings. Those skilled in the art should understand that these embodiments are merely illustrative of the technical principles of this application and are not intended to limit the scope of protection of this application.

[0021] In the description of this application, "processor" can include hardware, software, or a combination of both. A processor can be a central processing unit, microprocessor, graphics processor, digital signal processor, or any other suitable processor. A processor has data and / or signal processing capabilities. A processor can be implemented in software, in hardware, or a combination of both. Computer-readable storage media includes any suitable medium capable of storing program code, such as magnetic disks, hard disks, optical disks, flash memory, read-only memory, random access memory, etc.

[0022] The relevant user personal information that may be involved in the various embodiments of this application is processed in strict accordance with the requirements of laws and regulations, following the principles of legality, legitimacy, and necessity, based on the reasonable purpose of the business scenario, and includes personal information that users actively provide or that is generated as a result of using the product / service, as well as personal information obtained with user authorization.

[0023] The personal information processed in this application will vary depending on the specific product / service scenario and will be based on the specific scenario in which the user uses the product / service. This may involve the user's account information, device information, driving information, vehicle information, or other related information. This application will treat the user's personal information and its processing with the utmost diligence.

[0024] This application attaches great importance to the security of users' personal information and has taken reasonable and feasible security protection measures that comply with industry standards to protect users' information and prevent unauthorized access, disclosure, use, modification, damage or loss of personal information.

[0025] The following describes an embodiment of the point cloud localization method provided in this application.

[0026] See appendix Figure 1 , Figure 1 This is a schematic flowchart illustrating the main steps of a point cloud localization method according to an embodiment of this application. Figure 1 As shown, the point cloud localization method in this application embodiment mainly includes the following steps S101 to S104.

[0027] Step S101: Perform voxelization on the current frame point cloud to form multiple voxels.

[0028] The point cloud in the current frame can be acquired by radar, including but not limited to lidar.

[0029] In this embodiment, conventional voxelization methods in the field of point cloud technology can be used to voxelize the point cloud of the current frame.

[0030] Step S102: Determine the type of each voxel based on the distribution characteristics of the point cloud within each voxel. Distribution characteristics refer to the feature information that can characterize the distribution of the point cloud within a voxel.

[0031] In some implementations, the type of voxel can include ground voxels, vertical plane voxels, and non-planar voxels. A ground voxel is a voxel in which the cluster of point clouds forming a point cloud is a plane, and this plane is the ground. A vertical plane voxel is a voxel in which the cluster of point clouds forming a point cloud is a plane, and this plane is perpendicular to the ground. A non-planar voxel is a voxel in which the cluster of point clouds forming a point cloud is not planar, meaning that this cluster of point clouds can represent an object.

[0032] Since the distribution characteristics characterize the distribution of point clouds within a voxel, it is possible to accurately analyze whether the point cloud cluster formed by all the point clouds within a voxel is the ground, a plane perpendicular to the ground, or an object.

[0033] Step S103: Screen each type of voxel to balance the number of voxels of each type. For each type of voxel, screening can remove a portion of voxels. After screening each type of voxel, the number of voxels retained in each type will not be significantly greater than that of other types, thus preventing an imbalance in the number of voxels retained in each type.

[0034] Step S104: Register the selected and retained voxels with the prior map to obtain the pose of the point cloud in the current frame.

[0035] The prior map is a pre-built map based on the point cloud of the current frame's location, acquired by radar. During step S104, it's not necessary to build the prior map; a pre-built one can be used. Furthermore, this embodiment does not specifically limit the method for building the prior map, as long as it can be obtained.

[0036] When registering the selected and retained voxels with the prior map, the same voxelization method as the current frame point cloud can be used to voxelize the prior map, resulting in multiple map voxels. For the pose of each retained voxel, the map voxel on the prior map that matches this retained voxel is obtained, and this retained voxel and this map voxel form a matching pair. In this way, a matching pair can be obtained for each retained voxel. If both the retained voxel and the map voxel are considered as point clouds, then the above matching pair is also a point cloud matching pair. Based on this, conventional point cloud registration methods in the field of point cloud technology can be used, with the pose of the current frame point cloud as the variable to be optimized, and the above matching pair can be used for registration to obtain the optimal pose of the current frame point cloud. This optimal pose is the pose to be obtained in this embodiment.

[0037] The following is a brief explanation of the method for obtaining map voxels that match the preserved voxels on the prior map. Specifically, point clouds can be collected using radar on intelligent devices (including but not limited to vehicles, robots, etc.). These intelligent devices also include positioning devices based on RTK (Real-Time Kinematic) technology (hereinafter referred to as RTK devices). First, the pose obtained by the RTK device at the current frame point cloud location can be acquired. This pose is used as the initial pose. Based on this initial pose, the pose of the current preserved voxel is transformed to the world coordinate system. The pose of the preserved voxel transformed to the world coordinate system is... , This represents the initial pose described above. This indicates the pose prior to voxel transformation. Find the corresponding position on the prior map. The nearest neighbor map voxel is used as the map voxel that matches the preserved voxel.

[0038] Based on the methods described in steps S101 to S104 above, voxelization of the current frame point cloud achieves downsampling of the point cloud, which can greatly reduce computational overhead. Furthermore, the above method also divides voxels into multiple types and balances the number of voxels of each type by filtering. Compared with existing technologies that focus on using ground point clouds for registration, the above method can significantly improve registration accuracy, thereby obtaining the accurate pose of the current frame point cloud.

[0039] In one application scenario according to an embodiment of this application, a vehicle is equipped with a LiDAR and a device capable of executing the method described in the embodiments of this application. During the process of controlling the vehicle to drive automatically, the vehicle collects point clouds around the vehicle using the LiDAR and inputs the point clouds into the aforementioned device. The device can accurately obtain the pose of each frame of the point cloud based on each frame, and then, through coordinate system transformation, obtain the vehicle's pose at each moment in the point cloud frame. Based on this, the vehicle's driving trajectory can be planned according to the vehicle's pose at each moment, and the vehicle can be controlled to drive automatically safely and reliably according to the driving trajectory.

[0040] The following provides further explanation of steps S102 to S104.

[0041] I. Explanation of step S102 above.

[0042] In some embodiments of step S102 above, the distribution characteristics of the point cloud can be the normal distribution characteristics of the point cloud, which can include the covariance matrix of the point cloud distribution. Furthermore, the type of each voxel can be determined in the following ways: Step 11: For each voxel, obtain the flatness and normal vector of the point cloud cluster formed by the point cloud within the voxel based on the covariance matrix of the point cloud distribution within the voxel.

[0043] In this embodiment, the covariance matrix can be decomposed into eigenvalues ​​to obtain three eigenvalues ​​and their corresponding eigenvectors. The eigenvector of the smallest eigenvalue can be used as the normal vector of the point cloud cluster. Simultaneously, the flatness of the point cloud cluster can be calculated using the following formula (1).

[0044] (1) The meanings of the parameters in formula (1) are as follows: Indicates flatness, Represents the smallest eigenvalue. This represents the second smallest eigenvalue. It represents the sum of three eigenvalues.

[0045] Step 12: Obtain the angle between the normal vector and the ground normal vector.

[0046] The ground normal vector is [0 0 1]. In this embodiment, the angle between the normal vector and the ground normal vector can be obtained using a conventional vector angle calculation method.

[0047] Step 13: When the flatness is greater than the first set threshold, if the included angle is greater than the second set threshold, then the voxel is a vertical plane voxel; otherwise, the voxel is a ground voxel; when the flatness is less than or equal to the first set threshold, the voxel is a non-planar voxel.

[0048] If the flatness is greater than a first set threshold, it indicates that the point cloud cluster is a plane. Furthermore, if the included angle is greater than a second set threshold, it indicates that the angle between the plane's normal vector and the ground normal vector is relatively large. This plane is unlikely to be parallel to the ground and is therefore not the ground, but most likely a vertical plane. Therefore, the voxel is treated as a vertical plane voxel. Conversely, if the included angle is less than or equal to the second set threshold, it indicates that the plane is basically parallel to the ground and is most likely the ground; therefore, the voxel is treated as a ground voxel.

[0049] If the flatness is less than or equal to a first set threshold, it indicates that the point cloud cluster is not a plane; therefore, the voxel is treated as a non-planar voxel. Furthermore, since the point cloud cluster is not a plane, it also indicates that it is highly likely to be an object; therefore, the voxel can also be treated as an object voxel.

[0050] It should be noted that those skilled in the art can flexibly set the values ​​of the first and second threshold values ​​according to actual needs, and this embodiment does not impose specific limitations on this.

[0051] Based on the methods described in steps 11 to 13 above, the covariance matrix of the point cloud distribution within a voxel can be used to accurately obtain the flatness and normal vector that can characterize the geometric shape information of the point cloud cluster. Then, based on the flatness and normal vector, the geometric shape of the point cloud cluster can be accurately analyzed to determine whether it is the ground, a vertical plane, or a non-plane, thereby quickly and accurately completing the classification of voxel types.

[0052] Furthermore, in some embodiments, based on the method described in steps 11 to 13 above, it can also be achieved by... Figure 2 The following steps S1021 to S1027, as shown, determine the type of each voxel.

[0053] Step S1021: For each voxel, obtain the flatness and normal vector of the point cloud cluster formed by the point clouds within the voxel, based on the covariance matrix of the point cloud distribution within the voxel. The method for obtaining the flatness and normal vector is described in the aforementioned method embodiment.

[0054] Step S1022: Obtain the angle between the normal vector and the ground normal vector.

[0055] Step S1023: Determine whether the flatness is greater than the first set threshold; if not, proceed to step S1024; if yes, proceed to step S1025.

[0056] Step S1024: The voxel is a non-planar voxel.

[0057] Step S1025: Determine whether the included angle is greater than the second set threshold; if yes, proceed to step S1026; if no, proceed to step S1027.

[0058] Step S1026: The voxel is a vertical plane voxel.

[0059] Step S1027: The voxel is a ground voxel.

[0060] Based on the methods described in steps S1021 to S1027 above, a planarity judgment is first performed to quickly determine whether the current voxel is a non-planar voxel. Once it is determined to be a non-planar voxel, subsequent steps are unnecessary, improving the efficiency of type determination. If the current voxel is not a non-planar voxel, it is further determined whether it is a ground voxel or a vertical planar voxel based on the normal vector, ensuring the accuracy of voxel classification.

[0061] II. Explanation of step S103 above.

[0062] In some embodiments of step S103 above, it can be achieved by... Figure 3 The following steps S1031 to S1033 are shown to perform multi-level screening of various types of voxels.

[0063] Step S1031: Screen the ground voxels and vertical plane voxels separately to remove inferior voxels from the ground voxels and vertical plane voxels respectively.

[0064] Inferior voxels are voxels of poor quality. In this embodiment, conditions for judging whether a ground voxel is an inferior voxel can be preset. If a ground voxel meets these conditions, it is an inferior voxel and needs to be removed. Similarly, conditions for judging whether a vertical plane voxel is an inferior voxel can be preset. If a vertical plane voxel meets these conditions, it is an inferior voxel and needs to be removed. Those skilled in the art can flexibly set the content of the above conditions according to actual needs, and this embodiment does not specifically limit them.

[0065] Step S1032: Based on the preset maximum number of non-ground voxels and the number of vertical plane voxels retained after removing inferior voxels, non-planar voxels are screened so that the sum of the number of non-planar voxels and the number of vertical plane voxels retained is less than or equal to the above maximum number.

[0066] Non-ground voxels refer to voxels of other types besides ground voxels. In this embodiment, non-ground voxels can include both vertical planar voxels and non-planar voxels.

[0067] Step S1033: After removing inferior voxels, the ground voxels are screened again so that the proportion of ground voxels in all retained voxels is less than or equal to a set ratio.

[0068] All retained voxels include vertical plane voxels after removing inferior voxels, non-planar voxels that have completed screening, and ground voxels that have undergone two screenings in steps S1031 and S1033. This step can effectively reduce the number of ground voxels when the number of ground voxels is relatively large.

[0069] Based on the method described in steps S1031 to S1033 above, a multi-level screening strategy can be used to ensure the quality of retained voxels and balance the number of voxels of different types.

[0070] Furthermore, after performing multi-level screening of voxels of various types using the methods described in the above embodiments, the number of voxels retained for each type may be relatively large, and these voxels are redundant for the same type. To reduce the computational cost of registration, in some embodiments, the retained voxels obtained from the multi-level screening can be screened again. Specifically, after executing step S1033, the following steps can be performed... Figure 4 The step S1034 shown.

[0071] Step S1034: The voxelized point cloud of the current frame is rasterized again to form multiple grids, each grid containing multiple voxels; based on the grids, voxels of each type are uniformly sampled. Specifically, for each grid, each type of voxel within that grid is sampled. The ratio of the number of samples for each type of voxel can be preset, and sampling is performed according to this ratio, thereby minimizing redundancy while maintaining a balanced number of voxels of each type. In this embodiment, a conventional rasterization method can be used to rasterize the point cloud of the current frame.

[0072] The following provides a further explanation of step S1031.

[0073] In some embodiments of step S1031 above, ground voxels and vertical plane voxels can be screened in the following ways to remove inferior voxels.

[0074] First, a first map voxel matching the ground voxel on the prior map is obtained, and a second map voxel matching the vertical plane voxel on the prior map is obtained. Both the first and second map voxels are formed by voxelizing the prior map. It should be noted that the methods for voxelizing the prior map and obtaining the first and second map voxels are similar to the methods in step S104 of the aforementioned embodiments, and will not be repeated here.

[0075] Then, it is determined whether the ground voxel and the first map voxel satisfy the preset constraint conditions between two ground surfaces; if not, the ground voxel is a poor-quality voxel and is removed; otherwise, the ground voxel is not a poor-quality voxel and is retained. Similarly, it can be determined whether the vertical plane voxel and the second map voxel satisfy the preset constraint conditions between two vertical planes; if not, the vertical plane voxel is a poor-quality voxel and is removed; otherwise, the vertical plane voxel is not a poor-quality voxel and is retained.

[0076] Based on the above implementation method, map voxels that match ground voxels can be used to quickly and accurately determine whether ground voxels are inferior voxels, and map voxels that match vertical plane voxels can be used to quickly and accurately determine whether vertical plane voxels are inferior voxels.

[0077] The following section further explains the methods for determining whether the ground voxel and the first map voxel satisfy the preset constraints between the two ground surfaces, and for determining whether the vertical plane voxel and the second map voxel satisfy the preset constraints between the two vertical planes.

[0078] (a) Method for determining whether the ground voxel and the first map voxel satisfy the preset constraint conditions between the two grounds In this embodiment, the following steps 21 to 25 can be used to determine whether the ground voxel and the first map voxel satisfy the preset constraint conditions between the two grounds.

[0079] Step 21: Based on the covariance matrix of the point cloud distribution within the ground voxels, obtain the first flatness and the first normal vector of the point cloud cluster formed by the point cloud within the ground voxels.

[0080] The method for obtaining the first flatness and the first normal vector is similar to the methods in steps 11 to 12 of the aforementioned method embodiment, and will not be repeated here.

[0081] Step 22: Obtain the second flatness of the point cloud clusters formed by the point clouds within the first map voxel. The second flatness can be pre-set when the prior map is built and can be directly called when needed. Alternatively, the second flatness can also be obtained by using the relevant methods in steps 11 to 12 of the aforementioned method embodiments when performing step 22.

[0082] Step 23: Determine whether the first flatness and the second flatness are both greater than the third set threshold, and whether the angle between the first normal vector and the ground normal vector is less than the fourth set threshold; If so, it indicates that the point cloud clusters formed by the ground voxels and the point clouds within the first map voxels both represent a plane with high flatness, and these two planes are basically parallel. Therefore, we can proceed to step 24 to determine that they satisfy the constraint conditions between the two ground surfaces. In some implementations, the third set threshold can be greater than the first set threshold in the aforementioned method embodiments.

[0083] If not, proceed to step 25.

[0084] Step 24: Determine whether the preset constraint conditions between the two ground surfaces are met.

[0085] Step 25: It is determined that the preset constraint conditions between the two ground surfaces are not met.

[0086] Based on the methods described in steps 21 to 25 above, the flatness and the angle between the normal vectors can be used to conveniently and accurately determine whether the ground voxel and the first map voxel meet the preset constraint conditions between the two grounds.

[0087] (ii) Method for determining whether the vertical plane voxel and the second map voxel satisfy the preset constraint conditions between the two vertical planes In this embodiment, steps 31 to 35 can be used to determine whether the vertical plane voxel and the second map voxel satisfy the preset constraint conditions between the two vertical planes.

[0088] Step 31: Based on the covariance matrix of the point cloud distribution within the vertical plane voxels, obtain the second flatness and the second normal vector of the point cloud cluster formed by the point clouds within the vertical plane voxels.

[0089] The method for obtaining the first flatness and the first normal vector is similar to the methods in steps 11 to 12 of the aforementioned method embodiment, and will not be repeated here.

[0090] Step 32: Obtain the third flatness and third normal vector of the point cloud cluster formed by the point cloud within the second map voxel. The third flatness and third normal vector can be pre-set when the prior map is built, and can be directly called when needed. Alternatively, the third flatness and third normal vector can also be obtained by using the relevant methods in steps 11 to 12 of the aforementioned method embodiment when performing step 32.

[0091] Step 33: Determine whether the second flatness and the third flatness are both greater than the fifth set threshold, and whether the angle between the second normal vector and the third normal vector is less than the sixth set threshold; If so, it indicates that the point cloud clusters formed by the point clouds within the vertical plane voxels and the second map voxels both represent a plane with high flatness, and these two planes are basically parallel. Therefore, we can proceed to step 34 to determine that they satisfy the constraint conditions between the two vertical planes. In some embodiments, the fifth set threshold can be greater than the first set threshold in the aforementioned method embodiments.

[0092] If not, proceed to step 35.

[0093] Step 34: Determine whether the preset constraint conditions between the two perpendicular planes are met.

[0094] Step 35: It is determined that the preset constraint conditions between the two perpendicular planes are not met.

[0095] Based on the methods described in steps 31 to 35 above, the flatness and the angle between the normal vectors can be used to conveniently and accurately determine whether the vertical plane voxel and the second map voxel meet the preset constraint conditions between the two vertical planes.

[0096] III. Explanation of step S104 above.

[0097] In some embodiments of step S104 above, it can be achieved by... Figure 5 The following steps S1041 to S1043 are used to register the selected and retained voxels with the prior map.

[0098] Step S1041: For each preserved voxel, obtain the map voxel on the prior map that matches the preserved voxel. The map voxel is formed by voxelizing the prior map.

[0099] Step S1042: For each preserved voxel, construct a residual term based on the distribution characteristics of the point cloud within the preserved voxel and the distance difference between the distribution characteristics of the point cloud within the map voxel.

[0100] In some implementations, the distribution feature can be a normal distribution feature of the point cloud, which includes the mean and covariance matrix of the point cloud. To achieve this, a residual term can be constructed by obtaining the distance difference between the mean of the point cloud within the preserved voxel and the map voxel, and another residual term can be constructed by obtaining the distance difference between the covariance matrix of the point cloud within the preserved voxel and the map voxel; that is, two residual terms are constructed for each preserved voxel.

[0101] Step S1043: Register based on the residual terms of each retained voxel.

[0102] In this embodiment, a conventional point cloud registration method in the field of point cloud technology can be adopted. The pose of the current frame point cloud is used as the variable to be optimized, and the residual terms of each preserved voxel are used for registration to obtain the optimal pose of the current frame point cloud. This optimal pose is the pose to be obtained in this embodiment.

[0103] Based on the method described in steps S1041 to S1043 above, the distribution characteristics of point clouds within voxels can be used to register voxels with prior maps, thereby accurately obtaining the optimal pose of the point cloud in the current frame.

[0104] In one embodiment of the point cloud localization method provided in this application, the current frame point cloud is a point cloud acquired by radar on a smart device, and the smart device may further include a wheel speedometer and an IMU. In this case, after obtaining the pose of the current frame point cloud through the foregoing method embodiment, it can also be obtained through... Figure 6 The following steps S105 to S106 are shown to obtain the final pose.

[0105] Step S105: Obtain the pose at the current frame point cloud location using the wheel velocity meter and IMU. The pose obtained using the wheel velocity meter and IMU is the pose of the smart device at the current frame point cloud location. This embodiment does not limit the method of obtaining the pose using the wheel velocity meter and IMU, as long as the pose obtained through these two methods can be obtained.

[0106] Step S106: Fuse the pose of the current frame point cloud with the pose obtained by the wheel velocity meter and IMU to obtain the final pose.

[0107] In this embodiment, conventional filtering methods, such as the Extended Kalman Filter (EKF) method, can be used to fuse the pose of the current frame point cloud with the pose obtained by the wheel velocity meter and IMU.

[0108] In the method described in steps S105 to S106 above, the point cloud is obtained from the radar on the smart device. The radar or point cloud coordinate system is a local coordinate system relative to the smart device. Therefore, the optimal pose of the current frame point cloud obtained through the aforementioned method embodiments is only locally optimal relative to the smart device as a whole, not globally optimal. To address this, the method described above fuses the pose of the current frame point cloud with the pose obtained from the wheel velocity sensor and the IMU to obtain the globally optimal pose, i.e., the final pose described above. Therefore, this method ensures more accurate poses and higher positioning precision.

[0109] The following is in conjunction with the appendix Figure 7 The process of point cloud localization, which combines the methods described in steps S101 to S104 of the foregoing embodiments with the methods described in steps S105 to S106 of the foregoing embodiments, will be briefly explained.

[0110] In this embodiment, the input data includes the pose obtained by the RTK device, the current frame point cloud collected by the radar, the prior map, and the pose at the current frame point cloud time obtained by the wheel speed meter and IMU on the smart device.

[0111] First, the pose obtained from the RTK device is used as the initial pose. The current frame point cloud is transformed from the radar coordinate system to the world coordinate system, and then voxelized to form multiple voxels. The voxels are categorized, and the type of each voxel is determined, including ground voxels, vertical plane voxels, and non-planar voxels. Next, the map voxels matching each voxel on the prior voxels are obtained, and each voxel is paired with its matching map voxel to form a matching pair. Then, the voxels of each type are filtered to obtain the selected and retained voxels. The selected and retained voxels are registered with the prior map to obtain the pose of the current frame point cloud. Finally, the pose of the current frame point cloud, the pose obtained from the RTK device at the current frame point cloud time, and the pose obtained from the wheel velocity meter and IMU at the current frame point cloud time are fused to obtain the final pose.

[0112] It should be noted that although the steps in the above embodiments are described in a specific order, those skilled in the art will understand that in order to achieve the effect of this application, different steps do not necessarily have to be executed in such an order. They can be executed simultaneously (in parallel) or in other orders. These adjusted solutions are equivalent to the technical solutions described in this application and therefore will also fall within the protection scope of this application.

[0113] Those skilled in the art will understand that all or part of the processes in the method of the above-described embodiment can also be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable file, or some intermediate form. The computer-readable storage medium can include any entity or device capable of carrying the computer program code, a medium, a USB flash drive, a portable hard drive, a magnetic disk, an optical disk, a computer memory, a read-only memory, a random access memory, an electrical carrier signal, a telecommunication signal, and a software distribution medium, etc. It should be noted that the content included in the computer-readable storage medium can be appropriately added or removed according to the requirements of legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, the computer-readable storage medium does not include electrical carrier signals and telecommunication signals.

[0114] Another aspect of this application provides a computer-readable storage medium.

[0115] In one embodiment of a computer-readable storage medium according to this application, the computer-readable storage medium can be configured to store a program that performs the point cloud localization method of the above-described method embodiments. This program can be loaded and run by a processor to implement the point cloud localization method. For ease of explanation, only the parts related to the embodiments of this application are shown; for specific technical details not disclosed, please refer to the method section of the embodiments of this application. The computer-readable storage medium can be a storage device comprising various electronic devices. Optionally, in the embodiments of this application, the computer-readable storage medium is a non-transitory computer-readable storage medium.

[0116] Another aspect of this application provides a smart device.

[0117] In one embodiment of a smart device according to this application, the smart device may include at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores a computer program, which, when executed by the at least one processor, implements the method described in any of the above embodiments. The smart device described in this application may include driving equipment, smart vehicles, robots, and other devices. See appendix. Figure 8 , Figure 8 The image exemplarily illustrates a communication connection between memory 11 and processor 12 via a bus.

[0118] In some embodiments of this application, the smart device may further include at least one sensor for sensing information. The sensor is communicatively connected to any type of processor mentioned in this application. Optionally, the smart device may further include an autonomous driving system for guiding the smart device to drive autonomously or assisting in driving. The processor communicates with the sensor and / or the autonomous driving system to perform the methods described in any of the above embodiments.

[0119] The technical solution of this application has been described above with reference to one embodiment shown in the accompanying drawings. However, it will be readily understood by those skilled in the art that the scope of protection of this application is obviously not limited to these specific embodiments. Without departing from the principles of this application, those skilled in the art can make equivalent changes or substitutions to the relevant technical features, and the technical solutions after these changes or substitutions will all fall within the scope of protection of this application.

Claims

1. A point cloud localization method, characterized in that, The method includes: The point cloud of the current frame is voxelized to form multiple voxels; Based on the distribution characteristics of point clouds within each voxel, the type of each voxel is determined. Each type of voxel is screened separately to balance the number of each type of voxel; The selected and retained voxels are registered with the prior map to obtain the pose of the point cloud in the current frame.

2. The method according to claim 1, characterized in that, The distribution characteristics of the point cloud include the covariance matrix of the point cloud distribution, and the determination of the type of each voxel includes: For each voxel, the flatness and normal vector of the point cloud cluster formed by the point cloud within the voxel are obtained based on the covariance matrix of the point cloud distribution within the voxel. Obtain the angle between the normal vector and the ground normal vector; If the flatness is greater than a first set threshold, and the included angle is greater than a second set threshold, then the voxel is a vertical plane voxel; otherwise, the voxel is a ground voxel. When the flatness is less than or equal to a first set threshold, the voxel is a non-planar voxel.

3. The method according to claim 1, characterized in that, The types include ground voxels, vertical plane voxels, and non-planar voxels. The screening of each type of voxel includes multi-level screening through the following methods: The ground voxels and the vertical plane voxels are screened separately to remove inferior voxels from the ground voxels and the vertical plane voxels respectively; The non-planar voxels are screened based on the preset maximum number of non-ground voxels and the number of vertical planar voxels retained after removing inferior voxels, so that the sum of the number of non-planar voxels and the number of vertical planar voxels retained is less than or equal to the maximum number. The ground voxels after removing inferior voxels are screened again so that the proportion of the ground voxels in all retained voxels is less than or equal to a set ratio.

4. The method according to claim 3, characterized in that, After further screening of the ground voxels after removing inferior voxels, the multi-stage screening also includes: The point cloud of the current frame after voxelization is rasterized again to form multiple grids, each grid comprising multiple voxels. Based on the grid, voxels retained by each type of screening are uniformly sampled.

5. The method according to claim 3 or 4, characterized in that, The method also includes screening ground voxels and vertical plane voxels to remove inferior voxels by means of: Obtain a first map voxel on the prior map that matches the ground voxel, and obtain a second map voxel on the prior map that matches the vertical plane voxel; Determine whether the ground voxel and the first map voxel satisfy the preset constraint conditions between the two grounds; if not, remove the ground voxel. Determine whether the vertical plane voxel and the second map voxel satisfy the preset constraint conditions between the two vertical planes; if not, remove the vertical plane voxel. The first map voxel and the second map voxel are formed by voxelizing the prior map.

6. The method according to claim 5, characterized in that, The distribution characteristics of the point cloud include the covariance matrix of the point cloud distribution. The step of determining whether the ground voxel and the first map voxel satisfy a preset constraint condition between two ground surfaces includes: Based on the covariance matrix of the point cloud distribution within the ground voxels, the first flatness and the first normal vector of the point cloud cluster formed by the point cloud within the ground voxels are obtained. Obtain the second planarity of the point cloud cluster formed by the point cloud within the first map voxel; If both the first flatness and the second flatness are greater than the third set threshold, and the angle between the first normal vector and the ground normal vector is less than the fourth set threshold, then it is determined that the constraint condition is met; otherwise, it is determined that the constraint condition is not met.

7. The method according to claim 5, characterized in that, The distribution characteristics of the point cloud include the covariance matrix of the point cloud distribution. The determination of whether the vertical plane voxel and the second map voxel satisfy the preset constraints between the two vertical planes includes: Based on the covariance matrix of the point cloud distribution within the vertical plane voxels, obtain the second flatness and the second normal vector of the point cloud cluster formed by the point clouds within the vertical plane voxels. Obtain the third planarity and third normal vector of the point cloud cluster formed by the point cloud within the second map voxel; If both the second and third flatness are greater than the fifth set threshold, and the angle between the second and third normal vectors is less than the sixth set threshold, then the constraint condition is satisfied; otherwise, the constraint condition is not satisfied.

8. The method according to claim 1, characterized in that, The registration of the selected and retained voxels with the prior map includes: For each preserved voxel, obtain the map voxel on the prior map that matches the preserved voxel, wherein the map voxel is formed by voxelizing the prior map; For each preserved voxel, a residual term is constructed based on the distribution characteristics of the point cloud within the preserved voxel and the distance difference between the distribution characteristics of the point cloud within the map voxel. Registration is performed based on the residual term of each retained voxel.

9. The method according to claim 1, characterized in that, The current frame point cloud is a point cloud acquired by radar on a smart device. The smart device also includes a wheel speedometer and an IMU. After registering the selected and retained voxels with the prior map, the method further includes: Obtain the pose of the point cloud at the current frame using the wheel velocity meter and IMU; The pose of the current frame point cloud and the pose obtained by the wheel velocity meter and IMU are fused to obtain the final pose.

10. A computer-readable storage medium storing a plurality of program codes, characterized in that, The program code is adapted to be loaded and run by a processor to perform the point cloud localization method according to any one of claims 1 to 9.

11. A smart device, characterized in that, include: At least one processor; And, a memory communicatively connected to the at least one processor; The memory stores a computer program, which, when executed by the at least one processor, implements the point cloud localization method according to any one of claims 1 to 9.

Images