Relocation method and device, computer readable storage medium and electronic equipment

Abstract

The invention belongs to the technical field of automatic driving, and particularly relates to a repositioning method and device, a computer readable storage medium and electronic equipment. The method comprises the following steps: generating a multi-resolution sparse voxel map corresponding to a point cloud map; acquiring a laser point cloud, and performing translation rotation branch delimitation in the multi-resolution sparse voxel map according to the laser point cloud to obtain a first positioning pose; and performing point-line-surface feature matching in the point cloud map according to the first positioning pose to obtain a second positioning pose. According to the method and the device, translation and rotation branch delimitation can be carried out based on the multi-resolution sparse voxel map, so that an initial positioning pose is searched, point-line-surface feature matching is carried out on the basis, and a more accurate positioning pose can be obtained.

Description

Technical Field

[0001] This application belongs to the field of autonomous driving technology, and particularly relates to a relocalization method, device, computer-readable storage medium, and electronic device. Background Art

[0002] The positioning module based on 3D lidar is one of the core technology modules of autonomous driving vehicles, playing a key role in enhancing driving safety and environmental adaptability. The relocalization function in the positioning module determines the pose of the vehicle in a pre-constructed map. In the prior art, it mainly performs relocalization through the combined navigation of the Global Navigation Satellite System (GNSS) / Inertial Navigation System (INS) and point cloud registration algorithms such as the Normal Distribution Transform (NDT), Iterative Closest Point (ICP), and Generalized Iterative Closest Point (GICP).

[0003] However, in environments with poor satellite signals such as urban high-rise buildings and tunnels, the vehicle cannot accurately receive GNSS signals, which will reduce the positioning accuracy of the GNSS / INS combined navigation output, resulting in unsuccessful relocalization. In addition, the point cloud registration algorithm uses laser point clouds and point cloud maps for point cloud registration to obtain the positioning pose, which is sensitive to the initial value and easily falls into a local optimal solution. Summary of the Invention

[0004] In view of this, embodiments of the present application provide a relocalization method, device, computer-readable storage medium, and electronic device to solve the problem of poor accuracy existing in the existing relocalization methods.

[0005] The first aspect of the embodiments of the present application provides a relocalization method, which may include:

[0006] Generating a multi-resolution sparse voxel map corresponding to the point cloud map;

[0007] Obtaining the laser point cloud, and performing translational and rotational branch and bound in the multi-resolution sparse voxel map according to the laser point cloud to obtain a first positioning pose;

[0008] Performing point-line-plane feature matching in the point cloud map according to the first positioning pose to obtain a second positioning pose.

[0009] In a specific implementation of the first aspect, the obtaining of the first positioning pose by performing translational and rotational branch and bound on the laser point cloud in the multi-resolution sparse voxel map may include:

[0010] Determine the search range and search step size for translational and rotational branch and bound;

[0011] Determine a pose search set according to the search range and the search step size;

[0012] Based on the pose search set, perform pose transformation on the laser point cloud to obtain a first transformed laser point cloud;

[0013] Determine the point cloud matching score of the first transformed laser point cloud in the multi-resolution sparse voxel map;

[0014] Determine the pose corresponding to the maximum value of the point cloud matching score as the first positioning pose.

[0015] In a specific implementation of the first aspect, the determining of the point cloud matching score of the first transformed laser point cloud in the multi-resolution sparse voxel map may include:

[0016] Perform hash calculation on each laser point in the first transformed laser point cloud to obtain the hash value of each laser point;

[0017] Determine the point cloud matching score of the first transformed laser point cloud in the multi-resolution sparse voxel map according to the hash values of each laser point.

[0018] In a specific implementation of the first aspect, the determining of the point cloud matching score of the first transformed laser point cloud in the multi-resolution sparse voxel map according to the hash values of each laser point may include:

[0019] Respectively determine the laser point matching scores of the hash values of each laser point in the multi-resolution sparse voxel map;

[0020] Determine the point cloud matching score of the first transformed laser point cloud in the multi-resolution sparse voxel map according to the laser point matching scores of each laser point.

[0021] In a specific implementation of the first aspect, the obtaining of the second positioning pose by performing point, line and plane feature matching in the point cloud map according to the first positioning pose may include:

[0022] Respectively determine the nearest neighbor point fitting plane and the nearest neighbor point fitting line corresponding to the first positioning pose in the point cloud map;

[0023] Determine a plane distance error function corresponding to the plane fitted by the nearest neighbor points and a line distance error function corresponding to the line fitted by the nearest neighbor points, respectively;

[0024] Perform iterative solution of the positioning pose according to the plane distance error function and the line distance error function to obtain the second positioning pose.

[0025] In a specific implementation manner of the first aspect, the step of determining the plane fitted by the nearest neighbor points corresponding to the first positioning pose and the line fitted by the nearest neighbor points in the point cloud map may include:

[0026] Determine a local point cloud map corresponding to the first positioning pose in the point cloud map;

[0027] Perform pose transformation on the laser point cloud according to the first positioning pose to obtain a second transformed laser point cloud;

[0028] Determine the nearest neighbor points of the second transformed laser point cloud in the local point cloud map;

[0029] Perform plane fitting and line fitting on the nearest neighbor points to obtain the plane fitted by the nearest neighbor points and the line fitted by the nearest neighbor points.

[0030] In a specific implementation manner of the first aspect, the step of performing iterative solution of the positioning pose according to the plane distance error function and the line distance error function to obtain the second positioning pose may include:

[0031] Derive the plane distance error function and the line distance error function respectively to obtain a plane Jacobian matrix and a line Jacobian matrix;

[0032] Determine a positioning pose iterative matrix according to the plane Jacobian matrix and the line Jacobian matrix;

[0033] Determine the total positioning pose error according to the plane distance error function and the line distance error function;

[0034] Determine a positioning pose compensation amount according to the positioning pose iterative matrix and the total positioning pose error;

[0035] Compensate the current positioning pose according to the positioning pose compensation amount until a preset iteration termination condition is met to obtain the second positioning pose.

[0036] A second aspect of the embodiments of the present application provides a relocalization device, which may include:

[0037] A voxel map generation module, configured to generate a multi-resolution sparse voxel map corresponding to the point cloud map;

[0038] The first positioning pose determination module is used to obtain the laser point cloud and perform translational and rotational branch and bound in the multi-resolution sparse voxel map based on the laser point cloud to obtain the first positioning pose;

[0039] The second positioning pose determination module is used to perform point-line-plane feature matching in the point cloud map based on the first positioning pose to obtain the second positioning pose.

[0040] In a specific implementation manner of the second aspect, the first positioning pose determination module may include:

[0041] The search parameter determination sub-module is used to determine the search range and search step size of the translational and rotational branch and bound;

[0042] The pose search set determination sub-module is used to determine the pose search set according to the search range and the search step size;

[0043] The pose transformation sub-module is used to perform pose transformation on the laser point cloud based on the pose search set to obtain the first transformed laser point cloud;

[0044] The point cloud matching score determination sub-module is used to determine the point cloud matching score of the first transformed laser point cloud in the multi-resolution sparse voxel map;

[0045] The first positioning pose determination sub-module is used to determine the pose corresponding to the maximum value of the point cloud matching score as the first positioning pose.

[0046] In a specific implementation manner of the second aspect, the point cloud matching score determination sub-module may include:

[0047] The laser point hash value determination unit is used to perform hash calculation on each laser point in the first transformed laser point cloud to obtain each laser point hash value;

[0048] The point cloud matching score determination unit is used to determine the point cloud matching score of the first transformed laser point cloud in the multi-resolution sparse voxel map according to each laser point hash value.

[0049] In a specific implementation manner of the second aspect, the point cloud matching score determination unit may specifically be used to: respectively determine the laser point matching scores of each laser point hash value in the multi-resolution sparse voxel map; determine the point cloud matching score of the first transformed laser point cloud in the multi-resolution sparse voxel map according to each laser point matching score.

[0050] In a specific implementation manner of the second aspect, the second positioning pose determination module may include:

[0051] The nearest neighbor point fitting sub-module is used to respectively determine the nearest neighbor point fitting plane and the nearest neighbor point fitting line corresponding to the first positioning pose in the point cloud map;

[0052] The distance error function determination sub-module is used to respectively determine the plane distance error function corresponding to the nearest neighbor point fitting plane and the line distance error function corresponding to the nearest neighbor point fitting line;

[0053] The pose iterative solution sub-module is used to perform iterative solution of the positioning pose according to the plane distance error function and the line distance error function to obtain the second positioning pose.

[0054] In a specific implementation manner of the second aspect, the nearest neighbor point fitting sub-module may specifically be used to: determine a local point cloud map corresponding to the first positioning pose in the point cloud map; perform pose transformation on the laser point cloud according to the first positioning pose to obtain a second transformed laser point cloud; determine the nearest neighbor points of the second transformed laser point cloud in the local point cloud map; perform plane fitting and line fitting on the nearest neighbor points to obtain the nearest neighbor point fitting plane and the nearest neighbor point fitting line.

[0055] In a specific implementation manner of the second aspect, the pose iterative solution sub-module may specifically be used to: respectively take the derivatives of the plane distance error function and the line distance error function to obtain a plane Jacobian matrix and a line Jacobian matrix; determine a positioning pose iterative matrix according to the plane Jacobian matrix and the line Jacobian matrix; determine the total positioning pose error according to the plane distance error function and the line distance error function; determine a positioning pose compensation amount according to the positioning pose iterative matrix and the total positioning pose error; compensate the current positioning pose according to the positioning pose compensation amount until a preset iteration termination condition is met to obtain the second positioning pose.

