Relocation method and device and vehicle

Abstract

The invention relates to the technical field of relocation, in particular to a relocation method and device and a vehicle, and the method comprises the steps: synchronously operating a plurality of relocation threads based on a given initial pose and an input point cloud obtained in real time, so as to carry out the relocation calculation of a target object; wherein the plurality of relocation threads comprise a feature relocation thread and a branch and bound relocation thread; and if any one of the plurality of relocation threads preferentially outputs the optimal pose of the target object, interrupting all the other relocation threads in operation. Therefore, the problem that an existing positioning module is low in repositioning precision can be effectively solved.

Description

Technical Field

[0001] The present application relates to the field of repositioning technology, and in particular to a repositioning method, device and vehicle. Background Art

[0002] The positioning module based on 3D lidar is one of the core technical modules of autonomous vehicles, and plays a key role in improving driving safety and environmental adaptability.

[0003] In autonomous vehicles, if a GNSS / INS combined navigation system is used for positioning, the vehicle cannot accurately receive GNSS signals in environments with poor satellite signals, such as high-rise buildings and tunnels in cities. This will reduce the positioning accuracy of the GNSS / INS combined navigation output and may even lead to unsuccessful repositioning.

[0004] Among them, GNSS stands for Global Navigation Satellite System, which is the global navigation satellite system; INS stands for Inertial Navigation System, which is the inertial navigation system. Summary of the Invention

[0005] In view of this, embodiments of the present application provide a repositioning method, device, and vehicle, which can effectively solve the problem of low repositioning accuracy of existing positioning modules.

[0006] In a first aspect, an embodiment of the present application provides a relocation method, the method comprising: Based on a given initial pose and an input point cloud acquired in real time, multiple relocalization threads are synchronously run to perform relocalization calculations on the target object respectively; wherein the multiple relocalization threads include a feature relocalization thread and a branch and bound relocalization thread; If any one of the plurality of repositioning threads preferentially outputs the optimal posture of the target object, all other repositioning threads in operation are interrupted.

[0007] In some embodiments, if any one of the plurality of relocation threads preferentially outputs the optimal posture of the target object, then all other relocation threads in operation are interrupted, including: If the feature relocalization thread preferentially outputs the optimal pose of the target object, interrupting the branch and bound relocalization thread; If the branch and bound relocalization thread preferentially outputs the optimal pose of the target object, the feature relocalization thread is interrupted.

[0008] In some embodiments, the feature relocalization thread prioritizes outputting the optimal pose of the target object, including: Converting each point in the input point cloud into a local point cloud map according to the initial pose, and searching for the nearest neighbor point corresponding to each point in the local point cloud map; Use the nearest neighbor points of each point to fit a plane and a straight line; Distance error functions between points in the input point cloud and the plane, and between points and the line are respectively constructed to construct a least squares problem, and iterative optimization is performed based on the constructed distance error functions to solve the optimal posture of the target object.

[0009] In some embodiments, the distance error function between a point in the input point cloud and the plane, and between a point and the line, comprises: e ei = d e × ( R × q i + t-p 0); e si = n T ( R × q i + t )+ d s ; in, e si represents the error function from the point in the input point cloud to the plane, e ei represents the error function from a point in the input point cloud to a line, q i represents the input lidar point cloud, R represents the rotation matrix, t represents the translation vector, d s represents the intercept, p 0 represents a point on the line, n represents the plane unit normal vector, d e Represents a direction vector of unit length.

[0010] In some embodiments, the iterative optimization based on the constructed distance error function includes: The error function from the point in the input point cloud to the plane and the error function from the point in the input point cloud to the line are respectively derived to calculate the corresponding Jacobian matrix; based on the Jacobian matrix, the Gauss-Newton method is used to iteratively calculate and solve to obtain the optimal posture.

[0011] In some embodiments, the optimal posture is solved using the following formula: H= J s T × J s + J e T × J e , e = e si + e ei H△X = e in, J s represents the Jacobian matrix obtained by derivation of the error function from the point in the input point cloud to the plane, J e represents the Jacobian matrix obtained by derivation of the error function from the point to the line in the input point cloud, H represents the Hessian matrix, △X represents the pose increment, e represents the error function, e si represents the error function from the point in the input point cloud to the plane, e ei represents the error function from a point in the input point cloud to a line.

[0012] In some embodiments, the branch and bound relocation thread preferentially outputs the optimal pose of the target object, including: In the branch and bound relocalization thread, based on the set translation and rotation branch and bound strategy, each candidate pose is queried in the current pose space determined based on the input point cloud, and a matching score of the candidate pose is calculated using a pre-built multi-resolution sparse voxel map, and an optimal pose is determined according to the matching score.

[0013] In some embodiments, the translation and rotation branch and bound strategy based on the setting queries each candidate pose in the current pose space determined based on the input point cloud, calculates the matching score of the candidate pose using a pre-built multi-resolution sparse voxel map, and determines the optimal pose based on the matching score, including: The pose of each point in the target pose space is represented by a tree structure and converted to the global coordinate system; Calculating a hash key for each candidate pose corresponding to each node in the global coordinate system using a spatial hash function; Matching corresponding voxels in the multi-resolution sparse voxel map according to the hash key, counting the number of successfully matched voxels, and generating a matching score for the candidate pose based on the number of voxels; The matching score is used as an evaluation function and input into a branch and bound search algorithm to evaluate and prune nodes in the target pose space in the global coordinate system until the optimal pose is obtained.

[0014] In some embodiments, the multi-resolution sparse voxel map construction method includes: Determine the local point cloud map based on the initial pose; Convert each point in the local point cloud map to each spatial resolution level to obtain voxel coordinates of the corresponding spatial level; A corresponding hash key is calculated using a spatial hash function according to each point and the corresponding voxel coordinates, and the corresponding voxel coordinates are stored in a spatial hash table according to the hash key; A multi-resolution sparse voxel map is constructed according to the spatial hash table.

