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

Abstract

The present application relates to the technical field of autonomous driving, and in particular to a point cloud localization method, an intelligent device, and a computer readable storage medium, aiming at solving the problem of how to achieve accurate localization in a scenario where there is no GNSS localization system signal or the signal quality is poor. The method provided by the present application comprises: acquiring a local NDT map matching a global pose of an intelligent device; acquiring a de-distortion point cloud on the intelligent device and a first relative pose of the de-distortion point cloud which are obtained by a laser odometer; using a second relative pose of the de-distortion point cloud as a variable, using the first relative pose as an initial value of the variable, and performing point cloud registration on the de-distortion point cloud and the local NDT map to optimize the variable; and on the basis of an optimized second relative pose and the local NDT map, acquiring a point cloud pose of the de-distortion point cloud, wherein the first relative pose is the relative pose between the de-distortion point cloud and a neighboring point cloud, and the second relative pose is the relative pose between the de-distortion point cloud and the local NDT map. The method can improve the localization accuracy and reduce the computing power consumption.

Description

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

[0001] This application claims priority to Chinese patent application No. 202410143550.2, filed on February 1, 2024, entitled “Point cloud positioning method, intelligent device and computer-readable storage medium”. The entire contents of the above Chinese patent application are incorporated into this application by reference. Technical Field

[0002] The present application relates to the field of autonomous driving technology, and in particular to a point cloud positioning method, an intelligent device, and a computer-readable storage medium. Background Art

[0003] Autonomous driving requires accurate positioning of the vehicle to determine its position and orientation. This positioning allows the vehicle to plan a driving path and control the vehicle accordingly. In areas where there is no GNSS or other positioning system signal or signal quality is poor, such as parking lots, service areas, and complex intersections, an offline prior map can be loaded and used to locate the vehicle.

[0004] Currently, there are two main conventional methods for positioning using prior maps. One method is to perceive the vehicle's surrounding environment, establish a semantic vector map based on the perception results, and use this semantic vector map to locate the vehicle. This method has the advantages of small map size and low computing power consumption, but in scenarios without semantic labeling, the accuracy of the positioning results will be relatively low, that is, the generalization is poor. Another method is to use the SLAM (Simultaneous Localization and Mapping) method to obtain a feature map of a visual or laser point cloud, and use this feature map for vehicle positioning. This method has high generalization, but the map size is large and the computing power consumption is high.

[0005] Accordingly, this field requires a new technical solution to solve the above problems. Summary of the Invention

[0006] In order to overcome the above-mentioned defects, the present application is proposed to provide a point cloud positioning method, an intelligent device and a computer-readable storage medium that solve or at least partially solve the technical problem of how to accurately locate a vehicle in scenarios where there is no GNSS or other positioning system signal or the signal quality is poor.

[0007] In a first aspect, a point cloud positioning method is provided, the method comprising:

[0008] In response to the smart device entering the geo-fence, obtaining, based on the global pose of the smart device, a local NDT map that matches the global pose from a preset NDT map;

[0009] Obtain the dedistorted point cloud and its first relative pose obtained by the laser odometry on the smart device;

[0010] performing point cloud registration on the dedistorted point cloud and the local NDT map using the second relative pose of the dedistorted point cloud as a variable and the first relative pose as an initial value of the variable to optimize the variable and obtain an optimized second relative pose;

[0011] Acquire a point cloud pose of the dedistorted point cloud according to the optimized second relative pose and the local NDT map;

[0012] Among them, the global pose is the pose of the smart device in the global coordinate system; the dedistorted point cloud is the point cloud that has been dedistorted; the first relative pose is the relative pose between the dedistorted point cloud and an adjacent point cloud, and the adjacent point cloud is a frame of dedistorted point cloud that is adjacent to the dedistorted point cloud and located before the dedistorted point cloud; the second relative pose is the relative pose between the dedistorted point cloud and the local NDT map.

[0013] In a technical solution of the above point cloud positioning method, before obtaining the point cloud pose of the dedistorted point cloud based on the optimized second relative pose and the local NDT map, the method further includes:

[0014] If the point cloud registration fails, obtaining a point cloud pose of the dedistorted point cloud according to the first relative pose and the local NDT map.

[0015] In one technical solution of the above point cloud positioning method, the local NDT map is divided into a plurality of map voxels, and performing point cloud registration on the dedistorted point cloud and the local NDT map to optimize the variables includes:

[0016] Dividing the dedistorted point cloud into a plurality of point cloud voxels using a map voxel division method of the local NDT map;

[0017] According to the first relative pose, respectively obtaining, from the map voxels of the local NDT map, a map voxel that matches each of the point cloud voxels, and treating each of the point cloud voxels and its matching map voxel as a matching pair;

[0018] screening the matching pairs to obtain a plurality of target matching pairs;

[0019] Obtaining a priori height of the environment in which the smart device is currently located according to the local NDT map, and replacing the height in the initial value of the variable with the priori height;

[0020] performing point cloud registration on the dedistorted point cloud and the local NDT map according to the plurality of target matching pairs to optimize the variables;

[0021] The number of the multiple target matching pairs is a first set value, the point cloud voxels in the multiple target matching pairs are evenly distributed on the local NDT map, the matching score of each of the target matching pairs is greater than a second set value, and the matching score represents the degree of matching between the point cloud voxels in the target matching pairs and the map voxels.

[0022] In one technical solution of the above point cloud positioning method, obtaining the a priori height of the environment in which the smart device is currently located based on the local NDT map includes:

[0023] According to the global pose of the smart device, map voxels within a certain range centered on the smart device on the local NDT map are obtained;

[0024] The prior height is obtained according to the height of the map voxels within the certain range.

[0025] In a technical solution of the above point cloud positioning method, performing point cloud registration on the dedistorted point cloud and the local NDT map based on the multiple target matching pairs to optimize the variables includes:

[0026] projecting a point cloud voxel in each target matching pair onto the local NDT map according to the variable, and obtaining a first distribution feature of the point cloud within the point cloud voxel on the local NDT map;