[0056] A third aspect of the embodiments of the present application provides a computer-readable storage medium, where the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps of any of the above repositioning methods are implemented.

[0057] A fourth aspect of the embodiments of the present application provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, and when the processor executes the computer program, the steps of any of the above repositioning methods are implemented.

[0058] A fifth aspect of the embodiments of the present application provides a computer program product, and when the computer program product runs on an electronic device, the electronic device is enabled to execute the steps of any of the above repositioning methods.

[0059] The beneficial effects of the embodiments of the present application compared with the prior art are as follows: The embodiments of the present application generate a multi-resolution sparse voxel map corresponding to the point cloud map; obtain the laser point cloud, and perform translational and rotational branch and bound in the multi-resolution sparse voxel map according to the laser point cloud to obtain the first positioning pose; perform point-line-plane feature matching in the point cloud map according to the first positioning pose to obtain the second positioning pose. Through the embodiments of the present application, translational and rotational branch and bound can be performed based on the multi-resolution sparse voxel map, so as to search for the initial positioning pose. On this basis, point-line-plane feature matching can be performed to obtain a more accurate positioning pose. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the following will briefly introduce the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings in the following description are only some embodiments of the present application. For those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings.

[0061] Figure 1 It is a flowchart of an embodiment of a repositioning method in the embodiments of the present application;

[0062] Figure 2 It is a schematic flowchart of performing translational and rotational branch and bound in a multi-resolution sparse voxel map according to the laser point cloud;

[0063] Figure 3 It is a schematic flowchart of performing point-line-plane feature matching in the point cloud map according to the first positioning pose;

[0064] Figure 4 It is a structural diagram of an embodiment of a repositioning device in the embodiments of the present application;

[0065] Figure 5 It is a schematic block diagram of an electronic device in the embodiments of the present application. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0066] In order to make the object, features, and advantages of the present application more obvious and understandable, the following will clearly and completely describe the technical solutions in the embodiments of the present application in conjunction with the drawings in the embodiments of the present application. Obviously, the embodiments described below are only some embodiments of the present application, rather than all embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope of protection of the present application.

[0067] It should be understood that, as used in this specification and the appended claims, the term "comprising" indicates the presence of the described features, integers, steps, operations, elements, and / or components, but does not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and / or groups thereof.

[0068] It should also be understood that the terminology used in this application specification is for the purpose of describing particular embodiments only and is not intended to limit this application. As used in this application specification and the appended claims, unless the context clearly dictates otherwise, the singular forms "a", "an", and "the" are intended to include the plural forms.

[0069] It should be further understood that the term "and / or" used in this application specification and the appended claims refers to any combination and all possible combinations of one or more of the associated listed items, and includes these combinations.

[0070] As used in this specification and the appended claims, the term "if" can be interpreted as "when" or "once" or "in response to determining" or "in response to detecting" depending on the context. Similarly, the phrase "if determined" or "if [the described condition or event] is detected" can be interpreted as meaning "once determined" or "in response to determining" or "once [the described condition or event] is detected" or "in response to detecting [the described condition or event]" depending on the context.

[0071] In addition, in the description of this application, the terms "first", "second", "third", etc. are used only for distinguishing descriptions and cannot be understood as indicating or implying relative importance.

[0072] The positioning module based on 3D lidar is one of the core technology modules of an autonomous vehicle and plays a key role in enhancing driving safety and environmental adaptability. The relocalization function in the positioning module determines the pose of the vehicle in a pre-constructed map. In the prior art, it is mainly through the combined navigation of the Global Navigation Satellite System (GNSS) / Inertial Navigation System (INS) and point cloud registration algorithms such as the Normal Distribution Transform (NDT), Iterative Closest Point (ICP), Generalized Iterative Closest Point (GICP), etc. to perform relocalization.

[0073] However, in environments with poor satellite signals such as high-rise buildings and tunnels in the city, vehicles cannot accurately receive GNSS signals, which will reduce the positioning accuracy of the GNSS / INS integrated navigation output and lead to the failure of successful repositioning. In addition, the point cloud registration algorithm uses laser point clouds and point cloud maps for point cloud registration to obtain the positioning pose, which is sensitive to the initial value and easily falls into a local optimal solution.

[0074] In view of this, the embodiments of the present application provide a repositioning method, device, computer-readable storage medium, and electronic device to solve the problem of poor accuracy existing in the existing repositioning methods.

[0075] In the embodiments of the present application, translational and rotational branch and bound can be performed based on a multi-resolution sparse voxel map to search for an initial positioning pose. On this basis, point-line-plane feature matching can be performed to obtain a more accurate positioning pose.