[0015] In a second aspect, an embodiment of the present application provides a relocation device, the device comprising: A thread running module is used to synchronously run a plurality of relocalization threads based on a given initial pose and an input point cloud acquired in real time, so as to respectively perform relocalization calculations on the target object; wherein the plurality of relocalization threads include a feature relocalization thread and a branch and bound relocalization thread; The thread control module is configured to interrupt all other running repositioning threads when any one of the plurality of repositioning threads outputs the optimal posture of the target object first.

[0016] In a third aspect, an embodiment of the present application provides a vehicle, which is positioned using a repositioning method provided in the first aspect of the present application.

[0017] The embodiments of the present application have the following beneficial effects: In this application, based on a given initial pose and an input point cloud acquired in real time, several relocation threads are run synchronously to perform relocation calculations on the target object respectively; wherein, the several relocation threads include a feature relocation thread and a branch and delimit relocation thread; if any of the several relocation threads preferentially outputs the optimal pose of the target object, all other relocation threads in operation are interrupted. Among them, the several relocation threads include at least two relocation threads with different parameters such as the size of the strategy search range, the length of calculation time, and the sensitivity to the initial pose, in order to cope with the relocation needs in various environments. There is always one thread whose parameters meet the current environment and can quickly obtain the optimal pose, and interrupting other threads can reduce the amount of calculation. Therefore, this application can effectively solve the problem of low relocation accuracy of the existing positioning module. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the following is a brief introduction to the drawings required for use in the embodiments. It should be understood that the following drawings only show certain embodiments of the present application and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other relevant drawings can be obtained based on these drawings without creative work.

[0019] Figure 1 A flow chart of the relocation method according to an embodiment of the present application is shown; Figure 2 A flow chart of thread interrupt management in the relocation method according to an embodiment of the present application is shown; Figure 3 A workflow diagram of a feature relocation thread in the relocation method according to an embodiment of the present application is shown; Figure 4 A flowchart of constructing a multi-resolution sparse voxel map based on a spatial hash table in a relocation method according to an embodiment of the present application is shown; Figure 5 A workflow diagram of a branch and bound relocation thread in the relocation method according to an embodiment of the present application is shown. DETAILED DESCRIPTION

[0020] The technical solutions in the embodiments of the present application will be described clearly and completely below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all the embodiments.

[0021] The components of the embodiments of the present application generally described and illustrated in the drawings herein may be arranged and designed in a variety of different configurations. Therefore, the following detailed description of the embodiments of the present application provided in the drawings is not intended to limit the scope of the claimed application, but rather merely represents selected embodiments of the present application. All other embodiments obtained by those skilled in the art based on the embodiments of the present application without creative effort are within the scope of protection of the present application.

[0022] Hereinafter, the terms "including", "having" and their cognates used in various embodiments of the present application are intended only to indicate specific features, numbers, steps, operations, elements, components or combinations of the aforementioned items, and should not be understood as excluding the existence of one or more other features, numbers, steps, operations, elements, components or combinations of the aforementioned items or adding the possibility of one or more features, numbers, steps, operations, elements, components or combinations of the aforementioned items. In addition, the terms "first", "second", "third" and the like are only used to distinguish descriptions and should not be understood as indicating or implying relative importance.

[0023] Unless otherwise defined, all terms used herein (including technical and scientific terms) have the same meaning as commonly understood by those skilled in the art to which the various embodiments of the present application belong. The terms (such as those defined in generally used dictionaries) will be interpreted as having the same meaning as in the context of the relevant technical field and will not be interpreted as having an idealized meaning or an overly formal meaning unless clearly defined in the various embodiments of the present application.

[0024] The following describes some embodiments of the present application in detail with reference to the accompanying drawings. In the absence of conflict, the following embodiments and features in the embodiments may be combined with each other.

[0025] In order to solve the problem of low repositioning accuracy of existing positioning modules, the present application provides a repositioning method, device and vehicle, which aims to solve the repositioning problem in urban environments with dense buildings. A multi-threaded repositioning method is used to increase the search range of the optimal solution, effectively reduce the calculation time, and achieve fast, robust and accurate repositioning.

[0026] The relocation method is described below with reference to some specific embodiments.

[0027] Figure 1 A flow chart of a relocation method according to an embodiment of the present application is shown. Exemplarily, the relocation method includes the following steps: S100: Based on a given initial pose and a real-time input point cloud, multiple relocalization threads are synchronously run to perform relocalization calculations on the target object. The multiple relocalization threads may include two or more threads, such as a feature relocalization thread and a branch and bound relocalization thread.

[0028] The repositioning in the embodiment of the present application is to re-estimate the current pose in a pre-built point cloud map, which is usually used in scenarios such as recovery after pose loss, re-matching after initialization or jump, map switching, and initial pose ambiguity. For example, after the autonomous driving vehicle comes out of a tunnel and the GNSS signal is restored, the position in the map needs to be re-determined. The pose of the current input point cloud in the point cloud map is determined by matching the input point cloud with the point cloud map. For example, the input point cloud collected by a lidar is matched with the location cloud map. The goal of the repositioning method in the embodiment of the present application is to determine the current pose, not to build a map. It is used for recovery after pose loss or positioning recovery when the initial pose is inaccurate. It can be understood that in a preset scenario, based on a given initial pose and the input point cloud acquired in real time, several repositioning threads are run synchronously to perform repositioning calculations on the target object respectively. The preset scenario includes scenarios such as recovery after pose loss, re-matching after initialization or jump, map switching, and initial pose ambiguity.

[0029] A relocalization thread is a thread that executes the corresponding code to implement the relocalization function. For example, there may be two or more relocalization threads with different parameter values ​​such as the strategy search range size, calculation time, and sensitivity to the initial pose, to meet the relocalization needs in various environments.

