A pose initialization method

Abstract

The present disclosure provides a pose initialization method, which generates an octree of a candidate pose set by using a scan frame point cloud and a local point cloud map, and then determines a candidate pose of a current best node by using a preset convergence algorithm. The current pose can be obtained by point cloud data without relying on a high-precision map, which greatly reduces the calculation amount. It is not dependent on special scene constraints and is suitable for various scenes. The positioning device can realize positioning in a weak GNSS signal environment. The rotation histogram matching is used to screen error rotation candidate results, and the preset convergence algorithm is used to search and prune the nodes of the octree, which avoids the brute force search in the prior art, reduces the algorithm complexity, reduces the calculation amount, and improves the data processing efficiency.

Description

Technical Field

[0001] The present disclosure relates to the field of positioning technology, and in particular to a posture initialization method, device, medium and electronic device. Background Art

[0002] When starting, driving, parking, or waiting, an autonomous vehicle needs to obtain the position information of the vehicle body in the world coordinate system.

[0003] In open outdoor areas, accurate fixed solutions are obtained using Global Navigation Satellite System (GNSS) signals and differential signals. However, in weak GNSS signal environments, such as those blocked by tall buildings, tunnels, or indoor environments, only a single GNSS point solution can be obtained, making it impossible for the receiving device to communicate with the satellite.

[0004] In this case, auxiliary solutions are often used for vehicle positioning. One approach uses image features captured by cameras to match them with pre-built, high-precision digital maps to obtain positioning results. Another approach uses data collected by lidar to obtain the optimal positioning result through exhaustive analysis.

[0005] However, both of the above-mentioned auxiliary positioning methods rely on high-precision digital maps and rely on brute force search to obtain results. They have problems such as large data volume, long calculation time, significant impact from changes in the external environment, and low reliability of positioning results.

[0006] Therefore, the present disclosure provides a posture initialization method to solve one of the above technical problems. Summary of the Invention

[0007] The purpose of this disclosure is to provide a posture initialization method, device, medium, and electronic device that can solve at least one of the above-mentioned technical problems. The specific solution is as follows:

[0008] According to a specific embodiment of the present disclosure, in a first aspect, the present disclosure provides a posture initialization method, comprising:

[0009] Get the current estimated position and scan frame point cloud;

[0010] intercepting a local point cloud map based on the estimated position in the preset point cloud map;

[0011] Filtering the local point cloud map based on at least two preset resolutions to generate a probability grid map corresponding to the preset resolution, wherein the probability grid map includes a plurality of grids in a three-dimensional space, at least one grid includes the projected coordinates of point data in the local point cloud map, and a grid value of the grid is equal to a probability value of the projected coordinates being distributed in the grid;

[0012] Generating a candidate pose set based on the scan frame point cloud, wherein the candidate pose set includes multiple groups of combined candidate poses;

[0013] An octree of the candidate pose set is jointly constructed based on the probability grid maps of the at least two preset resolutions, wherein each layer depth in the octree represents a preset resolution and the smaller the depth, the higher the preset resolution. Each node of the octree has eight child nodes or zero child nodes, each node includes a candidate pose and a node score, the node score of each node is equal to the average of the grid values ​​of the grid where the projection coordinates of the candidate pose of the corresponding node in the probability grid map are located, and the node score of the parent node in the octree is equal to the maximum node score among its child nodes.

[0014] The nodes of the octree are searched and pruned based on a preset convergence algorithm to determine the candidate pose of the current best node.

[0015] According to a specific embodiment of the present disclosure, in a second aspect, the present disclosure provides a posture initialization device, comprising:

[0016] An acquisition unit, used to obtain the current estimated position and scanning frame point cloud;

[0017] A clipping unit, configured to clip a local point cloud map based on the estimated position in a preset point cloud map;

[0018] a map generation unit, configured to filter the local point cloud map based on at least two preset resolutions, respectively, to generate a probability grid map corresponding to the preset resolutions, wherein the probability grid map includes a plurality of grids in a three-dimensional space, at least one grid including the projected coordinates of point data in the local point cloud map, and a grid value of the grid equals a probability value of the distribution of the projected coordinates in the grid;

[0019] A set generating unit, configured to generate a candidate pose set based on the scan frame point cloud, wherein the candidate pose set includes a plurality of groups of combined candidate poses;

[0020] A construction unit is configured to jointly construct an octree of the candidate pose set based on the probability grid maps of the at least two preset resolutions, wherein each layer depth in the octree represents a preset resolution and a smaller depth corresponds to a higher preset resolution, each node of the octree has eight child nodes or zero child nodes, each node includes a candidate pose and a node score, the node score of each node is equal to the average of the grid values ​​of the grid where the projection coordinates of the candidate pose of the corresponding node in the probability grid map are located, and the node score of the parent node in the octree is equal to the maximum node score among its child nodes;

[0021] A determination unit is used to search and prune the nodes of the octree based on a preset convergence algorithm to determine the candidate pose of the current best node.

[0022] According to a specific embodiment of the present disclosure, in a third aspect, the present disclosure provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the posture initialization method as described in any one of the above items.

[0023] According to the specific implementation of the present disclosure, in a fourth aspect, the present disclosure provides an electronic device, comprising: one or more processors; a storage device for storing one or more programs, which, when the one or more programs are executed by the one or more processors, enables the one or more processors to implement the posture initialization method as described in any one of the above items.

[0024] Compared with the prior art, the above solution of the embodiment of the present disclosure has at least the following beneficial effects:

[0025] The present disclosure provides a posture initialization method, device, medium and electronic device. The present disclosure uses the scan frame point cloud and the local point cloud map to generate an octree of the candidate posture set, and then uses a preset convergence algorithm to determine the candidate posture of the current best node. It does not rely on high-precision maps, but can obtain the current posture through point cloud data, which greatly reduces the amount of calculation. It does not rely on special scene constraints and is applicable to a variety of scenes. It realizes the positioning of the positioning device in a weak GNSS signal environment. The rotation histogram matching is used to filter out the wrong rotation candidate results, and the preset convergence algorithm is used to search and prune the nodes of the octree, avoiding the brute force search in the prior art, reducing the algorithm complexity, reducing the amount of calculation, and improving data processing efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] Figure 1 A flowchart of a posture initialization method according to an embodiment of the present disclosure is shown;

[0027] Figure 2 A unit block diagram of a posture initialization device according to an embodiment of the present disclosure is shown;

[0028] Figure 3 A schematic diagram of a connection structure of an electronic device provided according to an embodiment of the present disclosure is shown. DETAILED DESCRIPTION

[0029] To make the objectives, technical solutions, and advantages of the present disclosure more clear, the present disclosure will be further described in detail below with reference to the accompanying drawings. It is apparent that the embodiments described are only a portion of the embodiments of the present disclosure, rather than all of them. All other embodiments derived by persons of ordinary skill in the art based on the embodiments of the present disclosure without creative effort are intended to fall within the scope of protection of the present disclosure.

[0030] The terms used in the embodiments of the present disclosure are for the purpose of describing specific embodiments only and are not intended to limit the present disclosure. The singular forms "a," "an," "the," and "the" used in the embodiments of the present disclosure and the appended claims are also intended to include plural forms, and unless the context clearly indicates otherwise, "a plurality" generally includes at least two.

[0031] It should be understood that the term "and / or" as used herein is merely a description of the relationship between associated objects, indicating that three possible relationships exist. For example, "A and / or B" can represent: A exists alone, A and B exist simultaneously, or B exists alone. Furthermore, the character " / " in this document generally indicates that the associated objects are in an "or" relationship.

[0032] It should be understood that although the terms first, second, third, etc. may be used to describe in the embodiments of the present disclosure, these descriptions should not be limited to these terms. These terms are only used to distinguish the descriptions. For example, the first can also be referred to as the second, and similarly, the second can also be referred to as the first without departing from the scope of the embodiments of the present disclosure.

[0033] As used herein, the words "if" and "if" may be interpreted as "at the time of" or "when" or "in response to determining" or "in response to detecting," depending on the context. Similarly, the phrases "if it is determined" or "if (stated condition or event) is detected" may be interpreted as "when it is determined" or "in response to the determination" or "when detecting (stated condition or event)" or "in response to detecting (stated condition or event)," depending on the context.

[0034] It should also be noted that the terms "include," "comprises," or any other variations thereof are intended to encompass non-exclusive inclusion, such that a product or device comprising a series of elements includes not only those elements but also other elements not explicitly listed, or elements inherent to such product or device. In the absence of further limitations, an element defined by the phrase "comprises a..." does not exclude the presence of other identical elements in the product or device comprising the element.

[0035] It should be noted in particular that any symbols and / or numbers in the specification that are not marked in the accompanying drawings are not drawing marks.

[0036] Optional embodiments of the present disclosure are described in detail below with reference to the accompanying drawings.

[0037] Example 1

[0038] The embodiment provided in the present disclosure is an embodiment of a posture initialization method.

[0039] The following combination Figure 1 The embodiments of the present disclosure are described in detail.

[0040] Step S101, obtaining the current estimated position and scanning frame point cloud.

[0041] Positioning devices need to obtain their position information in the world coordinate system for positioning. With the maturity of satellite positioning technology, GNSS signals and differential signals can be used to obtain accurate fixed solutions in open outdoor environments. However, in some scenarios, GNSS can only obtain a single-point solution due to obstruction by tall buildings, tunnels, or indoor environments. In environments with weak GNSS signals, positioning devices can obtain location information by matching sensor data, such as data obtained by on-board cameras and / or lidar, with pre-built high-precision maps.

[0042] The scanning frame point cloud is the point cloud obtained after the laser radar scans the surrounding environment for a week.

[0043] An estimated location is the location information obtained by a positioning device through any means in a weak GNSS signal environment. Whether obtained through GNSS or sensors, the estimated location information will differ from the actual location. Therefore, the estimated location information does not meet the preset accuracy.

[0044] In some specific embodiments, obtaining the current estimated position includes the following steps:

[0045] Step S101 - 1 : obtaining variance values ​​between the current multiple returned positions and the actual positions through the global navigation satellite system.

[0046] Step S101 - 2 : determining the returned position corresponding to any variance value that meets a preset gap condition as the current estimated position.

[0047] For example, the variance value that meets the preset gap condition is the maximum variance value among the multiple variance values, and the maximum variance value is also the value with the largest error between the returned position and the actual position.

[0048] When initializing the pose for the first time, the estimated position can be determined by the global navigation satellite system or by the sensor, and the estimated position is used as the initial position.

[0049] Step S102 : intercepting a local point cloud map based on the estimated position in the point cloud map of preset accuracy.

[0050] The preset accuracy can be high or low. The high and low accuracy are relative to the standard accuracy. The disclosed embodiments do not impose any restrictions on the accuracy of point cloud maps and are applicable to point cloud maps of any accuracy. Compared to the prior art, which relies heavily on high-precision point cloud maps for positioning initialization, this method can effectively control the amount of data processing, reduce the amount of computation, and improve positioning efficiency.

[0051] The point cloud map is a three-dimensional map formed by splicing multiple frames of point cloud after calculating their relative poses.