[0076] The execution subject of the embodiments of the present application can be an electronic device, including but not limited to computing devices such as mobile phones, tablet computers, desktop computers, notebooks, handheld computers, robots, and servers.

[0077] Please refer to Figure 1 , an embodiment of a repositioning method in the embodiments of the present application may include:

[0078] Step S101, generate a multi-resolution sparse voxel map corresponding to the point cloud map.

[0079] A voxel map is a three-dimensional space representation method with voxels as basic units. A voxel is the smallest volume unit in three-dimensional space, similar to the three-dimensional expansion of two-dimensional pixels. Each voxel represents a cube grid unit of a fixed size, and describes three-dimensional space information through its coordinate position and attributes.

[0080] In the embodiments of the present application, a multi-resolution sparse voxel map corresponding to the point cloud map can be generated. In the scenario of global repositioning, the point cloud map is a global point cloud map; in the scenario of local repositioning, the point cloud map is a local point cloud map, where the local point cloud map is a map cropped from the global point cloud map according to the initial pose of local repositioning, that is, a map obtained by cropping a certain range of the area around the initial pose as the center.

[0081] In the multi-resolution sparse voxel map, a tree structure with multiple levels can be formed, and the voxel grid resolution of each level is r l = 2 l r i where r iis the minimum voxel grid size, and l is the level of the corresponding tree structure. Each level contains an array of hash buckets, which can convert the three-dimensional point coordinates P(x, y, z) in the point cloud map into voxel coordinates v(v x , v y , v z ) and store them in the hash bucket array, and use the spatial hash function to calculate the corresponding hash bucket index, so as to obtain the spatial hash table. As shown in the following formula:

[0082]

[0083] hash(v) = f(v) mod T l

[0084] where f(v) is the spatial hash function of voxel coordinate v, T l is the size of the hash bucket at the level where the voxel coordinate v is located, and hash(v) is the hash bucket index corresponding to the voxel coordinate v.

[0085] Managing the multi-resolution sparse voxel map through the spatial hash table can effectively reduce the map storage space and improve the speed of loading the map.

[0086] Step S102, obtain the laser point cloud, and perform translational and rotational branch and bound (B&B) in the multi-resolution sparse voxel map according to the laser point cloud to obtain the first positioning pose.

[0087] As Figure 2 shown, step S102 may include the following process:

[0088] Step S1021, determine the search range and search step size of the translational and rotational branch and bound.

[0089] The pose space within the translational and rotational search range can be represented using a tree structure. Each child node of a node in the tree is a partition of the search space represented by that node, and each leaf node corresponds to a solution. The translational and rotational branches continuously divide the search space, and the non-leaf nodes determine an upper bound for the child nodes after each branch.

[0090] In the translational branch, the search ranges in the three axial directions can be flexibly set according to the actual situation. A parent node at each level can branch into 8 child nodes, and the search step size is the voxel grid resolution at that level, that is, r l = 2 l r i .

[0091] In the rotational branch, the roll angle and pitch angle can be set to a smaller search range where is the lower limit of the search range for roll angle and pitch angle, is the upper limit of the search range for roll angle and pitch angle. The search range of yaw angle can be flexibly set according to the actual situation, and the maximum search range is Among them, is the lower limit of the search range for yaw angle, is the upper limit of the search range for yaw angle. In the rotation branch, the search step size in each level can be divided into the same size, as shown in the following formula:

[0092]

[0093] Among them, W min is the lower limit of the search range, W max is the upper limit of the search range, d max is the farthest scanning distance of the laser point cloud, and δ(r l ) is the search step size of level l.

[0094] Step S1022: Determine the pose search set according to the search range and search step size.

[0095] After determining the search ranges and search step sizes of the translation branch and the rotation branch, each pose to be searched can be determined one by one according to the search step size within the corresponding search range, and these poses to be searched together constitute the pose search set.

[0096] Step S1023: Perform pose transformation on the laser point cloud based on the pose search set to obtain the first transformed laser point cloud.

[0097] Taking any pose c in the pose search set as an example, the pose transformation matrix corresponding to this pose can be determined and perform pose transformation on each laser point s in the laser point cloud according to this pose transformation matrix k respectively, so as to obtain each laser point in the first transformed laser point cloud Among them, k is the serial number of the laser point, 1 ≤ k ≤ K, and K is the total number of laser points in the laser point cloud.

[0098] Step S1024: Determine the point cloud matching score of the first transformed laser point cloud in the multi-resolution sparse voxel map.

[0099] In the embodiment of the present application, hash calculation can be performed on each laser point in the first transformed laser point cloud respectively, so as to obtain the hash value of each laser point. Then, the point cloud matching score of the first transformed laser point cloud in the multi-resolution sparse voxel map can be determined according to the hash value of each laser point.