[0030] Exemplarily, several relocalization threads include feature relocalization threads and branch-and-bound relocalization threads. The feature relocalization thread is a thread with a small strategy search range, low computational time, and high requirements for initial value accuracy. For example, the feature relocalization thread can use point cloud registration for relocalization, such as NDT (Normal Distributions Transform), ICP (Iterative Closest Point), GICP (Generalized ICP), and other algorithms. These algorithms use radar point clouds and point cloud maps for point cloud registration to obtain positioning poses, but these algorithms are sensitive to the accuracy of the initial values.

[0031] The branch-and-bound relocalization thread is a thread with a large search range, high computational time, and low requirements for initial value accuracy. For example, in the embodiments of the present application, the branch-and-bound relocalization thread can use a descriptor-based matching algorithm such as ScanContext to obtain the positioning pose, but it has been found that this method is computationally expensive.

[0032] Exemplarily, in an embodiment of the present application, when the acquired pose needs to be restored, initialized, or re-matched after a jump, the map is switched, or the initial pose is blurred, the pre-built feature relocalization thread and the branch and bound relocalization thread are run synchronously to perform relocalization calculations on the target object.

[0033] S200: If any one of the plurality of relocation threads outputs the optimal posture of the target object first, all other relocation threads in operation are interrupted.

[0034] In this application, several relocation threads run simultaneously. No matter what the environment is, there is always one thread whose parameters meet the current scenario, which will quickly relocate successfully and obtain the optimal posture. In order to improve the response speed, save computing resources and enhance robustness, the remaining relocation threads are interrupted.

[0035] like Figure 2 As shown, in step S200, any one of the plurality of relocation threads preferentially outputs the optimal posture of the target object, and then interrupts all other relocation threads in operation, including: S210: If the feature relocation thread preferentially outputs the optimal pose of the target object, the branch and bound relocation thread is interrupted.

[0036] For example, when the accuracy of a given initial pose is high, the feature relocalization thread prioritizes successful relocalization and outputs the optimal pose of the target object, interrupting the branch and bound relocalization thread. This feature relocalization thread has low computational time.

[0037] In one embodiment, to quickly obtain the optimal pose, the present invention uses the initial pose to determine a local point cloud map, narrowing the search space and reducing computational complexity. By establishing point correspondences, this provides basic data for subsequent construction of the error function and least-squares optimization. Because only the local map is searched, matching is fast and convergence is achieved quickly, making it suitable for scenarios where the initial pose is relatively accurate.

[0038] like Figure 3 As shown, in step S210, the feature relocalization thread prioritizes outputting the optimal pose of the target object, including: S211, converting each point in the input point cloud into a local point cloud map according to the initial pose, and searching for the nearest neighbor point corresponding to each point in the local point cloud map.

[0039] The input point cloud, also known as the input LiDAR point cloud, refers to a data set collected by an on-board 3D LiDAR sensor at a specific moment or within a short period of time, reflecting the three-dimensional geometric structure of the vehicle's surroundings. This type of point cloud is typically composed of a large number of discrete three-dimensional points, each of which contains spatial coordinate information (x, y, z) and sometimes additional attributes, such as intensity (laser reflection intensity, related to the target surface material); timestamp (timestamp): the acquisition time of the point; channel number (laser ID): indicating which transmission channel of the LiDAR the point was acquired by; color information (RGB): if the point cloud is fused with other sensors, color information may be added. In the embodiments of the present application, the input LiDAR point cloud refers to one or more frames of point cloud data collected in real time by the on-board 3D LiDAR during the operation of the autonomous vehicle, which is used to compare with a known local point cloud map to achieve accurate positioning of the vehicle in the map (i.e., "relocalization"). The input point cloud is the starting point of the relocalization process and is used to match with the existing local point cloud map.

[0040] Furthermore, step S211 specifically includes: a) According to the initial pose T i Get a local point cloud map.

[0041] The local point cloud map is equivalent to intercepting a point cloud from the global map. i A sub-map area with a certain search radius as the center. i : represents the initial input pose, typically a 6-degree-of-freedom transformation consisting of three translation components (x, y, z) and three rotation components (roll, pitch, yaw). In other words, a local point cloud map refers to the collection of map point clouds within a local area near a given initial pose, rather than the entire global map. This "localization" is intended to reduce computational effort and improve efficiency.

[0042] In the embodiment of the present application, data structures such as KD-Tree, OctoMap, and spatial hash table can be used to quickly retrieve data related to T i Adjacent point cloud areas; you can also set a spherical or cubic area with a fixed radius and extract the point cloud data therein as a local point cloud map.

[0043] b) Input the lidar point cloud q i Perform pose transformation. i : represents the laser radar scanning point cloud at the current moment (also called input point cloud or source point cloud). Pose transformation: q i Each point in the current estimated initial pose T iPerform coordinate transformation (from sensor coordinate system to map coordinate system). The transformed point cloud is recorded as: q i ' = T i ·q i .

[0044] c) Find the nearest neighbor point p in the local point cloud map i .

[0045] For the transformed point cloud q i 'For each point in the local map, find its nearest neighbor point p i , which is used to construct the error function later. For example, the nearest neighbor search algorithm is used to find the nearest neighbor point p i , such as: KD-Tree (suitable for static map structure), ANN (Approximate Nearest Neighbor), FLANN (Fast Library for Approximate Nearest Neighbors). In the feature relocalization thread, after transforming the input point cloud to the local map coordinate system, the nearest neighbor point is found for each point to form a point pair (q i ', p i ).

[0046] In one embodiment, the above point pairs may contain: first, mismatches: point qi' does not actually correspond to point pi; dynamic obstacles: such as pedestrians and vehicles, whose positions may change; second, repetitive structures: such as building walls and pillars, which are prone to mismatches; third, noise points: abnormal points caused by LiDAR measurement errors. Therefore, the present embodiment also incorporates pairing quality judgments, such as distance thresholds and normal vector consistency, to eliminate abnormal matches and perform quality screening on these point pairs to improve the robustness and accuracy of the subsequent least squares optimization.