[0052] The local point cloud map is a portion of the point cloud map extracted from the point cloud map. The local point cloud map includes the projected coordinates of the estimated position. For example, a cube with a preset side length is extracted from the point cloud map with the projected coordinates of the estimated position as the geometric center as the local point cloud map.

[0053] Step S103 : filtering the local point cloud map based on at least two preset resolutions to generate a probability grid map corresponding to the preset resolutions.

[0054] The probability grid map includes multiple grids in three-dimensional space, at least one grid includes the projected coordinates of the point data in the local point cloud map, and the grid value of the grid is equal to the probability value of the distribution of the projected coordinates in the grid. For example, in three-dimensional space, the resolution of the x-axis, y-axis, and z-axis is the grid size. When the x-axis resolution = the y-axis resolution = the z-axis resolution, each grid is a cube. The smaller the resolution, the higher the map accuracy. For example, if the x-axis resolution = the y-axis resolution = the z-axis resolution = 1 cm (the first preset resolution), then the grid size is 1 cm × 1 cm × 1 cm, then the positioning accuracy is 1 cm, and a probability grid map with a resolution of 1 cm is generated. If the x-axis resolution = the y-axis resolution = the z-axis resolution = 2 cm (the second preset resolution), then the grid size is 2 cm × 2 cm × 2 cm, then the positioning accuracy is 2 cm, and a probability grid map with a resolution of 2 cm is generated. Similarly, probability grid maps with multiple preset resolutions can be generated.

[0055] Step S104: generating a candidate pose set based on the scan frame point cloud.

[0056] The candidate pose set includes multiple groups of combined candidate poses.

[0057] The posture includes a position and a set of rotation angles. The set of rotation angles refers to a set of Euler angles. The Euler angles include: nutation angle θ, precession angle (i.e. precession angle) ψ and rotation angle Used to describe the pose of a rigid body / mobile coordinate system in a fixed coordinate system. Simply put, it uses the rotation components of the three axes X, Y, and Z to describe a 6-degree-of-freedom rotation. The multiple sets of candidate poses are pairwise combinations of multiple candidate positions and multiple sets of candidate rotation angles.

[0058] In some specific embodiments, generating a candidate pose set based on the scanned frame point cloud comprises the following steps:

[0059] In step S104 - 1 , for the scan frame point cloud, a candidate rotation set is generated with a preset angle step size, and a candidate position set is generated with a preset position step size.

[0060] The candidate rotation set includes multiple groups of candidate rotation angles, each group of candidate rotation angles refers to a group of Euler angles. For example, the candidate rotation set includes: A1(θ1, ψ1, )、A2(θ2,ψ2, )、A3(θ3,ψ3, ),……Aj(θj,ψj, ), where A1, A2, A3, ..., Aj are the group names of each group of candidate rotation angles, and the ones in the brackets are a group of candidate rotation angles.

[0061] The candidate position set includes multiple candidate positions. For example, the candidate position set includes: S1, S2, S3, ... Sk, where S1, S2, S3, ... Sk is the name of each candidate position.

[0062] Step S104 - 2 : Combining each group of candidate rotation angles in the candidate rotation set with each candidate position in the candidate position set in pairs to generate the candidate pose set.

[0063] The candidate pose set includes multiple groups of candidate poses.

[0064] For example, if the candidate rotation set includes: A1, A2, A3, ... Aj, and the candidate position set includes: S1, S2, S3, ... Sk, then after pairwise combination, multiple sets of candidate poses include: Z11 (A1, S1), Z12 (A1, S2), Z13 (A1, S3), ... Z1k (A1, Sk); Z21 (A2, S1), Z22 (A2, S2), Z23 (A2, S3), ... Z2k(A2, Sk); Z31(A3, S1), Z32(A3, S2), Z33(A3, S3), …Z3k(A3, Sk); …Zj1(Aj, S1), Zj2(Aj, S2), Zj3(Aj, S3), …Zjk(Aj, Sk); where Z is the group name of each candidate pose, and the ones in brackets are pairwise combinations of a group of candidate rotation angles and a candidate position.

[0065] In the disclosed embodiments, an octree is established based on the candidate pose set to search for the candidate pose of the current best node. That is, the size of the octree is determined by the number of candidate poses in the candidate pose set, which determines the computational complexity of pose initialization and the efficiency of data processing. In some specific embodiments, the method further includes the following steps:

[0066] Step S104-1a-1: Generate a first rotation histogram based on the local point cloud map.

[0067] The rotation histogram, also known as the histogram of directional gradients, is used to calculate the statistical value of the directional information of the local image gradient.

[0068] The first rotation histogram is used to calculate the statistical value of the direction information of the gradient of the local point cloud map. The first rotation histogram records the first projection coordinates of the point data in the local point cloud map.

[0069] Accordingly, generating a candidate rotation set with a preset angle step size for the scanned frame point cloud includes:

[0070] Step S104-1b-1, generating a second rotation histogram based on the scan frame point cloud.

[0071] The second rotation histogram is used to calculate the statistical value of the direction information of the gradient of the scanning frame point cloud. The second rotation histogram records the second projection coordinates of the point data in the scanning frame point cloud.

[0072] Step S104-1b-2: Filter the second projection coordinates in the second rotation histogram based on the first projection coordinates in the first rotation histogram to obtain matching second projection coordinates.

[0073] That is, if a second projection coordinate can coincide with any first projection coordinate, it can be determined that the second projection coordinate matches the first projection coordinate; if a second projection coordinate cannot coincide with any first projection coordinate, it can be determined that the second projection coordinate has no matching first projection coordinate.