[0100] Specifically, the laser point matching scores of each laser point hash value in the multi-resolution sparse voxel map can be determined according to the spatial hash table respectively, and then the point cloud matching score of the laser point cloud in the multi-resolution sparse voxel map can be determined according to each laser point matching score. As an example, the laser point matching scores can be summed up to obtain the point cloud matching score, as shown in the following formula:

[0101]

[0102] Among them, is the laser point matching score of the k-th laser point in the first transformed laser point cloud, and score(c) is the point cloud matching score corresponding to the pose c.

[0103] Step S1025: Determine the pose corresponding to the maximum point cloud matching score as the first positioning pose.

[0104] In the embodiment of the present application, breadth-first search (BFS) can be used to search in the pose search set. During the search process, the score of each non-leaf node is the upper limit of the scores of its child nodes. If the score of a child node is lower than the upper limit score, the branch is pruned. When the point cloud matching score reaches the maximum value, the corresponding pose can be determined as the first positioning pose.

[0105] By means of translational and rotational branch and bound, the dependence on the initial value can be effectively reduced, and falling into the local optimal solution can be avoided, so as to obtain a more accurate positioning pose.

[0106] Step S103: Perform point-line-plane feature matching in the point cloud map according to the first positioning pose to obtain the second positioning pose.

[0107] As Figure 3 shown, step S103 may include the following process:

[0108] Step S1031: Determine the nearest neighbor point fitting plane and the nearest neighbor point fitting line corresponding to the first positioning pose in the point cloud map.

[0109] In the embodiment of the present application, a local point cloud map corresponding to the first positioning pose can be determined in the point cloud map, that is, with the first positioning pose as the center, a certain range of the surrounding area in the global point cloud map is cropped, and the cropped area is the local point cloud map corresponding to the first positioning pose. According to the first positioning pose, the laser point cloud can be subjected to pose transformation to obtain the second transformed laser point cloud. Then, the nearest neighbor points of the second transformed laser point cloud can be determined in the local point cloud map, and plane fitting and line fitting are performed on the nearest neighbor points to obtain the nearest neighbor point fitting plane and the nearest neighbor point fitting line.

[0110] Step S1032: Determine the plane distance error function corresponding to the plane fitted by the nearest neighbor points and the line distance error function corresponding to the line fitted by the nearest neighbor points, respectively.

[0111] In the embodiment of the present application, the plane distance error function and the line distance error function are as shown in the following formula:

[0112] e si = n T (Rq i + t) + d s

[0113] e ei = d e ×(Rq i + t - p0)

[0114] Where q i is the laser point cloud, R and t are the rotation matrix and translation matrix corresponding to the first positioning pose respectively, n is the plane unit normal vector, d s is the intercept, d e is the direction vector of unit length, p0 is a point on the line, e si is the plane distance error function, and e ei is the line distance error function.

[0115] Step S1033: Perform iterative solution of the positioning pose according to the plane distance error function and the line distance error function to obtain the second positioning pose.

[0116] In the embodiment of the present application, the plane distance error function and the line distance error function can be differentiated respectively to obtain the plane Jacobian matrix and the line Jacobian matrix. As shown in the following formula:

[0117]

[0118] Where J s is the plane Jacobian matrix, and J e is the line Jacobian matrix.

[0119] According to the plane Jacobian matrix and the line Jacobian matrix, the positioning pose iteration matrix can be determined. As shown in the following formula:

[0120] H = J s T J s + J e T J e

[0121] Where H is the positioning pose iteration matrix.

[0122] Based on the planar distance error function and the straight-line distance error function, the total positioning pose error can be determined. As shown in the following formula:

[0123] e = e si + e ei

[0124] where e is the total positioning pose error.

[0125] Based on the positioning pose iteration matrix and the total positioning pose error, the positioning pose compensation amount can be determined. As shown in the following formula:

[0126] HΔX = e

[0127] where ΔX is the positioning pose compensation amount.

[0128] After obtaining the positioning pose compensation amount, the current positioning pose can be compensated according to the positioning pose compensation amount. The sum of the current positioning pose and the positioning pose compensation amount is used as the new current positioning pose, and the next round of iteration process is restarted until the preset iteration termination condition is met, that is, the positioning pose compensation amount is less than the preset compensation amount threshold, then the iteration process can be ended, and the current positioning pose is determined as the final second positioning pose. Among them, the specific value of the compensation amount threshold can be flexibly set according to the actual situation, and the embodiments of the present application do not make specific limitations on this.

[0129] In summary, the embodiments of the present application generate a multi-resolution sparse voxel map corresponding to the point cloud map; obtain the laser point cloud, and perform translational and rotational branch delimitation in the multi-resolution sparse voxel map according to the laser point cloud to obtain the first positioning pose; perform point-line-plane feature matching in the point cloud map according to the first positioning pose to obtain the second positioning pose. Through the embodiments of the present application, translational and rotational branch delimitation can be performed based on the multi-resolution sparse voxel map, so as to search for the initial positioning pose. On this basis, point-line-plane feature matching can be performed to obtain a more accurate positioning pose.