[0047] Exemplarily, the following steps are used to determine the pairing quality: Step 1: Nearest neighbor matching (NN Matching).

[0048] For the transformed point cloud q i 'For each point in the local point cloud map, find its nearest neighbor point p i ; Use KD-Tree, FLANN, spatial hashing and other structures to accelerate the nearest neighbor search; You can choose k-nearest neighbors (k-NN) and then select the best matching point from them.

[0049] Step 2: Distance Threshold Filtering. Calculate the distance d = |q between point pairs. i '-p iIf the distance is greater than a preset threshold, the point pair is considered mismatched and is removed. The preset threshold is usually set based on sensor accuracy, map resolution, and application scenario.

[0050] Step 3: Normal Consistency Filtering. In the local point cloud map, each point p i Its normal vector can usually be estimated; at the same time, point q i 'The input point cloud can also estimate its normal vector; calculate the angle between the two normal vectors; if the angle is greater than a certain threshold (such as 60°), it means that the surface directions of the two points are inconsistent, which is a mismatch.

[0051] S212, using the nearest neighbor points of each point to fit a plane and a straight line.

[0052] For each point in a 3D point cloud, we compute its nearest neighbor set and use these neighboring points to fit local geometric structures, such as planes or lines. This fitting can be used for tasks such as surface normal estimation of point clouds.

[0053] For example, in a 3D point cloud, each point has a local neighborhood. i , find its k nearest neighbor points {p i1 、p i2 ,...,p ik}, and then use these nearest neighbor points to fit a local plane or line model.

[0054] For example, the input point cloud is a set of three-dimensional points P = {p1, p2, ..., p n}, each point p i =(x i ,y i , z i ). Construct a set of neighboring points: For each point p i , using KD-tree, Ball Tree, or Brute-force methods to find its k nearest neighbors. Optional parameters include the k value (e.g., k = 10 to 50), the search radius r, and the Euclidean distance or other distance metric. Plane fitting methods (such as PCA and least squares) use the neighborhood point set as samples and fit a plane equation. Principal component analysis (PCA) is used to calculate the covariance matrix of the point set and the smallest eigenvector is used as the normal vector. Alternatively, a least squares fit is used to minimize the sum of the squared distances from the point to the plane.

[0055] Line fitting methods (such as PCA and RANSAC): Also based on a set of neighborhood points, they can fit a line (e.g., in the reduced-dimensional space). PCA can be used to find the dominant direction of the point cloud distribution, which serves as the line direction.

[0056] S213, constructing distance error functions between a point and a plane, and between a point and a line in the input point cloud, respectively, to construct a least squares problem, and performing iterative optimization based on the constructed distance error functions to solve the optimal pose of the target object.

[0057] Furthermore, the distance error functions between a point in the input point cloud and a plane, and between a point and a line, include: e ei = d e × ( R × q i + t-p 0); e si = n T ( R × q i + t )+ d s ; in, e si represents the error function from the point in the input point cloud to the plane, e ei represents the error function from a point in the input point cloud to a line, q i represents the input lidar point cloud, R represents the rotation matrix, t represents the translation vector, d s represents the intercept, p 0 represents a point on the line, n represents the plane unit normal vector, d e Represents a direction vector of unit length.

[0058] Furthermore, iterative optimization is performed based on the constructed distance error function, including: a) Derivatively calculate the corresponding Jacobian matrix for the error function from the point to the plane in the input point cloud and the error function from the point to the line in the input point cloud.

[0059] =－n T × R × q i ∧ = n =－d e ∧ × R × q i ∧ = d e ∧ × J s = [ －n T × R × q i ∧ , n ] T J e = [ －d e ∧ × R × q i ∧ , d e ∧ ] T in, J s Represents the Jacobian matrix obtained by derivation of the error function from the input point cloud to the plane, J e Represents the Jacobian matrix obtained by derivation of the error function from the point to the line in the input point cloud, represents an antisymmetric matrix.

[0060] b) Based on the Jacobian matrix, the Gauss-Newton method is used to iteratively calculate and solve the optimal posture.

[0061] For example, the following formula is used to solve the optimal pose: H= J s T × J s + J e T × J e , e = e si + e ei H△X = e ( ) in, J s Represents the Jacobian matrix obtained by derivation of the error function from the input point cloud to the plane, J e It represents the Jacobian matrix obtained by derivation of the error function from the point to the line in the input point cloud, H represents the Hessian matrix, △X represents the pose increment, e represents the error function, e si represents the error function from a point in the input point cloud to a plane, e ei Represents the error function from a point in the input point cloud to a line.

[0062] S220: If the branch and bound relocalization thread preferentially outputs the optimal pose of the target object, the feature relocalization thread is interrupted.

[0063] The branch-and-bound relocalization thread is a relocalization thread with a large strategic search range and low sensitivity to the initial pose. For example, if the accuracy of a given initial pose is low, the feature relocalization thread in the multithreaded process will fail. The branch-and-bound relocalization thread will prioritize relocalization and succeed, interrupting the feature relocalization thread. This branch-and-bound relocalization thread is insensitive to the accuracy of the initial pose. As a result, the multithreaded relocalization in this application achieves faster positioning, greater robustness, and higher accuracy.

[0064] In one implementation, to facilitate a quick and accurate search for the optimal pose, the present embodiment first constructs a multiresolution sparse voxel map (MSVH) based on a spatial hash table. A MSVH is a voxel-based 3D map representation method that supports multiple levels of resolution and sparse storage. It is commonly used for efficient modeling of point cloud maps, providing high-precision, low-memory, and multi-granularity map representations to facilitate subsequent perception and positioning tasks.