[0027] Obtaining a second distribution feature of the map voxels in each target matching pair on the local NDT map;

[0028] Obtaining a distance between a first distribution feature of a point cloud voxel and a second distribution feature of a map voxel in each target matching pair, and constructing a residual term based on the distance;

[0029] Iteratively optimizing the variables with the goal of minimizing the sum of the residual terms of all target matching pairs;

[0030] The variable when the sum of the residual terms reaches the minimum is obtained as the optimized second relative pose.

[0031] In one technical solution of the above point cloud positioning method, the method further includes determining whether the point cloud registration fails by:

[0032] Obtaining a difference between the optimized second relative pose and the initial value of the variable;

[0033] Obtaining the number of valid matching pairs in the target matching pairs;

[0034] determining whether the point cloud registration fails according to the difference and the number of valid matching pairs;

[0035] The effective matching pair is a target matching pair that makes the sum of the residual terms converge.

[0036] In one technical solution of the above point cloud positioning method, determining whether the point cloud registration fails based on the difference and the number of valid matching pairs includes:

[0037] If the difference is within a first preset range and the number of valid matching pairs is within a second preset range, the point cloud registration is successful; otherwise, the point cloud registration fails.

[0038] In one technical solution of the above point cloud positioning method, determining whether the point cloud registration fails based on the difference and the number of valid matching pairs includes:

[0039] Obtaining the ratio of the valid matching pairs in the target matching pairs;

[0040] If the difference is within a first preset range, the number of valid matching pairs is within a second preset range, and the ratio is within a third preset range, the point cloud registration is successful; otherwise, the point cloud registration fails.

[0041] In one technical solution of the above point cloud positioning method, the smart device is provided with a plurality of different sensors, and the method further comprises obtaining the global pose of the smart device by:

[0042] Acquire motion information of the smart device obtained by each of the multiple different sensors, where the motion information includes a position and / or posture of the smart device;

[0043] A multi-sensor fusion positioning method is adopted to fuse the motion information of the smart device obtained by the sensors to obtain the global position of the smart device.

[0044] In one technical solution of the above-mentioned point cloud positioning method, the fusing and positioning of the motion information of the smart device obtained by each sensor includes:

[0045] Obtaining a point cloud pose of the dedistorted point cloud;

[0046] A multi-sensor fusion positioning method is adopted to fuse the motion information of the smart device obtained by each sensor and the point cloud pose of the dedistorted point cloud to perform positioning.

[0047] In a second aspect, a computer-readable storage medium is provided, wherein a plurality of program codes are stored in the computer-readable storage medium, wherein the program codes are suitable for being loaded and run by a processor to execute the method described in any one of the technical solutions of the above-mentioned point cloud positioning method.

[0048] In a third aspect, an intelligent device is provided, comprising at least one processor; and a memory communicatively connected to the at least one processor; wherein a computer program is stored in the memory, and when the computer program is executed by the at least one processor, the method described in any one of the technical solutions of the above-mentioned point cloud positioning method is implemented.

[0049] The above one or more technical solutions of this application have at least one or more of the following beneficial effects:

[0050] In a technical solution for implementing the point cloud positioning method provided in the present application, in response to a smart device entering a geo-fence, a local NDT map that matches the global pose can be obtained from a preset NDT map based on the global pose of the smart device; a dedistorted point cloud obtained by a laser odometry on the smart device and its first relative pose are obtained; point cloud registration is performed on the dedistorted point cloud and the local NDT map using the second relative pose of the dedistorted point cloud as a variable and the first relative pose as an initial value of the variable to optimize the variable and obtain an optimized second relative pose; and a point cloud pose of the dedistorted point cloud is obtained based on the optimized second relative pose and the local NDT map; wherein the global pose is the pose of the smart device in the global coordinate system; the dedistorted point cloud is the point cloud that has been dedistorted; the first relative pose is the relative pose between the dedistorted point cloud and an adjacent point cloud, where the adjacent point cloud is a frame of dedistorted point cloud adjacent to and preceding the dedistorted point cloud; and the second relative pose is the relative pose between the dedistorted point cloud and the local NDT map.

[0051] In the above implementation scheme, the laser odometry is capable of high-precision positioning. Using the first relative pose obtained by the laser odometry as the initial value of the variable in point cloud registration can significantly improve the accuracy of point cloud registration. Since the dedistorted point cloud is a point cloud that has undergone dedistortion processing and has higher accuracy, using the dedistorted point cloud for point cloud registration can further improve the accuracy of point cloud registration. In addition, using the dedistorted point cloud obtained by the laser odometry for point cloud registration realizes the reuse of the dedistorted point cloud obtained by the laser odometry, eliminating the need for dedistortion processing on the point cloud, saving the computing power occupied by the dedistortion processing.

[0052] Both the dedistorted point cloud and the local NDT map are three-dimensional. Point cloud registration of the dedistorted point cloud and the local NDT map is effectively registering the 3D point cloud with the 3D map. This allows the smart device to accurately determine the point cloud pose in any 3D scene, eliminating the need to differentiate between scenes. This solution accurately determines the point cloud pose in parking lots, service areas, complex intersections, and other scenarios, demonstrating high generalizability. Furthermore, NDT maps are small in size and do not occupy excessive storage space.

[0053] In one technical solution for implementing the point cloud positioning method provided herein, if point cloud registration fails, a point cloud pose of the dedistorted point cloud can be obtained based on the first relative pose and the local NDT map. In some cases, the local NDT map may be partially missing or distorted, which can cause point cloud registration failure. However, based on this solution, a relatively accurate point cloud pose can still be obtained even when point cloud registration fails, ensuring that the smart device can continue to perform point cloud positioning stably after entering the geofence.