[0130] It should be understood that the magnitudes of the sequence numbers of the steps in the above embodiments do not mean the order of execution. The execution order of each process should be determined according to its function and internal logic, and should not constitute any limitation to the implementation process of the embodiments of the present application.

[0131] Corresponding to the repositioning method described in the above embodiments, Figure 4 Fig. shows a structural diagram of an embodiment of a repositioning device provided by the embodiments of the present application.

[0132] In this embodiment, a repositioning device may include:

[0133] A voxel map generation module 401, configured to generate a multi-resolution sparse voxel map corresponding to the point cloud map;

[0134] The first positioning pose determination module 402 is configured to obtain a laser point cloud and perform translational and rotational branch and bound in the multi-resolution sparse voxel map according to the laser point cloud to obtain a first positioning pose;

[0135] The second positioning pose determination module 403 is configured to perform point-line-plane feature matching in the point cloud map according to the first positioning pose to obtain a second positioning pose.

[0136] In a specific implementation manner of the embodiment of the present application, the first positioning pose determination module may include:

[0137] The search parameter determination sub-module is configured to determine the search range and search step of the translational and rotational branch and bound;

[0138] The pose search set determination sub-module is configured to determine a pose search set according to the search range and the search step;

[0139] The pose transformation sub-module is configured to perform pose transformation on the laser point cloud based on the pose search set to obtain a first transformed laser point cloud;

[0140] The point cloud matching score determination sub-module is configured to determine the point cloud matching score of the first transformed laser point cloud in the multi-resolution sparse voxel map;

[0141] The first positioning pose determination sub-module is configured to determine the pose corresponding to the maximum value of the point cloud matching score as the first positioning pose.

[0142] In a specific implementation manner of the embodiment of the present application, the point cloud matching score determination sub-module may include:

[0143] The laser point hash value determination unit is configured to perform hash calculation on each laser point in the first transformed laser point cloud to obtain each laser point hash value;

[0144] The point cloud matching score determination unit is configured to determine the point cloud matching score of the first transformed laser point cloud in the multi-resolution sparse voxel map according to each laser point hash value.

[0145] In a specific implementation manner of the embodiment of the present application, the point cloud matching score determination unit may be specifically configured to: respectively determine the laser point matching scores of each laser point hash value in the multi-resolution sparse voxel map; and determine the point cloud matching score of the first transformed laser point cloud in the multi-resolution sparse voxel map according to each laser point matching score.

[0146] In a specific implementation manner of the embodiment of the present application, the second positioning pose determination module may include:

[0147] The nearest neighbor point fitting sub-module is used to respectively determine the nearest neighbor point fitting plane and the nearest neighbor point fitting line corresponding to the first positioning pose in the point cloud map;

[0148] The distance error function determination sub-module is used to respectively determine the plane distance error function corresponding to the nearest neighbor point fitting plane and the line distance error function corresponding to the nearest neighbor point fitting line;

[0149] The pose iterative solution sub-module is used to perform iterative solution of the positioning pose according to the plane distance error function and the line distance error function to obtain the second positioning pose.

[0150] In a specific implementation manner of the embodiment of the present application, the nearest neighbor point fitting sub-module may specifically be used to: determine a local point cloud map corresponding to the first positioning pose in the point cloud map; perform pose transformation on the laser point cloud according to the first positioning pose to obtain a second transformed laser point cloud; determine the nearest neighbor points of the second transformed laser point cloud in the local point cloud map; perform plane fitting and line fitting on the nearest neighbor points to obtain the nearest neighbor point fitting plane and the nearest neighbor point fitting line.

[0151] In a specific implementation manner of the embodiment of the present application, the pose iterative solution sub-module may specifically be used to: respectively take the derivatives of the plane distance error function and the line distance error function to obtain a plane Jacobian matrix and a line Jacobian matrix; determine a positioning pose iterative matrix according to the plane Jacobian matrix and the line Jacobian matrix; determine a total positioning pose error according to the plane distance error function and the line distance error function; determine a positioning pose compensation amount according to the positioning pose iterative matrix and the total positioning pose error; compensate the current positioning pose according to the positioning pose compensation amount until a preset iteration termination condition is satisfied to obtain the second positioning pose.

[0152] Those skilled in the art can clearly understand that for the convenience and brevity of description, the specific working processes of the above-described devices, modules, and units can refer to the corresponding processes in the foregoing method embodiments and will not be elaborated herein.