[0065] The local point cloud map is constructed into a multiresolution sparse voxel map (MSVH) based on a spatial hash table. In other words, the local point cloud is mapped into a multiresolution voxel structure. This structure offers significant advantages in terms of spatial representation efficiency, memory utilization, and query speed.

[0066] For example, Figure 4 As shown in FIG, the method for constructing a multi-resolution sparse voxel map based on a spatial hash table includes the following steps: S221: Determine a local point cloud map based on the initial pose. In other words, the global point cloud map is intercepted based on the obtained initial pose to obtain a local point cloud map.

[0067] In scenarios such as autonomous driving, robot positioning and mapping, and augmented reality, when faced with large-scale point cloud maps, directly processing the global map will lead to problems such as waste of computing resources, high memory usage, and poor real-time performance. Therefore, the present application embodiment proposes: Obtain a global point cloud map and initial pose information; based on the acquired initial pose (such as the vehicle's initial position and pose), extract a local point cloud of the region of interest (ROI) from the global point cloud map. For example, extract a local point cloud within a specified range centered on the initial pose. The global point cloud map is typically a collection of 3D points, each containing coordinate information and potentially attributes such as color and intensity. The global point cloud is used to describe the position and pose (x, y, z, roll, pitch, yaw) of an object (such as a robot or vehicle) in 3D space to determine the spatial position and orientation for observation or manipulation. The initial pose is typically a 6-DOF pose: (x, y, z, roll, pitch, yaw).

[0068] Determine the region of interest (ROI) based on the initial pose, such as a spherical or cubic area with a set radius centered on the pose; perform spatial filtering on the global point cloud, extract the points within the region of interest, and obtain a local point cloud map (a set of points within a limited area).

[0069] S222: Convert each point in the local point cloud map to each spatial resolution level to obtain voxel coordinates of the corresponding spatial level.

[0070] Before step S222, it also includes defining multiple spatial resolution levels and voxel grid resolutions of each layer, which are used to represent different levels of fineness of three-dimensional space division. Specifically, in the multi-resolution sparse voxel map, the voxel grid resolution of each layer is ,in represents the minimum voxel grid size, Represents the level of the corresponding tree structure, each level contains an array of hash buckets.

[0071] S223 , using a spatial hash function to calculate a corresponding hash key based on each point and the corresponding voxel coordinates, and storing the corresponding voxel coordinates in a spatial hash table based on the hash key.

[0072] The spatial hash table is composed of one or more hash bucket arrays. Spatial Hash Table, an efficient spatial index structure, maps three-dimensional coordinates to one-dimensional hash keys, which is used to quickly query and manage non-empty voxels, realize fast insertion, search, deletion and other operations of voxels, and improve map processing efficiency. Each key in the spatial hash table corresponds to a voxel (or voxel block); each voxel can store point cloud statistical information in the space (such as the number of points, color mean, normal vector, etc.); it supports multiple levels of resolution (such as quadtree / octree structure), and low-resolution voxels contain summary information of high-resolution voxels. High resolution: used for precise matching; low resolution: used for fast coarse matching or global positioning. It can be understood that the multi-resolution sparse voxel map of the embodiment of the present application includes multiple resolution levels, which are used to support coarse search and fine matching respectively.

[0073] Convert each point in the local point cloud map to each spatial resolution level to obtain the corresponding voxel coordinates. According to each point and the corresponding voxel coordinates, use the spatial hash function to calculate the hash key. According to the hash key, the voxel coordinates are stored in the hash bucket array in the spatial hash table. The spatial hash table is used to store non-empty voxels to support fast access and update.

[0074] The 3D coordinates of the midpoint of the local point cloud map Convert to voxel coordinates And store it in the hash bucket array, and use the spatial hash function to calculate the index of the corresponding hash bucket (corresponding to the hash key), as shown below:

[0075]

[0076]

[0077]

[0078] in, Indicates the calculation of hash value, Indicates the l The size of the hash bucket of the layer, Indicates the l The voxel grid resolution of the layer, Represents the index of the hash bucket (corresponding to the hash key), Represents the modulo operation.

[0079] S224, constructing a multi-resolution sparse voxel map according to the spatial hash table.

[0080] Exemplarily, constructing a multi-resolution sparse voxel map according to the spatial hash table includes: Traverse the hash table of each spatial resolution level, extract non-empty voxels, and build a sparse voxel map; Establish parent-child relationships between voxels at different spatial resolution levels to form a multi-resolution sparse voxel map. Low-resolution voxels are the "parents" of high-resolution voxels; this can be organized using an octree or custom tree structure. A spatial hash table is the underlying data structure of the sparse voxel map. The multi-resolution sparse voxel map supports cross-level query, matching, and update operations for subsequent pose estimation, map matching, and other tasks.

[0081] The hash index is obtained by dividing the voxel coordinates v Mapping to a unique bucket allows for quick location retrieval when storing and querying voxel data. The input point cloud then computes the hash index and voxel coordinates for each point. The computed hash index (corresponding to the hash key) is used to find the corresponding voxel coordinates in the spatial hash table of the multi-resolution sparse voxel map and determine whether they are consistent. This allows for the calculation of a score threshold, which in turn determines the accuracy of the location based on the score.

[0082] In summary, the local point cloud in the local point cloud map in the embodiment of the present application is discretized into voxels and inserted into a spatial hash table for management; each hash key in the hash table corresponds to the location of a voxel. The spatial hash table only stores non-empty voxels, saving memory and improving query efficiency.

[0083] In one embodiment, in order to match the precise optimal pose, the present embodiment combines the branch and bound algorithm with the multi-resolution sparse voxel map to jointly determine the optimal pose. It can be understood that in step S220, the branch and bound relocalization thread prioritizes outputting the optimal pose of the target object, including: In the branch and bound relocalization thread, based on the set translation and rotation branch and bound strategy, each candidate pose is queried in the tree structure constructed based on the current pose space determined by the input point cloud, and the matching score of the candidate pose is calculated using the pre-built multi-resolution sparse voxel map, and the optimal pose is determined based on the matching score. Specifically, Figure 5 As shown, the following steps are included: S231, the pose of each point in the target pose space is represented by a tree structure and converted to a global coordinate system.

[0084] The pose of each point in the target pose space is represented using a tree structure. A pose transformation is performed on each node (candidate pose) in the tree structure to transform each candidate pose (node) into the global coordinate system. The target pose space is determined based on a preset translation and rotation search range; in other words, the pose space within the translation and rotation search range is represented using a tree structure. The pose space includes the input point cloud.

[0085] Specifically, the pose space within the translation and rotation search range is represented by a tree structure. To represent the hierarchical structure of the tree, each node's children represent a partition of the search space represented by that node, with each leaf node corresponding to a solution. Translations and rotations are used to continuously branch the search space, and non-leaf nodes define an upper bound for the child nodes after each branch.

[0086] In the translation branch, x, y, and z can customize the search distance based on the given initial pose, and each level has a parent node Can be branched into 8 child nodes, the search step is ; Roll and pitch settings in the rotation branch are used for small range search , where r stands for roll, p stands for pitch, and y stands for yaw. The search range of yaw can be customized according to the needs, and the maximum range supported is .

[0087] in, Indicates the maximum value, Indicates the minimum value, represents the search step corresponding to the translation branch, r i represents the minimum voxel grid size, l Indicates the level of the corresponding tree structure.

[0088] In the rotation branch, the search step length in each level is divided into the same size, and the search step length is ,in Expressed as the maximum scanning distance of the input laser point cloud: =

[0089] in, represents the search step corresponding to the rotation branch, Indicates the maximum value, Indicates the minimum value, Indicates the maximum scanning distance of the input laser point cloud.

[0090] S232, using a spatial hash function to calculate a hash key corresponding to the candidate pose of each node in the global coordinate system.

[0091] The current lidar point cloud obtained Perform posture The transformation is performed and the hash key of each laser point is calculated, and the score is calculated by matching it with the multi-resolution sparse voxel map based on the spatial hash table H.

[0092] S233 , matching corresponding voxels in the multi-resolution sparse voxel map according to the hash key, counting the number of successfully matched voxels, and generating a matching score for the candidate pose based on the number of voxels.

[0093] According to the hash key, the voxel corresponding to the hash key is searched in the multi-resolution sparse voxel map to see if it exists, and the number of successful matching points is counted to calculate the candidate pose matching score.

[0094] In step S234, the matching score is used as an evaluation function and input into the branch-and-bound search algorithm to evaluate and prune the nodes in the target pose space in the global coordinate system until the optimal pose is obtained. Specifically, the matching score is used to evaluate and prune the nodes in the search space until the optimal pose is obtained.

[0095] Set the initial match score threshold. For example, the initial match score threshold can be a percentage, such as 0.9 or 0.8, which represents the degree of similarity with the map environment.

[0096] In the embodiment of the present application, branch and bound is performed on translation and rotation, and a BFS (breadth-first search) method is used to search for the optimal solution 6-dimensional positioning pose. Specifically, the BFS method is used to search for the optimal pose solution with the maximum score. In the search, the score of each non-leaf node is the upper limit of the scores of its child nodes. If the child node score is lower than the upper limit, the branch is pruned. When the score is higher than the set matching score threshold, the corresponding optimal pose is obtained. The point cloud matching score is solved as follows:

[0097] Among them, c represents the parent node in the pose space, K represents the number of iterations, and H represents the spatial hash table. represents the pose, s k Represents the input laser point cloud.

[0098] The present invention sets the search range and local map according to the given initial value pose, adopts the spatial hash table method to manage the multi-resolution sparse voxel map, performs branch and bound on translation and rotation, and adopts the BFS method to search for the optimal solution 6-dimensional positioning pose; this strategy sets a large search range and high calculation time, but has low requirements on the accuracy of the initial value.

[0099] In one example, the embodiment of the present application does not limit the methods adopted by the feature relocalization thread and the branch and bound relocalization thread. Exemplarily, the feature relocalization thread includes but is not limited to the ICP relocalization thread, and the branch and bound relocalization thread includes but is not limited to the BBS relocalization thread. Then, after the initial pose is given, the two threads respectively calculate the matching score threshold. When the matching score of one of the threads meets the set conditional threshold, indicating that the relocalization is successful, the other thread will be immediately interrupted to output the relocalization pose. The score threshold of the ICP relocalization thread represents the Euclidean distance between the input lidar point cloud and the point cloud in the local point cloud map. For example, the conditional threshold is set to 0.1. The BBS relocalization thread uses the hash value of each laser point calculated by the input laser point cloud to match the multi-resolution sparse voxel map based on the spatial hash table to calculate the score. The score calculation formula score has been given, and its conditional threshold represents the degree of similarity with the map environment. For example, the conditional threshold is 0.9.

[0100] The present application also provides a relocation device. Exemplarily, the relocation device includes: A thread running module is used to synchronously run a plurality of relocalization threads based on a given initial pose and an input point cloud acquired in real time, so as to respectively perform relocalization calculations on the target object; wherein the plurality of relocalization threads include a feature relocalization thread and a branch and bound relocalization thread; The thread control module is configured to interrupt all other relocation threads in operation if any of the multiple relocation threads prioritizes outputting the optimal pose of the target object. It will be appreciated that the apparatus of this embodiment corresponds to the relocation method of the above-described embodiment, and the options in the above-described embodiment are also applicable to this embodiment, so they will not be described again here.

[0101] In one embodiment, the thread control module is specifically configured to: If the feature relocalization thread prioritizes outputting the optimal pose of the target object, the branch and bound relocalization thread is interrupted; If the branch and bound relocalization thread prioritizes outputting the optimal pose of the target object, the feature relocalization thread is interrupted.

[0102] Furthermore, the thread control module is further specifically used to: Convert each point in the input point cloud to a local point cloud map according to the initial pose, and find the nearest neighbor point corresponding to each point in the local point cloud map; Use the nearest neighbor points of each point to fit a plane and a straight line; The distance error functions between points and planes, and between points and lines in the input point cloud are constructed respectively to construct a least squares problem, and iterative optimization is performed based on the constructed distance error functions to solve the optimal pose of the target object.

[0103] Furthermore, the distance error functions between a point in the input point cloud and a plane, and between a point and a line, include: ; ; in, represents the error function from a point in the input point cloud to a plane, represents the error function from a point in the input point cloud to a line, q i represents the input lidar point cloud, R represents the rotation matrix, t represents the translation vector, represents the intercept, represents a point on a straight line, n represents the plane unit normal vector, Represents a direction vector of unit length.

[0104] Furthermore, iterative optimization is performed based on the constructed distance error function, including: The error function from the point to the plane in the input point cloud and the error function from the point to the line in the input point cloud are respectively derived to calculate the corresponding Jacobian matrix; based on the Jacobian matrix, the Gauss-Newton method is used to iteratively calculate and solve the optimal pose.

[0105] Furthermore, the following formula is used to solve the optimal pose:

[0106] in, Represents the Jacobian matrix obtained by derivation of the error function from the input point cloud to the plane, represents the Jacobian matrix obtained by derivation of the error function from the point to the line in the input point cloud, H represents the Hessian matrix, represents the pose increment, e represents the error function, represents the error function from a point in the input point cloud to a plane, Represents the error function from a point in the input point cloud to a line.

[0107] In one embodiment, the thread control module is specifically configured to: In the branch and bound relocalization thread, based on the set translation and rotation branch and bound strategy, each candidate pose is queried in the current pose space determined based on the input point cloud, and the matching score of the candidate pose is calculated using the pre-built multi-resolution sparse voxel map, and the optimal pose is determined based on the matching score.

[0108] Furthermore, the thread control module uses a translation and rotation branch-and-bound strategy to query each candidate pose in the current pose space determined based on the input point cloud, and uses a pre-built multi-resolution sparse voxel map to calculate the matching score of the candidate poses. The optimal pose is determined based on the matching score, including: The pose of each point in the target pose space is represented by a tree structure and converted to the global coordinate system; Use the spatial hash function to calculate the hash key of each node corresponding to the candidate pose in the global coordinate system; Match the corresponding voxels in the multi-resolution sparse voxel map according to the hash key, count the number of successfully matched voxels, and generate a matching score for the candidate pose based on the number of voxels; The matching score is used as an evaluation function and input into the branch and bound search algorithm to evaluate and prune the nodes in the target pose space in the global coordinate system until the optimal pose is obtained.

[0109] Furthermore, the multi-resolution sparse voxel map construction method includes: Determine the local point cloud map based on the initial pose; Convert each point in the local point cloud map to each spatial resolution level to obtain the voxel coordinates of the corresponding spatial level; According to each point and the corresponding voxel coordinates, the corresponding hash key is calculated using the spatial hash function, and the corresponding voxel coordinates are stored in the spatial hash table according to the hash key; A multi-resolution sparse voxel map is constructed based on the spatial hash table.

[0110] It can be understood that the device of this embodiment corresponds to the relocation method of the above embodiment, and the options in the above embodiment are also applicable to this embodiment, so they will not be described again here.

[0111] The present application also provides a vehicle. Exemplarily, the vehicle is positioned using the repositioning method of an embodiment of the present application.

[0112] It can be understood that the device of this embodiment corresponds to the relocation method of the above embodiment, and the options in the above embodiment are also applicable to this embodiment, so they will not be described again here.

[0113] The present application also provides a terminal device. Exemplarily, the terminal device includes a processor and a memory, wherein the memory stores a computer program, and the processor runs the computer program to enable the terminal device to execute the functions of the various modules in the above-mentioned relocation method or the above-mentioned relocation device.

[0114] The processor can be an integrated circuit chip with signal processing capabilities. The processor can be a general-purpose processor, including at least one of a central processing unit (CPU), a graphics processing unit (GPU), a network processor (NP), a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field-programmable gate array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components. The general-purpose processor can be a microprocessor or any conventional processor, etc., and can implement or execute the various methods, steps, and logic block diagrams disclosed in the embodiments of this application.

[0115] The memory may be, but is not limited to, random access memory (RAM), read-only memory (ROM), programmable read-only memory (PROM), erasable programmable read-only memory (EPROM), and electrically erasable programmable read-only memory (EEPROM). The memory is used to store computer programs, and the processor can execute the computer programs accordingly after receiving an execution instruction.

[0116] This application also provides a computer-readable storage medium for storing the computer program used in the terminal device. For example, the computer-readable storage medium may include, but is not limited to, various media capable of storing program code, such as a USB flash drive, a mobile hard drive, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0117] In the several embodiments provided in this application, it should be understood that the disclosed devices and methods can also be implemented in other ways. The device embodiments described above are merely schematic. For example, the flowcharts and structure diagrams in the accompanying drawings show the possible architectures, functions and operations of the devices, methods and computer program products according to the multiple embodiments of the present application. In this regard, each box in the flowchart or block diagram can represent a module, a program segment or a part of the code, and the module, program segment or a part of the code contains one or more executable instructions for implementing the specified logical functions. It should also be noted that in an alternative implementation, the functions marked in the box can also occur in an order different from that marked in the accompanying drawings. For example, two consecutive boxes 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 structure diagram and / or flowchart, and the combination of boxes in the structure diagram and / or flowchart, can be implemented using a dedicated hardware-based system that performs the specified function or action, or can be implemented using a combination of dedicated hardware and computer instructions.