[0054] In a technical solution for implementing the point cloud positioning method provided in the present application, when performing point cloud registration, a map voxel division method of a local NDT map can be used to divide the dedistorted point cloud into multiple point cloud voxels; according to the first relative pose, map voxels matching each point cloud voxel are respectively obtained from the map voxels of the local NDT map, and each point cloud voxel and its matching map voxel are respectively regarded as a matching pair; the matching pairs are screened to obtain multiple target matching pairs; according to the local NDT map, the prior height of the current environment of the smart device is obtained, and the height in the initial value of the variable is replaced by the prior height; according to the multiple target matching pairs, the dedistorted point cloud and the local NDT map are point cloud registered to optimize the variable; the number of the multiple target matching pairs is a set value, the point cloud voxels in the multiple target matching pairs are evenly distributed on the local NDT map, the matching score of each target matching pair is greater than the second set value, and the matching score represents the degree of matching between the point cloud voxel and the map voxel in the target matching pair.

[0055] In the above implementation scheme, screening the matching pairs can reduce the number of matching pairs, thereby saving the computing power occupied by point cloud registration. At the same time, screening the matching pairs can also ensure that the matching pairs participating in the point cloud registration have high quality, thereby ensuring the accuracy of the point cloud registration.

[0056] When smart devices are in underground parking lots, especially underground multi-story parking lots, high positioning stability is required. Replacing the height in the initial value of the variable with the above-mentioned prior height can ensure high positioning stability and improve the accuracy of point cloud registration. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] The disclosure of this application will become more easily understood with reference to the accompanying drawings. Those skilled in the art will readily appreciate that these drawings are for illustrative purposes only and are not intended to limit the scope of protection of this application. Among them:

[0058] FIG1 is a schematic flow chart of the main steps of a point cloud positioning method according to an embodiment of the present application;

[0059] FIG2 is a schematic flow chart of the main steps of performing point cloud registration on a dedistorted point cloud and a local NDT map according to one embodiment of the present application;

[0060] FIG3 is a flowchart illustrating the main steps of obtaining the global posture of a smart device according to an embodiment of the present application;

[0061] FIG4 is a flow chart showing the main steps of performing point cloud positioning, path planning, and navigation for a vehicle according to an embodiment of the present application;

[0062] FIG5 is a schematic diagram of the main structure of a smart device according to an embodiment of the present application.

[0063] List of reference numerals:

[0064] 11: Memory; 12: Processor. DETAILED DESCRIPTION

[0065] Some embodiments of the present application are described below with reference to the accompanying drawings. Those skilled in the art should understand that these embodiments are only used to explain the technical principles of the present application and are not intended to limit the scope of protection of the present application.

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

[0067] The relevant user personal information that may be involved in the various embodiments of this application is strictly in accordance with the requirements of laws and regulations, following the principles of legality, legitimacy and necessity, and based on the reasonable purposes of business scenarios, to process the personal information that users actively provide during the use of products / services or generated due to the use of products / services, as well as the personal information obtained with the user's authorization.

[0068] The user personal information processed by this application will vary depending on the specific product / service scenario and must 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 a high degree of diligence.

[0069] This application attaches great importance to the security of user personal information and has taken reasonable and feasible security protection measures that comply with industry standards to protect user information and prevent personal information from being accessed, disclosed, used, modified, damaged or lost without authorization.

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

[0071] Referring to FIG1 , FIG1 is a schematic flow chart of the main steps of a point cloud positioning method according to an embodiment of the present application. As shown in FIG1 , the point cloud positioning method in the embodiment of the present application mainly includes the following steps S101 to S104.

[0072] Step S101: In response to the smart device entering the geo-fence, a local Normal Distribution Transform (NDT) map matching the global posture of the smart device is obtained from a preset NDT map according to the global posture of the smart device.

[0073] Smart devices may include driving devices, smart cars, robots, and other devices. Optionally, the smart device may also include an autonomous driving system that guides the smart device to drive itself or assist in driving.

[0074] A geofence can be a polygonal area used to mark a geographic area in the real world. The vertices of the polygonal area correspond to the latitude and longitude coordinates in the real world. The geographic area marked by the geofence can be covered by the preset NDT map.

[0075] The preset NDT map is a map pre-established using conventional NDT technology. When point cloud positioning is required, this NDT map can be directly called. The embodiment of the present application does not specifically limit the method for establishing the NDT map.

[0076] The global position of the smart device is the position of the smart device in the global coordinate system. The global coordinate system is a coordinate system with the earth as a reference. In this embodiment, a conventional global coordinate system can be used, for example, the Northeast Celestial Coordinate System can be used.

[0077] The following is a brief description of the method for obtaining a local NDT map.

[0078] First, determine the pose of the smart device on the NDT map. If the map coordinate system of the preset NDT map is also a global coordinate system, then this global pose can also represent the pose of the smart device on the NDT map. If the map coordinate system of the preset NDT map is not a global coordinate system, the global pose can be converted to the NDT map based on the conversion relationship between the global coordinate system and the map coordinate system to obtain the pose of the smart device on the NDT map.

[0079] Then, a local NDT map is obtained based on the position and posture of the smart device on the preset NDT map. In some embodiments, the preset NDT map can be pre-divided into multiple map blocks, the map ranges of these map blocks are continuous, and each map block is a local NDT map. After obtaining the position and posture of the smart device on the NDT map, it is possible to obtain in which local NDT map this position and posture is located. This local NDT map is the local NDT map that matches the global position and posture. In some embodiments, based on the position and posture of the smart device on the preset NDT map, an area within a certain range centered on the smart device on the NDT map can be obtained as a local NDT map. Those skilled in the art can flexibly set the specific numerical value of the above-mentioned certain range according to actual needs, and this embodiment does not specifically limit this.

[0080] Step S102: Obtain a dedistorted point cloud and a first relative pose obtained by a laser odometry on the smart device.

[0081] The laser odometry is pre-installed on the smart device, which is also equipped with a laser radar. The laser radar can collect a three-dimensional point cloud around the smart device. The laser odometry can use the three-dimensional point cloud to obtain the motion state of the smart device, including but not limited to position, attitude, speed, etc. To ensure the accuracy of the motion state, the laser odometry first dedistorts the three-dimensional point cloud to obtain a dedistorted point cloud, and then obtains the motion state based on the dedistorted point cloud. In addition, during the process of obtaining the motion state, the relative pose between each two adjacent dedistorted point clouds is obtained, and the relative pose is then used to obtain the motion state. The relative pose is the change in pose between two adjacent dedistorted point clouds, which can represent the coordinate transformation relationship between the two adjacent dedistorted point clouds. The change in pose includes the change in position and the change in attitude. In some embodiments, a laser odometry obtained by the laser odometry implementation method disclosed in Chinese Patent Application No. 202211569521.X can be used.