[0153] In the above embodiments, the descriptions of the various embodiments have their respective emphases. For parts not detailed or recorded in a certain embodiment, reference may be made to the relevant descriptions of other embodiments.

[0154] Figure 5 The schematic block diagram of an electronic device provided by an embodiment of the present application is shown. For the convenience of description, only parts related to the embodiment of the present application are shown.

[0155] As shown in Figure 5 the figure, the electronic device 5 of this embodiment includes: a processor 50, a memory 51, and a computer program 52 stored in the memory 51 and executable on the processor 50. When the processor 50 executes the computer program 52, it implements the steps in the above-mentioned various relocation method embodiments, such as Figure 1 the steps S101 to S103 shown in the figure. Alternatively, when the processor 50 executes the computer program 52, it implements the functions of each module / unit in the above-mentioned various device embodiments, such as Figure 4 the functions of the modules 401 to 403 shown in the figure.

[0156] Exemplarily, the computer program 52 can be divided into one or more modules / units. The one or more modules / units are stored in the memory 51 and executed by the processor 50 to complete this application. The one or more modules / units can be a series of computer program instruction segments capable of performing specific functions, and these instruction segments are used to describe the execution process of the computer program 52 in the electronic device 5.

[0157] The electronic device 5 may include, but is not limited to, computing devices such as mobile phones, tablet computers, desktop computers, laptops, palmtop computers, robots, and servers. Those skilled in the art can understand that Figure 5 this is merely an example of the electronic device 5 and does not constitute a limitation on the electronic device 5. It may include more or fewer components than shown in the figure, or combine certain components, or have different components. For example, the electronic device 5 may further include input / output devices, network access devices, buses, etc.

[0158] The processor 50 may be a central processing unit (CPU), or may also be other general-purpose processors, digital signal processors (DSPs), application specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor or the processor may also be any conventional processor, etc.

[0159] The memory 51 may be an internal storage unit of the electronic device 5, such as a hard disk or memory of the electronic device 5. The memory 51 may also be an external storage device of the electronic device 5, such as a plug-in hard disk, a Smart Media Card (SMC), a Secure Digital (SD) card, a Flash Card, etc. equipped on the electronic device 5. Further, the memory 51 may also include both the internal storage unit of the electronic device 5 and external storage devices. The memory 51 is used to store the computer program and other programs and data required by the electronic device 5. The memory 51 may also be used to temporarily store data that has been output or is to be output.

[0160] Those skilled in the art can clearly understand that, for the convenience and brevity of description, only the above division of each functional unit and module is used as an example. In practical applications, the above functions can be allocated to different functional units and modules according to needs, that is, the internal structure of the device is divided into different functional units or modules to complete all or part of the functions described above. Each functional unit and module in the embodiments can be integrated in a processing unit, or each unit can exist physically alone, or two or more units can be integrated in one unit. The above integrated units can be implemented in the form of hardware or in the form of software functional units. In addition, the specific names of each functional unit and module are only for the convenience of mutual distinction and do not limit the protection scope of this application. The specific working processes of the units and modules in the above system can refer to the corresponding processes in the foregoing method embodiments and will not be elaborated herein.

[0161] In the above embodiments, the descriptions of the respective embodiments have their own emphases. For parts not detailed or recorded in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0162] Those of ordinary skill in the art can realize that the units and algorithm steps of each example described in combination with the embodiments disclosed herein can be implemented by electronic hardware, or by a combination of computer software and electronic hardware. Whether these functions are executed in hardware or software depends on the specific application and design constraints of the technical solution. Professionals can use different methods to implement the described functions for each specific application, but such implementation should not be considered to exceed the scope of this application.

[0163] In the embodiments provided in this application, it should be understood that the disclosed device / electronic device and method can be implemented in other ways. For example, the device / electronic device embodiments described above are merely illustrative. For example, the division of the modules or units is only a logical function division. In actual implementation, there may be other division methods. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the displayed or discussed couplings or direct couplings or communication connections to each other can be through some interfaces. The indirect couplings or communication connections of the devices or units can be in electrical, mechanical or other forms.

[0164] The units described as separate components may or may not be physically separated. The components shown as units may or may not be physical units, that is, they can be located in one place or distributed to multiple network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0165] In addition, in each embodiment of this application, the functional units can be integrated in a processing unit, or each unit can exist physically alone, or two or more units can be integrated in one unit. The above integrated units can be implemented in the form of hardware or in the form of software functional units.

[0166] If the integrated module / unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, to implement all or part of the processes in the above embodiment methods of this application, it can also be completed by a computer program instructing relevant hardware. The computer program can be stored in a computer-readable storage medium. When the computer program is executed by a processor, the steps of the above-mentioned method embodiments can be implemented. Among them, the computer program includes computer program code, and the computer program code can be in source code form, object code form, executable file or some intermediate form, etc. The computer-readable storage medium can include: any entity or device that can carry the computer program code, recording medium, USB flash drive, mobile hard disk, magnetic disk, optical disk, computer memory, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), electrical carrier signal, telecommunication signal, and software distribution medium, etc. It should be noted that the content included 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, the computer-readable storage medium does not include electrical carrier signals and telecommunication signals.