[0074] Step S104-1b-3, obtaining point data related to the matched second projection coordinates from the scan frame point cloud.

[0075] For the unmatched second projection coordinates, the corresponding point data in the scan frame point cloud is eliminated, while the point data in the scan frame point cloud related to the matched second projection coordinates is retained.

[0076] Step S104-1b-4: generating a candidate rotation set with a preset angle step size for the point data related to the matched second projection coordinates in the scan frame point cloud.

[0077] Before generating the candidate rotation set, this specific embodiment screens the scan frame point cloud and eliminates point data in the scan frame point cloud that is not related to the local point cloud map, thereby reducing the number of candidate rotation angles in the candidate rotation set, thereby reducing the number of candidate poses and the size of the generated octree, reducing the amount of calculation, and improving the efficiency of data processing.

[0078] Step S105 : constructing an octree of the candidate pose set based on the probability grid maps of the at least two preset resolutions.

[0079] Each depth level in the octree represents a preset resolution, and smaller depths correspond to higher preset resolutions. Each node in the octree has eight or zero child nodes. A node with eight child nodes is called a parent node, and a node without child nodes is called a leaf node.

[0080] Each node includes a candidate pose and a node score. The method of node scoring is to downsample the probability grid map by the preset depth of the octree, that is, the maximum sampling method is adopted: Vnew = max(8^(tree_depth-depth)grids), that is, the node score of each node is equal to the average value of the grid values ​​of the grid where the projection coordinates of the candidate pose of the corresponding node are located in the probability grid map. The node score of the parent node in the octree is equal to the largest node score among its child nodes. Due to the use of the maximum sampling method, the evaluation score of the node with a smaller depth is always higher than that of the lower-level nodes. This can serve as the upper bound of the preset convergence algorithm described below.

[0081] Step S106: Search and prune the nodes of the octree based on a preset convergence algorithm to determine the candidate pose of the current best node.

[0082] The search is to access the nodes in the octree.

[0083] The pruning is to delete all the child nodes of a subtree, and the parent node becomes a leaf node. The purpose of pruning is to delete subtrees that do not meet the search conditions, reduce the number of search nodes, and speed up the search progress.

[0084] The preset convergence algorithm uses the upper bound determined by the maximum sampling method as the basis for pruning.

[0085] The candidate pose of the current best node is the pose that best matches the actual position of the positioning device in the currently generated octree.

[0086] In order to improve the efficiency of the search, in some specific embodiments, searching and pruning the nodes of the octree based on a preset convergence algorithm to determine the candidate pose of the current best node includes the following steps:

[0087] Step S106a: searching and pruning the nodes of the octree based on a preset convergence algorithm and a preset adaptive threshold, and determining a candidate pose of the current best node.

[0088] In some specific embodiments, searching and pruning the nodes of the octree based on a preset convergence algorithm and a preset adaptive threshold to determine the candidate pose of the current best node includes the following steps:

[0089] Step S106a-1: Search the nodes in the octree to obtain multiple searched nodes.

[0090] Step S106a-2a, when the multiple nodes are all leaf nodes, when the depths of the multiple nodes are all preset depths, and the node scores of the multiple nodes are all greater than a preset adaptive threshold, determine that the multiple nodes are all target nodes.

[0091] For example, if the depth of multiple leaf nodes is b, the preset depth is also b; the preset adaptive threshold is M, and the node scores of multiple leaf nodes are N1, N2, N3,...Ni, N1, N2, N3,...Ni, all greater than M, then it is determined that the multiple nodes are all target nodes.

[0092] Step S106a-3a: determining the target node corresponding to the maximum value from the node scores of the multiple target nodes as the current best node, and the candidate poses of the current best node.

[0093] For example, continuing with the above example, if N3 is the maximum value among the node scores of multiple nodes, then the target node to which N3 belongs is the current best node, and the candidate pose of the target node to which N3 belongs is also the candidate pose of the current best node.

[0094] In some other specific embodiments, after searching the nodes in the octree and obtaining the searched nodes, the following steps are further included:

[0095] Step S106a-2b, when the multiple nodes are all leaf nodes, when the depths of the multiple nodes are all less than the preset depth, and the node scores of the multiple nodes are all greater than the preset adaptive threshold, the corresponding node is split based on the preset resolution corresponding to the next depth of any node in the multiple nodes and the candidate pose of the corresponding node, and eight child nodes of the corresponding node are generated until the depth of the split child node is equal to the preset depth.

[0096] For example, if the depth of any leaf node is a, the preset depth is b, and a is less than b; the preset adaptive threshold is M, and the node scores of multiple leaf nodes are N1, N2, N3,...Ni, respectively, and N1, N2, N3,...Ni are all greater than M, then each leaf node is split, and each leaf node becomes a parent node, and the parent node generates eight child nodes, each child node becomes a leaf node, and is split according to the preset resolution corresponding to the depth of the leaf node; based on the maximum sampling method, the node score of each leaf node is equal to the average value of the grid value of the grid where the projection coordinates of its candidate pose are located in the probability grid map, and the node score of the parent node is equal to the maximum node score among its child nodes (i.e., leaf nodes); based on the above method, the leaf nodes can be split continuously until the depth of the split leaf node is equal to the preset depth.

[0097] In some other specific embodiments, after searching the nodes in the octree and obtaining the searched nodes, the following steps are further included:

[0098] Step S106a-2c: when the multiple nodes are all parent nodes, when the depths of the multiple nodes are all less than a preset depth, and when the node scores of the multiple nodes are all less than or equal to a preset adaptive threshold, prune the multiple nodes separately.