[0082] In this embodiment, each frame of the dedistorted point cloud obtained by the laser odometry and the first relative pose of each frame of the dedistorted point cloud can be obtained. The first relative pose is the relative pose between the dedistorted point cloud and an adjacent point cloud, where the adjacent point cloud is a frame of dedistorted point cloud that is adjacent to the dedistorted point cloud and located before the dedistorted point cloud.

[0083] Step S103: Using the second relative pose of the dedistorted point cloud as a variable and the first relative pose as the initial value of the variable, perform point cloud registration on the dedistorted point cloud and the local NDT map to optimize the variable and obtain an optimized second relative pose.

[0084] The second relative pose is the relative pose between the dedistorted point cloud and the local NDT map. This relative pose is the change in pose between the dedistorted point cloud and the local NDT map. It can represent the coordinate transformation relationship between the dedistorted point cloud and the local NDT map. The change in pose includes the change in position and the change in attitude.

[0085] In this embodiment, a conventional point cloud registration method can be used to perform point cloud registration on the dedistorted point cloud and the local NDT map using the second relative pose of the dedistorted point cloud as a variable. This embodiment does not specifically limit the method of point cloud registration. As long as the dedistorted point cloud and the local NDT map can be registered and the variable optimization is completed, the optimized second relative pose can be obtained.

[0086] Since the first relative pose is a relatively accurate pose obtained by the laser odometry, using it as the initial value of the variable can significantly improve the accuracy of point cloud registration. In addition, directly reusing the dedistorted point cloud obtained by the laser odometry for point cloud registration eliminates the need for dedistortion processing, saving computing power required for dedistortion processing.

[0087] Step S104: Obtaining the point cloud pose of the dedistorted point cloud according to the optimized second relative pose and the local NDT map.

[0088] According to the aforementioned step S103, the second relative pose can actually represent the coordinate transformation relationship between the dedistorted point cloud and the local NDT map. Therefore, the dedistorted point cloud can be transformed or projected onto the local NDT map based on the optimized second relative pose to obtain the pose of the dedistorted point cloud on the local NDT map. This pose is the point cloud pose to be obtained in this step.

[0089] Based on the method described in steps S101 to S104 above, the laser odometry, which offers high-precision positioning advantages, can be utilized to improve the accuracy of point cloud registration and reduce computing power consumption, thereby improving the accuracy and efficiency of point cloud positioning. Furthermore, since the dedistorted point cloud and the local NDT map are both three-dimensional, the above method actually performs point cloud registration on the three-dimensional point cloud and the three-dimensional map. This allows the point cloud pose to be accurately determined in any three-dimensional scene, eliminating the need to distinguish between scenes. Whether it's a parking lot, a service area, or a complex intersection, the above method can accurately determine the point cloud pose, demonstrating high generalization.

[0090] The above steps S103 and S104 are further explained below.

[0091] 1. Explain step S103.

[0092] In some implementations of the above step S103 , the dedistorted point cloud and the local NDT map may be registered by performing the following steps S1031 to S1035 shown in FIG. 2 to optimize the variables, that is, to optimize the second relative pose.

[0093] Step S1031: using the map voxel division method of the local NDT map to divide the dedistorted point cloud into a plurality of point cloud voxels.

[0094] In this embodiment, the local NDT map is divided into multiple map voxels. The dedistorted point cloud is divided using the same map voxel division method as that used for the local NDT map. This ensures that the resolution of the map voxels and the point cloud voxels are the same, which improves the accuracy of obtaining matching pairs in subsequent steps. In this embodiment, conventional voxel division methods can be used to divide the local NDT map and the dedistorted point cloud into voxels, and this embodiment is not specifically limited to this method.

[0095] Step S1032: According to the first relative pose, map voxels matching each point cloud voxel are obtained from the map voxels of the local NDT map, and each point cloud voxel and its matching map voxel are respectively regarded as a matching pair.

[0096] In this step, the first relative pose is used as the initial value of the variable, which is also the initial value of the second relative pose. The second relative pose is the relative pose between the dedistorted cloud and the local NDT map. Therefore, the point cloud voxel can be converted or projected onto the local NDT map according to the first relative pose to obtain the pose of the point cloud voxel on the local NDT map. Then, a map voxel with the nearest neighbor is obtained. This map voxel is the map voxel that matches the point cloud voxel. The pose of the point cloud voxel on the local NDT map can be expressed as P w =T * ×Pc , T * Represents the initial value of the second relative pose, that is, the first relative pose, P c is the pose of the point cloud voxel in the dedistorted point cloud, that is, the pose before conversion or projection to the local NDT map.

[0097] After obtaining the map voxels that match the point cloud voxels, the matching score between the point cloud voxels and the map voxels can be obtained based on their poses. The size of the matching score can indicate the degree of matching between the two. For example, the larger the matching score, the higher the degree of matching between the two, and the better the quality of the matching pair formed by the two; the smaller the matching score, the lower the degree of matching between the two, and the worse the quality of the matching pair formed by the two. In this embodiment, a conventional matching score acquisition method can be used to obtain the matching score between the point cloud voxels and the map voxels in each matching pair. For example, in some embodiments, the Euclidean distance between the point cloud voxels and the map voxels can be obtained based on their poses, and the inverse of the Euclidean distance can be used as the matching score. This embodiment does not specifically limit the method for obtaining the matching score, as long as the matching score between the point cloud voxels and the map voxels in each matching pair can be obtained.

[0098] Step S1033: Screen the matching pairs to obtain multiple target matching pairs.

[0099] The number of the plurality of target matching pairs is a first set value, the point cloud voxels in the plurality of target matching pairs are evenly distributed on the local NDT map, and the matching score of each target matching pair is greater than a second set value.