[0167] The embodiments described above are only used to illustrate the technical solutions of the present application, rather than to limit it; although the present application has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions recorded in the foregoing embodiments, or perform equivalent replacements on some of the technical features; and these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the various embodiments of the present application, and should all be included in the protection scope of the present application.

Claims

1. A relocation method, characterized in that, Including: Generating a multi-resolution sparse voxel map corresponding to the point cloud map; Obtaining the laser point cloud, and performing translational and rotational branch and bound in the multi-resolution sparse voxel map according to the laser point cloud to obtain the first positioning pose; Performing point-line-plane feature matching in the point cloud map according to the first positioning pose to obtain the second positioning pose.

2. The relocating method according to claim 1, wherein The performing translational and rotational branch and bound in the multi-resolution sparse voxel map according to the laser point cloud to obtain the first positioning pose includes: Determining the search range and search step size of the translational and rotational branch and bound; Determining a pose search set according to the search range and the search step size; Based on the pose search set, performing pose transformation on the laser point cloud to obtain the first transformed laser point cloud; Determining the point cloud matching score of the first transformed laser point cloud in the multi-resolution sparse voxel map; Determining the pose corresponding to the maximum value of the point cloud matching score as the first positioning pose.

3. The relocating method according to claim 2, wherein The determining the point cloud matching score of the first transformed laser point cloud in the multi-resolution sparse voxel map includes: Performing hash calculation on each laser point in the first transformed laser point cloud to obtain the hash value of each laser point; Determining the point cloud matching score of the first transformed laser point cloud in the multi-resolution sparse voxel map according to the hash values of each laser point.

4. The relocating method according to claim 3, wherein The determining the point cloud matching score of the first transformed laser point cloud in the multi-resolution sparse voxel map according to the hash values of each laser point includes: Respectively determining the laser point matching score of each laser point hash value in the multi-resolution sparse voxel map; Determining the point cloud matching score of the first transformed laser point cloud in the multi-resolution sparse voxel map according to the laser point matching scores of each laser point.

5. The relocating method according to any one of claims 1 to 4, characterized in that, The performing point-line-plane feature matching in the point cloud map according to the first positioning pose to obtain the second positioning pose includes: Respectively determining the nearest neighbor point fitting plane and the nearest neighbor point fitting line corresponding to the first positioning pose in the point cloud map; Respectively determining the plane distance error function corresponding to the nearest neighbor point fitting plane and the line distance error function corresponding to the nearest neighbor point fitting line; Performing iterative solution of the positioning pose according to the plane distance error function and the line distance error function to obtain the second positioning pose.

6. The relocating method according to claim 5, wherein The respectively determining the nearest neighbor point fitting plane and the nearest neighbor point fitting line corresponding to the first positioning pose in the point cloud map includes: Determining the local point cloud map corresponding to the first positioning pose in the point cloud map; Performing pose transformation on the laser point cloud according to the first positioning pose to obtain the second transformed laser point cloud; Determining the nearest neighbor points of the second transformed laser point cloud in the local point cloud map; Performing plane fitting and line fitting on the nearest neighbor points to obtain the nearest neighbor point fitting plane and the nearest neighbor point fitting line.

7. The relocating method according to claim 5, wherein The performing iterative solution of the positioning pose according to the plane distance error function and the line distance error function to obtain the second positioning pose includes: Derive the planar distance error function and the linear distance error function respectively to obtain a planar Jacobian matrix and a linear Jacobian matrix; Determine a positioning pose iteration matrix according to the planar Jacobian matrix and the linear Jacobian matrix; Determine a total positioning pose error according to the planar distance error function and the linear distance error function; Determine a positioning pose compensation amount according to the positioning pose iteration matrix and the total positioning pose error; Compensate the current positioning pose according to the positioning pose compensation amount until a preset iteration termination condition is satisfied to obtain the second positioning pose.

8. A relocation device, characterized in that, Including: A voxel map generation module for generating a multi-resolution sparse voxel map corresponding to the point cloud map; A first positioning pose determination module for acquiring a laser point cloud and performing translational and rotational branch bounding in the multi-resolution sparse voxel map according to the laser point cloud to obtain a first positioning pose; A second positioning pose determination module for performing point-line-plane feature matching in the point cloud map according to the first positioning pose to obtain a second positioning pose.

9. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by a processor, the steps of the repositioning method according to any one of claims 1 to 7 are implemented.

10. An electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, the steps of the repositioning method according to any one of claims 1 to 7 are implemented.

Images