[0099] For example, if the depth of multiple parent nodes is a, the preset depth is b, and a is less than b; the preset adaptive threshold is M, and the node scores of multiple nodes are N1, N2, N3, ...Ni, N1, N2, N3, ...Ni, all less than or equal to M, then each leaf node is pruned.

[0100] After pruning, the nodes below the parent node are not searched, which speeds up the search and improves the efficiency of data processing.

[0101] In some specific embodiments, after determining the candidate pose of the current best node, the following steps are further included:

[0102] Step S107 , using the candidate pose of the current best node as the initial pose, and using a preset point cloud registration algorithm that depends on the initial pose to obtain an evaluation distance value or an optimization score.

[0103] The preset point cloud registration algorithms that rely on the initial pose include: ICP algorithm and UPF / UKF algorithm.

[0104] The evaluation distance value refers to the distance value between the position information in the candidate pose of the current best node and the actual position information.

[0105] Step S108 : when the evaluation distance value or the optimization score does not meet the preset accuracy condition, re-determine the candidate pose of another optimal node.

[0106] To avoid erroneous results, after the current best node is finally obtained, the candidate poses of the current best node are evaluated. This ensures that the candidate poses of the current best node meet the preset accuracy requirements. If the evaluation result does not meet the preset accuracy requirements, the candidate poses of another best node are re-determined. The disclosed embodiment adopts a pose initialization method, continuously performing iterative calculations until a candidate pose that meets the preset accuracy requirements is obtained.

[0107] In some specific embodiments, when the evaluation distance value or the optimization score does not meet the preset accuracy condition, re-determining the candidate pose of another optimal node includes the following steps:

[0108] Step S108a, when the evaluation distance value or the optimization score is greater than the preset first accuracy threshold and less than or equal to the preset second accuracy threshold, the candidate poses in the candidate pose set are reduced, a new candidate pose set is generated, and the operation of jointly constructing the octree of the candidate pose set based on the probability grid maps of the at least two preset resolutions is triggered.

[0109] The first accuracy threshold is preset as the optimal accuracy threshold. When the evaluation distance value or the optimization score is close to the optimal accuracy threshold, the number of candidate poses in the above-mentioned candidate pose set is reduced. After generating a new candidate pose set, return to step S105, jointly construct an octree of the new candidate pose set based on the probability grid maps of at least two preset resolutions, and then search and prune the nodes of the octree based on the preset convergence algorithm to determine the candidate pose of another optimal node, and then evaluate the candidate pose of another optimal node again until the evaluation result is less than or equal to the preset first accuracy threshold, that is, the candidate pose of the optimal node reaches the accuracy requirement.

[0110] In some other specific embodiments, when the evaluation distance value or the optimization score does not meet the preset accuracy condition, re-determining the candidate pose of another optimal node includes the following steps:

[0111] Step S108b: When the evaluation distance value or the optimization score is greater than a preset second accuracy threshold, the operation of obtaining the current estimated position and the scanning frame point cloud is triggered.

[0112] When the evaluation distance value or the optimization score distance best accuracy threshold is larger (greater than the preset second accuracy threshold), return to step S101, reacquire the current estimated position and scanning frame point cloud, and start new posture initialization.

[0113] In some specific embodiments, the method further comprises the following steps:

[0114] Step S109: When the evaluation distance value or the optimization score is less than or equal to a preset first accuracy threshold, the candidate posture of the current best node is determined as the current posture.

[0115] That is, the accuracy requirement for the candidate pose of the optimal node.

[0116] The disclosed embodiment utilizes the scanned frame point cloud and the local point cloud map to generate an octree of the candidate pose set, and then uses a preset convergence algorithm to determine the candidate pose of the current best node. It does not rely on high-precision maps, but can obtain the current pose through point cloud data, which greatly reduces the amount of calculation. It does not rely on special scene constraints and is applicable to a variety of scenes. It realizes the positioning of the positioning device in a weak GNSS signal environment. The rotation histogram matching is used to filter out the wrong rotation candidate results, and the preset convergence algorithm is used to search and prune the nodes of the octree, avoiding the brute force search in the prior art, reducing the algorithm complexity, reducing the amount of calculation, and improving the data processing efficiency.

[0117] Example 2

[0118] The present disclosure also provides an apparatus embodiment that is consistent with the above embodiment, and is used to implement the method steps described in the above embodiment. The interpretation based on the same name meaning is the same as that of the above embodiment, and has the same technical effect as the above embodiment, and will not be repeated here.

[0119] like Figure 2 As shown, the present disclosure provides a posture initialization device 200, comprising:

[0120] An acquisition unit 201 is used to acquire the current estimated position and the scan frame point cloud;

[0121] A clipping unit 202 is configured to clip a local point cloud map based on the estimated position in a preset point cloud map;

[0122] a map generation unit 203 configured to filter the local point cloud map based on at least two preset resolutions to generate a probability grid map corresponding to the preset resolutions, wherein the probability grid map includes a plurality of grids in a three-dimensional space, at least one grid including the projected coordinates of point data in the local point cloud map, and a grid value of the grid equals a probability value of the projected coordinates being distributed in the grid;

[0123] A set generating unit 204 is configured to generate a candidate pose set based on the scan frame point cloud, wherein the candidate pose set includes multiple groups of combined candidate poses;

[0124] A construction unit 205 is configured to jointly construct an octree of the candidate pose set based on the probability grid maps of the at least two preset resolutions, wherein each layer depth in the octree represents a preset resolution and a smaller depth corresponds to a higher preset resolution, each node of the octree has eight child nodes or zero child nodes, each node includes a candidate pose and a node score, the node score of each node is equal to the average grid value of the grid where the projection coordinates of the candidate pose of the corresponding node in the probability grid map are located, and the node score of the parent node in the octree is equal to the maximum node score among its child nodes;