[0100] Since a single point cloud voxel can form a matching pair, the number of matching pairs can be very large. Excessive matching pairs can significantly reduce the efficiency of point cloud registration. To address this, this embodiment pre-sets a first set value, such that the number of matching pairs retained after screening is equal to this first set value, thereby reducing the number of matching pairs and improving the efficiency of point cloud registration.

[0101] If matching pairs are randomly screened, high-quality matching pairs may be removed, leaving many matching pairs of poor quality. When a large number of matching pairs of poor quality are used for point cloud registration, the accuracy of point cloud registration will be reduced. As can be seen from the description of step S1032 above, the matching score of the matching pair can represent the quality of the matching pair. Therefore, in this embodiment, the matching score of the matching pair can be used for screening to select high-quality matching pairs. If the matching score of the matching pair is greater than the second set value, it indicates that the matching pair is a high-quality matching pair; otherwise, the matching pair is not a high-quality matching pair.

[0102] In addition, if the matching pairs are randomly screened, the matching pairs retained by the screening may also result in an uneven distribution, that is, the point cloud voxels in these matching pairs are unevenly distributed on the local NDT map. When these matching pairs are unevenly distributed, the accuracy of the point cloud registration will also be reduced. Therefore, in this embodiment, the matching pairs can be screened according to the principle of uniform distribution of matching pairs. It should be noted that this embodiment does not specifically limit the method of uniformly screening matching pairs, as long as it is ensured that the matching pairs retained by the screening can be evenly distributed. In addition, those skilled in the art can also flexibly set the values ​​of the above-mentioned first set value and the second set value according to actual needs, and this embodiment also does not make specific limitations.

[0103] Step S1034: According to the local NDT map, the prior height of the environment in which the smart device is currently located is obtained, and the height in the initial value of the variable is replaced by the prior height.

[0104] When the smart device is in an underground parking lot, especially an underground multi-story parking lot, the requirements for height positioning stability are relatively high. However, if there is no GNSS or other positioning system signal or the signal quality is poor in the underground parking lot, it will be impossible to obtain a relatively accurate height. Even if the first relative posture obtained by the laser odometer with the advantage of high-precision positioning is used as the initial value of the variable, there may still be deviations in the height of this initial value. To this end, in this embodiment, the height of the local NDT map can be used to obtain the height of the environment where the smart device is currently located. Since the local NDT map is a relatively accurate map that has been established in advance, the height of the local NDT map can be used to accurately obtain the height of the environment where the smart device is currently located, and this height can be used as the prior height. Then, the height in the initial value of the variable is replaced with the prior height to improve the accuracy of this initial value, which is beneficial to improve the accuracy of point cloud alignment.

[0105] Step S1035: performing point cloud registration on the dedistorted point cloud and the local NDT map based on the multiple target matching pairs to optimize the variables.

[0106] In this embodiment, a conventional point cloud registration method can be used to perform point cloud registration on the dedistorted point cloud and the local NDT map based on multiple target matching pairs. This embodiment does not specifically limit the method of point cloud registration. As long as the point clouds of the dedistorted point cloud and the local NDT map can be registered and variable optimization can be completed to obtain the optimized second relative pose, it will be sufficient.

[0107] Based on the method described in steps S1031 to S1035 above, the accuracy of point cloud registration can be significantly improved and the computing power occupied by point cloud registration can be reduced.

[0108] The above steps S1034 and S1035 are further explained below.

[0109] (1) Step S1034 is described.

[0110] In some implementations of the above step S1034, the prior height of the current environment of the smart device can be obtained according to the local NDT map by following steps 11 and 12.

[0111] Step 11: Based on the global pose of the smart device, obtain the map voxels within a certain range centered on the smart device on the local NDT map.

[0112] As described in the aforementioned step S101, the pose of the smart device on the local NDT map can be obtained based on the global pose of the smart device. The point at this pose can be used as the center to obtain map voxels within a certain range of this center.

[0113] Those skilled in the art can flexibly set the specific numerical value of the first range according to actual needs, and this embodiment does not specifically limit this.

[0114] Step 12: Obtain a priori height based on the heights of the map voxels within the certain range. For example, the average height of all map voxels within the certain range can be obtained and used as the priori height.

[0115] Based on the method described in steps 11 to 12 above, the local NDT map can be used to conveniently and accurately obtain the prior height of the environment in which the smart device is currently located.

[0116] (2) Explain step S1035.

[0117] In some implementations of the above step S1035, the following steps 21 to 25 may be performed to perform point cloud registration on the dedistorted point cloud and the local NDT map based on multiple target matching pairs to optimize variables and obtain an optimized second relative pose.

[0118] Step 21: Project the point cloud voxels in each target matching pair onto the local NDT map based on the variables, and obtain a first distribution feature of the point cloud within the point cloud voxel on the local NDT map. The first distribution feature can be a normal distribution feature of the point cloud, which includes the mean and covariance matrix of the point cloud.

[0119] Step 22: Obtain a second distribution feature of the map voxels in each target matching pair on the local NDT map. The second distribution feature may be a normal distribution feature of the point cloud in the map voxels, the normal distribution feature including the mean and covariance matrix of the point cloud.

[0120] Step 23: Obtain the distance between the first distribution feature of the point cloud voxel and the second distribution feature of the map voxel in each target matching pair, and construct a residual term based on the distance.

[0121] When both the first and second distribution features include means and covariance matrices, two residual terms need to be constructed, one is a residual term constructed based on the distance between the means, and the other is a residual term constructed based on the distance between the covariance matrices.

[0122] Step 24: Iteratively optimize the variables with the goal of minimizing the sum of the residual terms of all target matching pairs.

[0123] Step 25: Obtain the variable where the sum of the residual terms reaches the minimum as the optimized second relative pose. After obtaining the optimized second relative pose, step S104 can be executed to obtain the point cloud pose of the dedistorted point cloud based on the optimized second relative pose and the local NDT map.

[0124] Based on the method described in steps 21 to 25 above, the distribution characteristics of the point cloud voxels and the point cloud within the map voxels can be used to accurately complete the point cloud registration of the dedistorted point cloud and the local NDT map to obtain the optimal second relative pose.