[0118] In addition, the functional modules or units in the various embodiments of the present application can be integrated together to form an independent part, or each module can exist independently, or two or more modules can be integrated to form an independent part.

[0119] If the functions are implemented in the form of software function modules and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application, or the part that contributes to the prior art, or part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes a number of instructions for enabling a computer device (which can be a smart phone, personal computer, server, or network device, etc.) to execute all or part of the steps of the method described in each embodiment of the present application.

[0120] The above is only a specific implementation method of the present application, but the scope of protection of the present application is not limited thereto. Any technician familiar with this technical field can easily think of changes or replacements within the technical scope disclosed in this application, which should be covered by the scope of protection of the present application.

Claims

1. A relocation method, characterized in that: include: Based on a given initial pose and an input point cloud acquired in real time, multiple relocalization threads are synchronously run to perform relocalization calculations on the target object respectively; wherein the multiple relocalization threads include a feature relocalization thread and a branch and bound relocalization thread; If any one of the plurality of repositioning threads preferentially outputs the optimal posture of the target object, all other repositioning threads in operation are interrupted.

2. The relocation method according to claim 1, wherein: If any one of the plurality of relocation threads preferentially outputs the optimal posture of the target object, all other relocation threads in operation are interrupted, including: If the feature relocalization thread preferentially outputs the optimal pose of the target object, interrupting the branch and bound relocalization thread; If the branch and bound relocalization thread preferentially outputs the optimal pose of the target object, the feature relocalization thread is interrupted.