[0125] The determination unit 206 is configured to search and prune the nodes of the octree based on a preset convergence algorithm to determine a candidate pose of the current best node.

[0126] Optionally, the determining unit 206 includes:

[0127] The first determination subunit is configured to search and prune the nodes of the octree based on a preset convergence algorithm and a preset adaptive threshold, and determine a candidate pose of a current optimal node.

[0128] Optionally, the first determining subunit includes:

[0129] A first acquisition subunit, configured to search for nodes in the octree and acquire a plurality of searched nodes;

[0130] A first determining subunit is configured to determine that the multiple nodes are all target nodes when the multiple nodes are all leaf nodes, when the depths of the multiple nodes are all preset depths, and when the node scores of the multiple nodes are all greater than a preset adaptive threshold;

[0131] The second determining subunit is configured to determine, from the node scores of the multiple target nodes, a target node corresponding to a maximum value as the current best node, and a candidate position of the current best node.

[0132] Optionally, the first determining subunit includes:

[0133] A splitting sub-unit is used to search the nodes in the octree, and after obtaining the searched multiple nodes, when the multiple nodes are all leaf nodes, when the depths of the multiple nodes are all less than a preset depth, and when the node scores of the multiple nodes are all greater than a preset adaptive threshold, split the corresponding node based on the preset resolution corresponding to the next depth of any node in the multiple nodes and the candidate pose of the corresponding node, and generate eight child nodes of the corresponding node until the depth of the split child node is equal to the preset depth.

[0134] Optionally, the first determining subunit includes:

[0135] A pruning subunit is used to search for nodes in the octree, and after obtaining multiple searched nodes, when the multiple nodes are all parent nodes, when the depths of the multiple nodes are all less than a preset depth, and the node scores of the multiple nodes are all less than or equal to a preset adaptive threshold, prune the multiple nodes separately.

[0136] Optionally, the set generating unit 204 includes:

[0137] a first generating subunit, configured to generate, for the scanned frame point cloud, a candidate rotation set with a preset angle step size, and a candidate position set with a preset position step size, wherein the candidate rotation set includes a plurality of sets of candidate rotation angles, and the candidate position set includes a plurality of candidate positions;

[0138] The second generating subunit is configured to combine each group of candidate rotation angles in the candidate rotation set with each candidate position in the candidate position set in pairs to generate the candidate pose set, wherein the candidate pose set includes multiple groups of candidate poses.

[0139] Optionally, the first generating subunit includes:

[0140] a third generating subunit, configured to generate a first rotation histogram based on the local point cloud map, wherein the first rotation histogram records first projection coordinates of point data in the local point cloud map;

[0141] a fourth generating subunit, configured to generate a second rotation histogram based on the scan frame point cloud, wherein the second rotation histogram records the second projection coordinates of the point data in the scan frame point cloud;

[0142] a second acquisition subunit, configured to filter the second projection coordinates in the second rotation histogram based on the first projection coordinates in the first rotation histogram to obtain matching second projection coordinates;

[0143] a third acquisition subunit, configured to acquire point data associated with the matched second projection coordinates from the scan frame point cloud;

[0144] The fifth generating subunit is configured to generate a candidate rotation set with a preset angle step size for point data related to the matched second projection coordinates in the scan frame point cloud.

[0145] Optionally, the device further includes:

[0146] An evaluation unit, configured to, after determining the candidate pose of the current best node, use the candidate pose of the current best node as the initial pose and obtain an evaluation distance value or an optimization score using a preset point cloud registration algorithm that depends on the initial pose;

[0147] A repositioning unit is used to re-determine a candidate position of another optimal node when the evaluation distance value or the optimization score does not meet a preset accuracy condition.

[0148] Optionally, the repositioning unit includes:

[0149] The first repositioning subunit is used to reduce the candidate poses in the candidate pose set, generate a new candidate pose set, and trigger the execution of the operation of jointly constructing the octree of the candidate pose set based on the probability grid maps of the at least two preset resolutions when the evaluation distance value or the optimization score is greater than a preset first accuracy threshold and less than or equal to a preset second accuracy threshold.

[0150] Optionally, the repositioning unit includes:

[0151] The second repositioning subunit is configured to trigger the execution of the operation of obtaining the current estimated position and the scanning frame point cloud when the evaluation distance value or the optimization score is greater than a preset second accuracy threshold.

[0152] Optionally, the device further includes:

[0153] A positioning unit is configured to determine, when the evaluation distance value or the optimization score is less than or equal to a preset first accuracy threshold, the candidate posture of the current best node as the current posture.

[0154] Optionally, the acquiring unit 201 includes:

[0155] a fourth acquisition subunit, configured to acquire variance values ​​between the current multiple returned positions and the actual positions through a global navigation satellite system;

[0156] The third determining subunit is configured to determine a returned position corresponding to any variance value that meets a preset gap condition as a current estimated position.

[0157] The disclosed embodiment utilizes the scanned frame point cloud and the local point cloud map to generate an octree of the candidate pose set, and then uses a preset convergence algorithm to determine the candidate pose of the current best node. It does not rely on high-precision maps, but can obtain the current pose through point cloud data, which greatly reduces the amount of calculation. It does not rely on special scene constraints and is applicable to a variety of scenes. It realizes the positioning of the positioning device in a weak GNSS signal environment. The rotation histogram matching is used to filter out the wrong rotation candidate results, and the preset convergence algorithm is used to search and prune the nodes of the octree, avoiding the brute force search in the prior art, reducing the algorithm complexity, reducing the amount of calculation, and improving the data processing efficiency.