[0125] 2. Explain step S104.

[0126] When performing point cloud registration on the dedistorted point cloud and the local NDT map in step S103, registration failure may occur. At this time, the optimized second relative pose obtained by point cloud registration has low accuracy. If the above-mentioned optimized second relative pose is still used in step S104 to obtain the point cloud pose of the dedistorted point cloud, the accuracy of the point cloud pose will be reduced.

[0127] In some embodiments of step S104, if point cloud registration is determined to be successful, the point cloud pose of the dedistorted point cloud is obtained based on the optimized second relative pose and the local NDT map. If point cloud registration is determined to be unsuccessful, the point cloud pose of the dedistorted point cloud is obtained based on the first relative pose and the local NDT map. Specifically, the dedistorted point cloud can be converted or projected onto the local NDT map based on the first relative pose to obtain the pose of the dedistorted point cloud on the local NDT map. This pose is the point cloud pose to be obtained.

[0128] The following describes a method for determining whether point cloud registration has failed.

[0129] In some implementations of the above step S104 , whether the point cloud registration fails can be determined by following steps S1041 to S1043 .

[0130] Step S1041: Obtain the difference between the optimized second relative pose and the initial value of the variable, wherein the initial value of the variable is also the initial value of the second relative pose.

[0131] Step S1042: Obtain the number of valid matching pairs in the target matching pairs.

[0132] The effective matching pair is a target matching pair that makes the sum of the residual terms converge. The sum of the residual terms is the sum of the residual terms of all target matching pairs in the method described in steps 21 to 25.

[0133] The convergence of the sum of the residual terms means that the sum of the residual terms approaches the minimum. According to the method described in the aforementioned steps 21 to 25, it can be seen that the embodiment of the present application is to iteratively optimize the variables with the goal of minimizing the sum of the residual terms of all target matching pairs. Therefore, if a matching pair can make the sum of the residual terms converge, it indicates that the matching pair is valid, otherwise the matching pair is invalid. In this embodiment, a conventional effective evaluation method can be used to evaluate whether each matching pair is valid. This embodiment does not specifically limit the effective evaluation method, as long as it can evaluate whether the matching pair is valid.

[0134] Step S1043: Determine whether the point cloud registration fails based on the difference value and the number of valid matching pairs.

[0135] According to the description of the aforementioned method embodiment, the accuracy of the initial value of the second relative pose (i.e., the initial value of the variable) is already relatively high. If the second relative pose optimized after point cloud registration differs significantly from this initial value, it indicates that the point cloud registration may have failed. If the number of valid matching pairs is relatively small, it also indicates that the point cloud registration may have failed. Therefore, in this embodiment, it is possible to determine whether the point cloud registration has failed based on the above-mentioned difference and the number of valid matching pairs. For example, if the difference is relatively large and the number of valid matching pairs is relatively small, the point cloud registration has failed.

[0136] In some implementations, whether point cloud registration fails can be determined based on the difference value and the number of valid matching pairs in the following manner.

[0137] If the difference is within the first preset range and the number of valid matching pairs is within the second preset range, the point cloud registration is successful; otherwise, the point cloud registration fails.

[0138] If the difference is within the first preset range, it indicates that the difference is relatively small; if the number of valid matching pairs is within the second preset range, it indicates that the number of valid matching pairs is relatively large. Those skilled in the art can flexibly set the values ​​of the first and second preset ranges according to actual needs, and this embodiment does not specifically limit this.

[0139] Through the above implementation, it is possible to accurately determine whether the point cloud registration fails based on the two dimensions of difference and the number of valid matching pairs.

[0140] In some implementations, whether point cloud registration fails can be determined based on the difference value and the number of valid matching pairs in the following manner.

[0141] Obtain the ratio of valid matching pairs to target matching pairs. If the difference is within a first preset range, the number of valid matching pairs is within a second preset range, and the ratio is within a third preset range, then the point cloud registration succeeds; otherwise, the point cloud registration fails.

[0142] The first and second preset ranges are the same as those in the aforementioned method. If the ratio of valid matching pairs is within the third preset range, it indicates that the ratio of valid matching pairs is relatively high. Those skilled in the art can flexibly set the value of the third preset range according to actual needs, and this embodiment does not specifically limit this.

[0143] Through the above implementation, it is possible to accurately determine whether point cloud registration fails based on the three dimensions of difference, number and ratio of valid matching pairs.

[0144] The following is a brief description of how to obtain the global pose of a smart device.

[0145] In some embodiments of the present application, a plurality of different sensors are provided on the smart device, and the plurality of sensors may include a lidar, a wheel speed meter, an IMU, a camera, etc., and the global posture of the smart device can be obtained through the following steps S201 to S202 shown in FIG3 .

[0146] Step S201: acquiring motion information of the smart device obtained by each of a plurality of different sensors, wherein the motion information includes the position and / or posture of the smart device, and may also include the speed, acceleration, etc. of the smart device.

[0147] In this embodiment, a conventional motion information acquisition method can be used to obtain the motion information of the smart device based on the data obtained by each sensor. This embodiment does not specifically limit how each sensor obtains the motion information of the smart device, as long as the motion information of the smart device obtained by each sensor can be obtained.

[0148] Step S202: adopting a multi-sensor fusion positioning method to fuse the motion information of the smart device obtained by each sensor to obtain the global position and posture of the smart device.

[0149] In this embodiment, a conventional multi-sensor fusion positioning method can be used to fuse and locate the above-mentioned motion information to obtain the global posture of the smart device. This embodiment does not specifically limit the multi-sensor fusion positioning method. As long as the motion information of the smart device obtained by each sensor can be fused and located to obtain the global posture of the smart device, it will be sufficient.

[0150] Based on the method described in steps S201 to S202 above, even in a scenario where there is no GNSS or other positioning system signal or the signal quality is poor, the global pose of the smart device can be accurately obtained, thereby ensuring the accuracy of point cloud positioning.