3. The relocation method according to claim 1, wherein: The feature relocalization thread prioritizes outputting the optimal pose of the target object, including: Converting each point in the input point cloud into a local point cloud map according to the initial pose, and searching for the nearest neighbor point corresponding to each point in the local point cloud map; Use the nearest neighbor points of each point to fit a plane and a straight line; Distance error functions between points in the input point cloud and the plane, and between points and the line are respectively constructed to construct a least squares problem, and iterative optimization is performed based on the constructed distance error functions to solve the optimal posture of the target object.

4. The relocation method according to claim 3, wherein: The distance error function between a point in the input point cloud and the plane, and between a point and the line, includes: e ei = d e ×（ R × q i + t-p 0）； e si = n T （ R × q i + t ）+ d s ； in, e si represents the error function from the point in the input point cloud to the plane, e ei represents the error function from a point in the input point cloud to a line, q i represents the input lidar point cloud, R represents the rotation matrix, t represents the translation vector, d s represents the intercept, p 0 represents a point on the line, n represents the plane unit normal vector, d e Represents a direction vector of unit length.

5. The relocation method according to claim 3, wherein: The iterative optimization based on the constructed distance error function includes: The error function from the point in the input point cloud to the plane and the error function from the point in the input point cloud to the line are respectively derived to calculate the corresponding Jacobian matrix; based on the Jacobian matrix, the Gauss-Newton method is used to iteratively calculate and solve to obtain the optimal posture.