[0158] Example 3

[0159] like Figure 3 As shown, this embodiment provides an electronic device, comprising: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions that can be executed by the one processor, and the instructions are executed by the at least one processor to enable the at least one processor to perform the method steps described in the above embodiment.

[0160] Example 4

[0161] An embodiment of the present disclosure provides a non-volatile computer storage medium, wherein the computer storage medium stores computer-executable instructions, and the computer-executable instructions can execute the method steps described in the above embodiment.

[0162] Example 5

[0163] Reference below Figure 3 , which shows a schematic diagram of the structure of an electronic device suitable for implementing the embodiments of the present disclosure. The terminal device in the embodiments of the present disclosure may include, but is not limited to, mobile terminals such as mobile phones, laptop computers, digital broadcast receivers, PDAs (personal digital assistants), PADs (tablet computers), PMPs (portable multimedia players), in-vehicle terminals (such as in-vehicle navigation terminals), and fixed terminals such as digital TVs and desktop computers. Figure 3 The electronic device shown is only an example and should not limit the functions and scope of use of the embodiments of the present disclosure.

[0164] like Figure 3 As shown, the electronic device may include a processing device (e.g., a central processing unit, a graphics processing unit, etc.) 301, which can perform various appropriate actions and processes according to a program stored in a read-only memory (ROM) 302 or a program loaded from a storage device 308 into a random access memory (RAM) 303. Various programs and data required for the operation of the electronic device are also stored in the RAM 303. The processing device 301, the ROM 302, and the RAM 303 are connected to each other via a bus 304. An input / output (I / O) interface 305 is also connected to the bus 304.

[0165] Typically, the following devices may be connected to the I / O interface 305: an input device 306 including, for example, a touch screen, a touchpad, a keyboard, a mouse, a camera, a microphone, an accelerometer, a gyroscope, etc.; an output device 305 including, for example, a liquid crystal display (LCD), a speaker, a vibrator, etc.; a storage device 308 including, for example, a magnetic tape, a hard disk, etc.; and a communication device 309. The communication device 309 may allow the electronic device to communicate with other devices wirelessly or by wire to exchange data. Although Figure 3 The electronic device is shown with various devices, but it should be understood that it is not required to implement or possess all of the devices shown. More or fewer devices may be implemented or possessed instead.

[0166] In particular, according to an embodiment of the present disclosure, the process described above with reference to the flowchart can be implemented as a computer software program. For example, an embodiment of the present disclosure includes a computer program product, which includes a computer program carried on a computer-readable medium, and the computer program includes program code for executing the method shown in the flowchart. In such an embodiment, the computer program can be downloaded and installed from the network via the communication device 309, or installed from the storage device 308, or installed from the ROM 302. When the computer program is executed by the processing device 301, the above-mentioned functions defined in the method of the embodiment of the present disclosure are performed.

[0167] It should be noted that the computer-readable medium mentioned above in the present disclosure may be a computer-readable signal medium or a computer-readable storage medium, or any combination of the two. A computer-readable storage medium may be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, device, or component, or any combination of the above. More specific examples of computer-readable storage media may include, but are not limited to: an electrical connection with one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above. In the present disclosure, a computer-readable storage medium may be any tangible medium that contains or stores a program that can be used by or in conjunction with an instruction execution system, device, or component. In the present disclosure, a computer-readable signal medium may include a data signal propagated in baseband or as part of a carrier wave, which carries computer-readable program code. Such a propagated data signal may take a variety of forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination of the above. A computer-readable signal medium may also be any computer-readable medium other than a computer-readable storage medium that can transmit, propagate, or transport a program for use by or in conjunction with an instruction execution system, apparatus, or device. The program code contained on the computer-readable medium may be transmitted using any suitable medium, including but not limited to wires, optical cables, RF (radio frequency), etc., or any suitable combination thereof.

[0168] The computer-readable medium may be included in the electronic device, or may exist independently without being incorporated into the electronic device.

[0169] Computer program code for performing the operations of the present disclosure may be written in one or more programming languages, or a combination thereof, including object-oriented programming languages ​​such as Java, Smalltalk, C++, and conventional procedural programming languages ​​such as "C" or similar programming languages. The program code may be executed entirely on the user's computer, partially on the user's computer, as a stand-alone software package, partially on the user's computer and partially on a remote computer, or entirely on the remote computer or server. In cases involving a remote computer, the remote computer may be connected to the user's computer through any type of network, including a local area network (LAN) or a wide area network (WAN), or may be connected to an external computer (e.g., through the Internet using an Internet service provider).

[0170] The flowcharts and block diagrams in the accompanying drawings illustrate the possible implementation architecture, functions and operations of the systems, methods and computer program products according to various embodiments of the present disclosure. In this regard, each box in the flowchart or block diagram can represent a module, program segment, or a part of code, and the module, program segment, or a part of code contains one or more executable instructions for realizing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in a different order than that marked in the accompanying drawings. For example, two boxes represented in succession can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and / or flowchart, and the combination of the boxes in the block diagram and / or flowchart, can be implemented with a dedicated hardware-based system that performs the specified function or operation, or can be implemented with a combination of dedicated hardware and computer instructions.

[0171] The units involved in the embodiments described in this disclosure may be implemented in software or hardware, wherein the name of a unit does not necessarily limit the unit itself.

Claims