[0151] According to the aforementioned method embodiment, after the smart device enters the geo-fence, the dedistorted point cloud and the local NDT map can be aligned, and then the point cloud pose of the dedistorted point cloud can be obtained based on the second relative pose obtained by the point cloud alignment and the local NDT map.

[0152] In some implementations of step S202 above, the point cloud pose can also be used as motion information of the smart device obtained by a virtual sensor, and the point cloud pose can be fused with motion information obtained by other sensors for positioning, thereby improving the accuracy of the global pose. Specifically, when using a multi-sensor fusion positioning method to fuse the motion information of the smart device obtained by each sensor for positioning, the point cloud pose of the dedistorted point cloud can be obtained, and then the multi-sensor fusion positioning method can be used to fuse the motion information of the smart device obtained by each sensor and the point cloud pose of the dedistorted point cloud for positioning to obtain the global pose of the smart device.

[0153] Through this implementation, after the smart device enters the geo-fence, the accuracy of the global posture of the smart device in the geo-fence can be improved, further improving the accuracy of point cloud positioning.

[0154] The following, in conjunction with Figure 4, briefly describes a point cloud positioning method that employs the methods described in the aforementioned embodiments in some embodiments. In this embodiment, the smart device is a vehicle, and the input data includes a dedistorted point cloud obtained by a laser odometry, a first phase pose, an NDT map, and the vehicle's global pose. The output data includes the point cloud pose of the dedistorted point cloud.

[0155] As shown in Figure 4, the global position of the vehicle is obtained during the vehicle's driving process, and whether the vehicle enters the geofence is determined based on the global position; if not, the vehicle continues to determine whether it enters the geofence based on the obtained global position; if it enters, the following steps 1 to 13 are executed.

[0156] Step 1: Obtain the dedistorted point cloud and the first relative pose obtained by the laser odometry, and synchronize the data of the first relative pose and the global pose of the vehicle.

[0157] Step 2: Obtain the global NDT map corresponding to the current geofence from the global NDT map buffer, and obtain the local NDT map that matches the global pose from the global NDT map based on the vehicle's global pose.

[0158] Step 3: Obtain multiple matching pairs based on the dedistorted point cloud and the local NDT map.

[0159] Step 4: Filter the matching pairs to obtain multiple target matching pairs.

[0160] Step 5: Based on the local NDT map, obtain the prior height of the vehicle's current environment and replace the height in the initial value of the variable in the point cloud registration with this prior height.

[0161] Step 6: Perform point cloud registration based on the target matching pair and the initial value of the variable replaced with the prior height to optimize the variable and obtain the optimized second relative pose.

[0162] Step 7: Evaluate the effectiveness of point cloud registration, that is, evaluate whether the matching pairs are valid and obtain valid matching pairs.

[0163] Step 8: Determine whether the point cloud registration is successful.

[0164] If successful, the point cloud pose of the dedistorted point cloud is obtained according to the optimized second relative pose and the local NDT map; if unsuccessful, the point cloud pose of the dedistorted point cloud is obtained according to the first relative pose and the local NDT map.

[0165] Step 9: Generate a point cloud positioning result message based on the point cloud pose of the dedistorted point cloud. The result message includes the point cloud pose of the dedistorted point cloud.

[0166] Step 10: Use a multi-sensor fusion positioning method to fuse the vehicle motion information obtained by each sensor on the vehicle and the point cloud pose of the dedistorted point cloud to obtain the global pose of the vehicle.

[0167] Step 11: Based on the global position of the vehicle, determine the road topology where the vehicle is located, that is, perform topological positioning of the vehicle.

[0168] Step 12: Plan the vehicle's driving path based on the road topology at the vehicle's location, that is, perform path planning for the vehicle.

[0169] Step 13: The vehicle cockpit controller controls the vehicle to drive automatically according to the planned path, that is, performs cockpit navigation on the vehicle.

[0170] Based on the methods described in steps 1 through 13 above, accurate point cloud positioning can be performed, allowing for safe and reliable autonomous vehicle control, even in scenarios where there is no GNSS or other positioning system signal or the signal quality is poor. It should be noted that the specific methods of steps 1 through 13 above can be found in the relevant methods of the aforementioned method embodiments and will not be further described here.

[0171] It should be pointed out that although the various 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 the present application, different steps do not have to be performed in such an order. They can be performed 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 scope of protection of this application.

[0172] It will be understood by those skilled in the art that all or part of the processes in the method for implementing the above embodiment of the present application can also be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a computer-readable storage medium. When the computer program is executed by a processor, it can implement the steps of each of the above method embodiments. The computer program includes computer program code, which can be in source code form, object code form, executable file or some intermediate form. The computer-readable storage medium can include: any entity or device, medium, USB flash drive, mobile hard disk, magnetic disk, optical disk, computer memory, read-only memory, random access memory, electric carrier signal, telecommunication signal and software distribution medium that can carry the computer program code. It should be noted that the content contained in the computer-readable storage medium can be appropriately increased or decreased according to the requirements of legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, computer-readable storage media do not include electric carrier signals and telecommunication signals.

[0173] Another aspect of the present application provides a computer-readable storage medium.

[0174] In an embodiment of a computer-readable storage medium according to the present application, the computer-readable storage medium can be configured to store a program for executing the point cloud positioning method of the above-mentioned method embodiment, and the program can be loaded and executed by the processor to implement the above-mentioned point cloud positioning method. For ease of explanation, only the parts related to the embodiment of the present application are shown. For specific technical details not disclosed, please refer to the method section of the embodiment of the present application. The computer-readable storage medium can be a storage device formed by various electronic devices. Optionally, the computer-readable storage medium in the embodiment of the present application is a non-transitory computer-readable storage medium.

[0175] Another aspect of the present application provides a smart device.

[0176] In an embodiment of an intelligent device according to the present application, the intelligent 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, and when the computer program is executed by the at least one processor, it implements the method described in any of the above embodiments. The intelligent device described in this application may include a driving device, a smart car, a robot, and other devices. Referring to Figure 5, Figure 5 exemplarily illustrates a memory 11 and a processor 12 communicatively connected via a bus.

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