6. The relocation method according to claim 5, wherein: The following formula is used to solve the optimal pose: H= J s T × J s + J e T × J e ， e = e si + e ei H△X = e in, J s represents the Jacobian matrix obtained by derivation of the error function from the point in the input point cloud to the plane, J e represents the Jacobian matrix obtained by derivation of the error function from the point to the line in the input point cloud, H represents the Hessian matrix, △X represents the pose increment, e represents the error function, e si represents the error function from the point in the input point cloud to the plane, e ei represents the error function from a point in the input point cloud to a line.

7. The relocation method according to claim 1, wherein: The branch and bound relocation thread preferentially outputs the optimal pose of the target object, including: In the branch and bound relocalization thread, based on the set translation and rotation branch and bound strategy, each candidate pose is queried in the current pose space determined based on the input point cloud, and a matching score of the candidate pose is calculated using a pre-built multi-resolution sparse voxel map, and an optimal pose is determined according to the matching score.

8. The relocation method according to claim 7, wherein: The translation and rotation branch and bound strategy based on the setting queries each candidate pose in the current pose space determined based on the input point cloud, calculates the matching score of the candidate pose using a pre-built multi-resolution sparse voxel map, and determines the optimal pose according to the matching score, including: The pose of each point in the target pose space is represented by a tree structure and converted to the global coordinate system; Calculating a hash key for each candidate pose corresponding to each node in the global coordinate system using a spatial hash function; Matching corresponding voxels in the multi-resolution sparse voxel map according to the hash key, counting the number of successfully matched voxels, and generating a matching score for the candidate pose based on the number of voxels; The matching score is used as an evaluation function and input into a branch and bound search algorithm to evaluate and prune nodes in the target pose space in the global coordinate system until the optimal pose is obtained.

9. The relocation method according to claim 7, wherein: The multi-resolution sparse voxel map construction method includes: Determine the local point cloud map based on the initial pose; Convert each point in the local point cloud map to each spatial resolution level to obtain voxel coordinates of the corresponding spatial level; A corresponding hash key is calculated using a spatial hash function according to each point and the corresponding voxel coordinates, and the corresponding voxel coordinates are stored in a spatial hash table according to the hash key; A multi-resolution sparse voxel map is constructed according to the spatial hash table.

10. A repositioning device, characterized in that: The device comprises: A thread running module is used to synchronously run a plurality of relocalization threads based on a given initial pose and an input point cloud acquired in real time, so as to respectively perform relocalization calculations on the target object; wherein the plurality of relocalization threads include a feature relocalization thread and a branch and bound relocalization thread; The thread control module is configured to interrupt all other running repositioning threads when any one of the plurality of repositioning threads outputs the optimal posture of the target object first.

11. A vehicle, characterized in that: The vehicle is positioned using the repositioning method according to any one of claims 1 to 9.

Images