1. A posture initialization method, characterized in that: include: Get the current estimated position and scan frame point cloud; intercepting a local point cloud map based on the estimated position in the preset point cloud map; Filtering the local point cloud map based on at least two preset resolutions to generate a probability grid map corresponding to the preset resolution, wherein the probability grid map includes a plurality of grids in a three-dimensional space, at least one grid includes the projected coordinates of point data in the local point cloud map, and a grid value of the grid is equal to a probability value of the projected coordinates being distributed in the grid; Generating a candidate pose set based on the scan frame point cloud, wherein the candidate pose set includes multiple groups of combined candidate poses; An octree of the candidate pose set is jointly constructed based on the probability grid maps of the at least two preset resolutions, wherein each layer depth in the octree represents a preset resolution and the smaller the depth, the higher the preset resolution. Each node of the octree has eight child nodes or zero child nodes, each node includes a candidate pose and a node score, the node score of each node is equal to the average of the grid values ​​of the grid where the projection coordinates of the candidate pose of the corresponding node in the probability grid map are located, and the node score of the parent node in the octree is equal to the maximum node score among its child nodes. Searching and pruning the nodes of the octree based on a preset convergence algorithm to determine the candidate pose of the current best node; Generating a candidate pose set based on the scanned frame point cloud includes: For the scanned frame point cloud, generating a candidate rotation set with a preset angle step size, and generating a candidate position set with a preset position step size, wherein the candidate rotation set includes multiple groups of candidate rotation angles, and the candidate position set includes multiple candidate positions; Combining each group of candidate rotation angles in the candidate rotation set with each candidate position in the candidate position set in pairs to generate the candidate pose set, wherein the candidate pose set includes multiple groups of candidate poses; The method further comprises: generating a first rotation histogram based on the local point cloud map, wherein the first rotation histogram records first projection coordinates of point data in the local point cloud map; Accordingly, generating a candidate rotation set with a preset angle step size for the scanned frame point cloud includes: generating a second rotation histogram based on the scan frame point cloud, wherein the second rotation histogram records the second projection coordinates of the point data in the scan frame point cloud; Filter the second projection coordinates in the second rotation histogram based on the first projection coordinates in the first rotation histogram to obtain matching second projection coordinates; Acquire point data associated with the matched second projection coordinates from the scan frame point cloud; A candidate rotation set is generated with a preset angle step size for point data associated with the matched second projection coordinates in the scan frame point cloud.

2. The method according to claim 1, characterized in that The searching and pruning of the nodes of the octree based on a preset convergence algorithm to determine the candidate pose of the current best node includes: The nodes of the octree are searched and pruned based on a preset convergence algorithm and a preset adaptive threshold to determine the candidate pose of the current best node.

3. The method according to claim 2, characterized in that The searching and pruning of the nodes of the octree based on a preset convergence algorithm and a preset adaptive threshold to determine the candidate pose of the current best node includes: Searching for nodes in the octree to obtain multiple searched nodes; When the multiple nodes are all leaf nodes, when the depths of the multiple nodes are all preset depths, and the node scores of the multiple nodes are all greater than a preset adaptive threshold, determining that the multiple nodes are all target nodes; A target node corresponding to a maximum value among the node scores of the multiple target nodes is determined as the current best node, and a candidate position of the current best node.

4. The method according to claim 3, characterized in that After searching for the nodes in the octree and obtaining the searched multiple nodes, the method further includes: When the multiple nodes are all leaf nodes, when the depths of the multiple nodes are all less than the preset depth, and the node scores of the multiple nodes are all greater than the preset adaptive threshold, the corresponding node is split based on the preset resolution corresponding to the next depth of any node in the multiple nodes and the candidate pose of the corresponding node, and eight child nodes of the corresponding node are generated until the depth of the split child nodes is equal to the preset depth.

5. The method according to claim 3, characterized in that After searching for the nodes in the octree and obtaining the searched multiple nodes, the method further includes: When the multiple nodes are all parent nodes, when the depths of the multiple nodes are all less than a preset depth, and when the node scores of the multiple nodes are all less than or equal to a preset adaptive threshold, the multiple nodes are pruned separately.

6. The method according to claim 1, characterized in that After determining the candidate poses of the current best node, the method further includes: Taking the candidate pose of the current best node as the initial pose, and using a preset point cloud registration algorithm that depends on the initial pose to obtain an evaluation distance value or an optimization score; When the evaluation distance value or the optimization score does not meet the preset accuracy condition, a candidate pose of another optimal node is re-determined.

7. The method according to claim 6, characterized in that When the evaluation distance value or the optimization score does not meet the preset accuracy condition, re-determining another candidate pose of the best node includes: When the evaluation distance value or the optimization score is greater than a preset first accuracy threshold and less than or equal to a preset second accuracy threshold, the candidate poses in the candidate pose set are reduced, a new candidate pose set is generated, and the operation of jointly constructing the octree of the candidate pose set based on the probability grid maps of the at least two preset resolutions is triggered.

8. The method according to claim 6, characterized in that When the evaluation distance value or the optimization score does not meet the preset accuracy condition, re-determining another candidate pose of the best node includes: When the evaluation distance value or the optimization score is greater than a preset second accuracy threshold, the operation of obtaining the current estimated position and the scanning frame point cloud is triggered.

9. The method according to claim 6, characterized in that The method further comprises: When the evaluation distance value or the optimization score is less than or equal to a preset first accuracy threshold, the candidate pose of the current best node is determined to be the current pose.

10. The method according to claim 1, characterized in that The obtaining of the current estimated position includes: Obtain variance values ​​of multiple current returned positions and actual positions through the global navigation satellite system; The returned position corresponding to any variance value that meets the preset gap condition is determined as the current estimated position.

Images