[0178] Thus far, the technical solution of the present application has been described in conjunction with an embodiment shown in the accompanying drawings. However, it is readily understood by those skilled in the art that the scope of protection of the present application is obviously not limited to these specific embodiments. Without departing from the principles of the present application, those skilled in the art may make equivalent changes or substitutions to the relevant technical features, and the technical solutions after such changes or substitutions will fall within the scope of protection of the present application.

Claims

1. A point cloud positioning method, characterized in that: The method comprises: In response to the smart device entering the geo-fence, obtaining, based on the global pose of the smart device, a local NDT map that matches the global pose from a preset NDT map; Obtain the dedistorted point cloud and its first relative pose obtained by the laser odometry on the smart device; performing point cloud registration on the dedistorted point cloud and the local NDT map using the second relative pose of the dedistorted point cloud as a variable and the first relative pose as an initial value of the variable to optimize the variable and obtain an optimized second relative pose; Acquire a point cloud pose of the dedistorted point cloud according to the optimized second relative pose and the local NDT map; Among them, the global pose is the pose of the smart device in the global coordinate system; the dedistorted point cloud is the point cloud that has been dedistorted; the first relative pose is the relative pose between the dedistorted point cloud and an adjacent point cloud, and the adjacent point cloud is a frame of dedistorted point cloud that is adjacent to the dedistorted point cloud and located before the dedistorted point cloud; the second relative pose is the relative pose between the dedistorted point cloud and the local NDT map.

2. The method according to claim 1, characterized in that Before acquiring the point cloud pose of the dedistorted point cloud according to the optimized second relative pose and the local NDT map, the method further includes: If the point cloud registration fails, obtaining a point cloud pose of the dedistorted point cloud according to the first relative pose and the local NDT map.

3. The method according to claim 1, characterized in that The local NDT map is divided into a plurality of map voxels, and performing point cloud registration on the dedistorted point cloud and the local NDT map to optimize the variable includes: Dividing the dedistorted point cloud into a plurality of point cloud voxels using a map voxel division method of the local NDT map; According to the first relative pose, respectively obtaining, from the map voxels of the local NDT map, a map voxel that matches each of the point cloud voxels, and treating each of the point cloud voxels and its matching map voxel as a matching pair; screening the matching pairs to obtain a plurality of target matching pairs; Obtaining a priori height of the environment in which the smart device is currently located according to the local NDT map, and replacing the height in the initial value of the variable with the priori height; performing point cloud registration on the dedistorted point cloud and the local NDT map according to the plurality of target matching pairs to optimize the variables; The number of the multiple target matching pairs is a first set value, the point cloud voxels in the multiple target matching pairs are evenly distributed on the local NDT map, the matching score of each of the target matching pairs is greater than a second set value, and the matching score represents the degree of matching between the point cloud voxels in the target matching pairs and the map voxels.

4. The method according to claim 3, characterized in that The obtaining, according to the local NDT map, a priori height of the environment in which the smart device is currently located, includes: According to the global pose of the smart device, map voxels within a certain range centered on the smart device on the local NDT map are obtained; The prior height is obtained according to the height of the map voxels within the certain range.

5. The method according to claim 3, characterized in that The performing point cloud registration on the dedistorted point cloud and the local NDT map according to the multiple target matching pairs to optimize the variables includes: projecting a point cloud voxel in each target matching pair onto the local NDT map according to the variable, and obtaining a first distribution feature of the point cloud within the point cloud voxel on the local NDT map; Obtaining a second distribution feature of the map voxels in each target matching pair on the local NDT map; Obtaining a distance between a first distribution feature of a point cloud voxel and a second distribution feature of a map voxel in each target matching pair, and constructing a residual term based on the distance; Iteratively optimizing the variables with the goal of minimizing the sum of the residual terms of all target matching pairs; The variable when the sum of the residual terms reaches the minimum is obtained as the optimized second relative pose.

6. The method according to claim 5, characterized in that The method further includes determining whether the point cloud registration fails by: Obtaining a difference between the optimized second relative pose and the initial value of the variable; Obtaining the number of valid matching pairs in the target matching pairs; determining whether the point cloud registration fails according to the difference and the number of valid matching pairs; The effective matching pair is a target matching pair that makes the sum of the residual terms converge.

7. The method according to claim 6, characterized in that The determining whether the point cloud registration fails according to the difference and the number of valid matching pairs includes: If the difference is within a first preset range and the number of valid matching pairs is within a second preset range, the point cloud registration is successful; otherwise, the point cloud registration fails.

8. The method according to claim 6, characterized in that The determining whether the point cloud registration fails according to the difference and the number of valid matching pairs includes: Obtaining the ratio of the valid matching pairs in the target matching pairs; If the difference is within a first preset range, the number of valid matching pairs is within a second preset range, and the ratio is within a third preset range, the point cloud registration is successful; otherwise, the point cloud registration fails.

9. The method according to claim 1, characterized in that The smart device is provided with a plurality of different sensors, and the method further comprises obtaining the global pose of the smart device by: Acquire motion information of the smart device obtained by each of the multiple different sensors, where the motion information includes a position and / or posture of the smart device; A multi-sensor fusion positioning method is adopted to fuse the motion information of the smart device obtained by the sensors to obtain the global position of the smart device.

10. The method according to claim 9, characterized in that The fusing and positioning the motion information of the smart device obtained by the sensors includes: Obtaining a point cloud pose of the dedistorted point cloud; A multi-sensor fusion positioning method is adopted to fuse the motion information of the smart device obtained by each sensor and the point cloud pose of the dedistorted point cloud to perform positioning.

11. A computer-readable storage medium storing a plurality of program codes, characterized in that: The program code is suitable for being loaded and run by a processor to execute the point cloud positioning method according to any one of claims 1 to 10.

12. A smart device, characterized in that: include: at least one processor; and, a memory communicatively coupled to the at least one processor; Wherein, a computer program is stored in the memory, and when the computer program is executed by the at least one processor, the point cloud positioning method according to any one of claims 1 to 10 is implemented.

Images