Continuous Time Pose Estimation Method Based on Multi-Resolution Hash Map

Abstract

This solution provides a continuous-time pose estimation method based on a multi-resolution hash map, including: S1, obtaining the laser radar data of the mobile device at the current moment; S2, splitting each frame of laser radar data and converting it into global coordinate system laser radar data under the global coordinate system; S3, extracting the key points of each frame of laser radar data in the global coordinate system laser radar data, constructing the point surface residual based on the hash map of all key points of the current frame laser radar data, constructing the position consistency residual and speed consistency residual of the current frame laser radar data based on the previous frame laser radar data and constructing an optimization problem, and obtaining the pose corresponding to each frame of laser radar data based on the solution of the optimization problem; S4, updating the multi-resolution hash map based on the pose of each frame of laser radar data, and removing the out-of-field data in the multi-resolution hash map, thereby realizing a fast, efficient and low-latency continuous-time pose estimation method.

Description

Technical Field

[0001] The present application relates to the technical field of positioning of mobile devices, and in particular to a continuous-time pose estimation method based on a multi-resolution hash map. Background Art

[0002] With the rapid development of autonomous driving technology in recent years, the perception and positioning of mobile devices have become particularly important. The perception and positioning of mobile devices need to rely on the constructed environment map to feedback accurate posture.

[0003] Currently, the positioning technology of mobile devices either relies on expensive combined inertial navigation equipment or is based on multi-line lidar. Combined inertial navigation equipment is expensive and it is difficult to provide accurate positioning in areas where satellite signals are blocked. Therefore, multi-line lidar technology is receiving more and more attention.

[0004] In the positioning based on multi-line LiDAR, the traditional methods mainly use the ICP algorithm (Iterative Closest Point) or feature extraction and pose estimation methods based on LOAM (Lidar Odometry and Mapping). However, these methods need to store map information through the KD-Tree data structure and estimate the pose by constructing point-line and point-surface residuals. This means that as the mobile device moves, the positioning of the multi-line LiDAR needs to frequently rebuild and access the KD-Tree data structure, which not only leads to low computational efficiency, but also greatly consumes system resources. More importantly, this type of traditional method does not fully consider the continuity of the movement of the mobile device. As the running time increases, the accumulated error will gradually increase, and it will eventually be difficult to meet the mobile device's needs for fast and accurate positioning. Summary of the invention

[0005] The present application provides a continuous-time pose estimation method based on a multi-resolution hash map, which splits the laser data and uses a multi-resolution hash map to manage the data and determine the distribution of the adaptive hash map. The continuous-time motion assumption of the laser data is used to construct continuous-time constraints and pose and velocity consistency constraints, thereby realizing a fast, efficient, and low-latency continuous-time pose estimation method.

[0006] In a first aspect, an embodiment of the present application provides a continuous-time pose estimation method based on a multi-resolution hash map, comprising the following steps: S1, obtaining laser radar data: obtaining laser radar data of a mobile device at the current moment; S2, performing a first processing on the laser radar data: splitting each frame of laser radar data to obtain multiple frames of laser radar data, and converting the laser radar data to be processed into global coordinate system laser radar data in a global coordinate system; S3, performing a second processing on the global coordinate system laser radar data: extracting the laser point of each frame of laser radar data in the global coordinate system laser radar data, determining the hash map index corresponding to the current laser point in the multi-resolution hash map based on the laser point, constructing a point-surface residual based on the hash map of all laser points of the current frame laser radar data, constructing a position consistency residual and a speed consistency residual of the current frame laser radar data based on the previous frame laser radar data, constructing an optimization problem based on the point-surface residual, the position consistency residual and the speed consistency residual, and obtaining the pose corresponding to each frame of laser radar data based on the solution of the optimization problem; S4, performing a third processing on the global coordinate system laser radar data: updating the multi-resolution hash map based on the pose of each frame of laser radar data, and removing out-of-field data in the multi-resolution hash map.

[0007] In a second aspect, an embodiment of the present application provides a readable storage medium, in which a computer program is stored. The computer program includes a program code for controlling a process to execute a process, and the process includes the continuous-time pose estimation method based on a multi-resolution hash map.

[0008] The main contributions and innovations of the present invention are as follows:

[0009] 1. Efficient data management and fast access: The present invention innovatively proposes a data management method of a multi-resolution hash map. Compared with the traditional data management structure, this method can access the nearest neighbor data with extremely efficient time complexity, greatly improving the efficiency of data retrieval and processing, and providing a solid data processing foundation for application scenarios with high real-time requirements.

[0010] 2. Adaptive hash map access strategy: Through a unique design, the present invention realizes adaptive determination of the hash map to be accessed based on the distance information of the laser point. This adaptive strategy can intelligently switch between hash maps of different resolutions, effectively reducing data storage and computational complexity while ensuring data accuracy, and significantly enhancing the system's ability to process complex and changing environmental data.

[0011] 3. High-precision continuous-time pose estimation: The continuous-time pose estimation method provided by the present invention cleverly uses the continuous-time motion hypothesis to fundamentally improve the accuracy of pose estimation. More importantly, the method can simultaneously perform distortion processing during the estimation process, effectively overcoming the data deviation problem caused by environmental factors or the device's own motion, and providing more accurate and reliable pose information for mobile devices.

[0012] 4. Optimization of environmental adaptability based on logical constraints: By building logical constraints with pose consistency and speed consistency, the pose estimation method based on logical constraints proposed in this invention enables mobile devices to operate more stably and accurately in complex and diverse environments. This method significantly improves the adaptability of mobile devices to different environments and successfully broadens its application scope in various scenarios, whether it is a complex indoor space or an outdoor dynamic environment, it can achieve accurate pose estimation and reliable operation.

[0013] 5. Optimize laser data processing and improve real-time performance: This invention uses a unique laser data splitting method to effectively reduce the data delay of pose estimation, thereby greatly increasing the release frequency of pose estimation. This optimization enables mobile devices to respond to environmental changes more promptly, achieve fast and accurate decision-making and control in dynamic environments, and effectively promote the real-time development of related application fields.

[0014] Details of one or more embodiments of the present application are set forth in the following drawings and description to make other features, objects, and advantages of the present application more readily apparent. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] The drawings described herein are used to provide a further understanding of the present application and constitute a part of the present application. The illustrative embodiments of the present application and their descriptions are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

[0016] Figure 1 It is a logical schematic diagram of the continuous-time pose estimation method based on multi-resolution hash maps provided by this scheme.

[0017] Figure 2 is a schematic diagram for splitting lidar data.

[0018] Figure 3 It is a schematic diagram for constructing an optimization problem.

[0019] Figure 4 is a schematic diagram of updating a multi-resolution hash map.

[0020] Figure 5 This is a schematic diagram of the effect of splitting lidar data.

[0021] Figure 6It is a schematic diagram of the hardware structure of an electronic device according to an embodiment of the present application. DETAILED DESCRIPTION

[0022] Exemplary embodiments will be described in detail herein, examples of which are shown in the accompanying drawings. When the following description refers to the drawings, the same numbers in different drawings represent the same or similar elements unless otherwise indicated. The implementations described in the following exemplary embodiments do not represent all implementations consistent with one or more embodiments of this specification. Instead, they are merely examples of devices and methods consistent with some aspects of one or more embodiments of this specification as detailed in the appended claims.

[0023] It should be noted that: in other embodiments, the steps of the corresponding method are not necessarily performed in the order shown and described in this specification. In some other embodiments, the steps included in the method may be more or less than those described in this specification. In addition, a single step described in this specification may be decomposed into multiple steps for description in other embodiments; and multiple steps described in this specification may be combined into a single step for description in other embodiments.

[0024] Embodiment 1

[0025] This scheme provides a continuous-time pose estimation method based on a multi-resolution hash map, comprising the following steps:

[0026] S1. Get LiDAR data:

[0027] Get the laser radar data of the mobile device at the current moment;

[0028] S2. Perform the first processing on the laser radar data:

[0029] Each frame of laser radar data is split to obtain multiple frames of laser radar data, and the laser radar data to be processed is converted into global coordinate system laser radar data in a global coordinate system;

[0030] S3, performing a second process on the global coordinate system laser radar data:

[0031] Extract the key points of each frame of lidar data in the global coordinate system lidar data, determine the hash map index corresponding to the current key point in the multi-resolution hash map based on the key points, construct the point-surface residual based on the hash map of all key points of the current frame lidar data, construct the position consistency residual and speed consistency residual of the current frame lidar data based on the previous frame lidar data, construct an optimization problem based on the point-surface residual, position consistency residual and speed consistency residual, and obtain the pose corresponding to each frame of lidar data based on the solution of the optimization problem;

[0032] S4. Perform the third processing on the global coordinate system laser radar data:

[0033] Update the multi-resolution hash map based on the pose of each frame of lidar data, and remove the out-of-view data in the multi-resolution hash map.

[0034] The continuous-time pose estimation method based on multi-resolution hash maps proposed in this scheme splits the lidar data generated by traditional multi-line lidar to reduce the data delay of pose estimation and thus increase the release frequency of pose estimation; it uses the data management method of multi-resolution hash maps to manage it, thereby achieving The time complexity of accessing the nearest neighbor LiDAR data is ; and this scheme can adaptively determine the hash map of the resolution that needs to be accessed according to the distance information of the laser point; this scheme uses the continuous time motion assumption to improve the accuracy of pose estimation, and can also perform distortion processing; this scheme performs logical constraints based on position consistency residuals and speed consistency residuals to improve the adaptability of mobile devices to the environment and broaden the application scenarios.

[0035] The "continuous-time motion assumption" mentioned in this scheme is not only reflected in the fact that this scheme builds position consistency and speed consistency based on the lidar data of the previous and next frames, but also mainly in that this scheme interpolates the laser posture of the current laser point according to the timing information of each laser point. The processed frame of lidar data contains a series of continuously distributed laser points at different times, and each laser point corresponds to a laser posture that changes with time, which is approximated as a series of continuous motions, called continuous-time motion.

[0036] In addition, it should be noted that simple point-surface residual constraints may not converge in some degraded scenes or scenes of intense motion due to insufficient constraints or inaccurate initial values ​​(such as oscillation of iterative optimization results), and thus fail to meet the continuity of actual continuous motion. Figure 3 As shown, the actual movement is continuous. In order to ensure the continuity of continuous movement, it is necessary to keep the end position of the previous frame of LiDAR data and the starting position of the current frame of LiDAR data as consistent as possible. This solution hopes that the starting position of the current frame needs to be close to the end position of the previous frame data, so a pose consistency constraint is added here. Similarly, in a very short interval, the speed change is also asymptotic, and the speed of the previous frame and the speed of the current frame will not change much, so a speed consistency constraint is generated; by giving different weight factors to the three types of constraints, namely point-surface residual, speed consistency, and pose consistency, the final optimization method can be guaranteed to converge quickly and improve the adaptability of mobile devices to the environment.

[0037] In step S1, Get LiDAR Data:

[0038] Get the LiDAR data of the mobile device at the current moment and cache it in memory.

[0039] In some embodiments, the lidar data comes from lidar data generated by a multi-line lidar.

[0040] The step of “S2, performing a first process on the laser radar data” further includes: S2-1: splitting each frame of laser radar data to obtain multiple frames of laser radar data:

[0041] like Figure 5 As shown, this solution splits each frame of lidar data into multiple frames of lidar data according to the timestamp of the lidar data. The advantage of this is that it can reduce the data delay of pose estimation.

[0042] Specifically, each frame of laser radar data is split into at least two frames of laser radar data according to the timestamp of the laser radar data. Figure 2 As shown, the embodiment of this solution splits each frame of laser radar data into 2 frames of laser radar data: Split into and ,Will Split into and , each frame of split lidar data is used as a single frame of data for subsequent processing and analysis.

[0043] S2-2: And convert the laser radar data to be processed into the global coordinate system laser radar data in the global coordinate system:

[0044] This solution converts the coordinate system of the laser radar data to be processed after splitting each frame of laser radar data. This solution converts the laser radar data in the laser coordinate system into global coordinate system laser radar data in the global coordinate system according to the position and posture of the laser point in the laser radar data to be processed.

[0045] In actual scenarios, when a mobile device (such as a robot, self-driving vehicle, etc.) is in motion, the lidar will continue to collect lidar data. Since the mobile device is in motion, it is necessary to determine the position of these lidar data in space when processing each frame of lidar data.

[0046] In some embodiments, this scheme initializes the starting pose of the starting timestamp and the ending pose of the ending timestamp of each frame of lidar data to be processed by the uniform motion model of the mobile device, and then interpolates the laser pose of the current laser point according to the timing information of each laser point, and converts the laser pose of all laser points into the global coordinate system to obtain the global coordinate system lidar data of the current frame lidar data.

[0047] The timing information of each laser point in each frame of split lidar data is calculated based on the start timestamp and end timestamp of the current frame lidar data and the timestamp of the current laser point. The specific calculation method is as follows:

[0048] ;

[0049] Where i is the index of the current frame lidar data, is the timestamp of the ith laser point, is the starting timestamp of the current frame of lidar data, The end timestamp of the current frame of lidar data.

[0050] The starting point pose of the starting point timestamp of each frame of lidar data is expressed as: ; The endpoint pose of the endpoint timestamp of each frame of lidar data is expressed as: ; This scheme assumes that the mobile device is moving in a uniform straight line, and then the starting pose and the end pose of the current frame of lidar data can be initialized according to the pose of the previous frame of lidar data and the pose of the next frame of lidar data.

[0051] The calculation method of interpolating the laser pose of the current laser point based on the timing information of each laser point is as follows:

[0052] ;

[0053] ;

[0054] ;

[0055] in Represents the timing information of the current laser point, Indicates the laser pose of the current laser point. and Data representing the starting point position, and Data representing the end point pose.

[0056] The conversion method of the coordinate system of each laser point is as follows:

[0057] ;

[0058] in and is the data in the laser pose of the current laser point i, represents the current laser point i in the laser coordinate system L, Represents the current laser point i in the global coordinate system w.

[0059] In the step of "Step S3, perform a second process on the global coordinate system LiDAR data": This solution manages the global coordinate system LiDAR data based on a multi-resolution hash map. The multi-resolution hash map refers to dividing the space into spatial regions of different resolutions based on the basic idea of ​​a hash table, and mapping the laser points to the corresponding spatial regions through a hash function. In actual operation, for points closer to the laser origin, a higher-resolution hash map may be used to accurately represent local details; while for points farther away, a lower-resolution hash map is used to reduce data storage and computational complexity while ensuring a certain degree of accuracy. Compared with the traditional KD-Tree data structure that stores map information, the multi-resolution hash map can provide strong support for accurate pose calculation through efficient data management and adaptive resolution determination, enabling the entire system to process LiDAR data more quickly and accurately and estimate the pose of the mobile device.

[0060] The specific step S3 includes the following steps:

[0061] S3-1. Extract the key points of each frame of lidar data in the global coordinate system lidar data:

[0062] The current frame of the global coordinate system lidar data is randomly reordered, and each frame of the randomly reordered lidar data is downsampled with a voxel grid to extract some laser points as key points. The advantage of this is that it ensures that the laser points after voxel grid sampling will not be affected by the sequence of laser points, and the data sampled by the voxel grid can meet the random distribution as much as possible.

[0063] This solution uses voxel grid sampling to traverse the lidar data in sequence, calculates the sampling grid index to which it belongs, and only takes the first data belonging to the current grid as the key point data after sampling. Subsequent data belonging to this grid will be discarded; the sampled data is used for subsequent calculations.

[0064] It should be explained that the data obtained by downsampling the original laser points in the lidar data to a certain resolution is called a key point.

[0065] S3-2. Determine the hash map index corresponding to the current key point in the multi-resolution hash map based on the key point, and construct the point surface residual based on the hash map of all key points of the current frame lidar data:

[0066] The hash map index of each key point in the multi-resolution hash map is calculated according to the distance from the key point to the laser origin. The neighboring points of the current key point are searched in the corresponding hash map and the weight and normal vector are calculated. The point-surface residual of the current key point is calculated based on the weight and normal vector. The point-surface residuals of all key points are summarized to obtain the point-surface residuals of the current frame lidar data.

[0067] More specifically, based on the distance between the key point and the laser origin Calculate search radius ;

[0068] ;

[0069] in are the preset parameters, namely, the maximum radius, the minimum radius and the distance index;

[0070] Then determine the corresponding hash map index based on the search radius , the calculation formula is as follows:

[0071] ;

[0072] Where distance is the numerical index closest to the first parameter in the second parameter. All resolutions for a multiresolution hash map.

[0073] Then calculate in the corresponding hash map, traverse the neighboring space to obtain the neighboring points of the current key point , calculate the covariance of neighboring points , decomposition covariance Get the normal vector and weight , where the normal vector is the minimum value of the covariance decomposition, and the weight is the ratio of the difference between the largest and second largest eigenvalues ​​to the minimum value.

[0074] The formula for calculating the point-surface residual of the current key point based on the weight and normal vector is as follows:

[0075] ;

[0076] in is the point closest to the neighboring points; is the current key point; is the current laser point’s pose, which is calculated by the start pose and the end pose; is the normal vector of the current key point, is the weight of the current key point.

[0077] The formula for traversing all key points to obtain the point surface residual of the current frame lidar data is as follows: ;

[0078] Where p() refers to the i A function is used to calculate the point-surface residual, Ks refers to the key point, and s is the number of key points.

[0079] S3-3, constructing the position consistency residual of the current frame of lidar data based on the previous frame of lidar data;

[0080] The position consistency residual is constructed by using the end position of the previous frame of lidar data and the starting position of the current frame of lidar data. The calculation formula is as follows:

[0081] ;

[0082] in is the end point position of the previous frame of lidar data, The starting point of the current frame of lidar data.

[0083] In order to ensure the continuity of continuous motion, it is necessary to ensure that the end position of the previous frame of lidar data and the starting position of the current frame of lidar data are consistent as much as possible.

[0084] S3-4. Construct the velocity consistency residual of the current frame of lidar data based on the previous frame of lidar data:

[0085] The speed consistency residual is constructed by using the speed of the previous frame of lidar data and the speed of the current frame of lidar data. The calculation formula is as follows: ;

[0086] in are the starting and ending positions of the previous frame of lidar data, The starting and ending positions of the current frame of lidar data.

[0087] In order to ensure the continuity of continuous motion, the speed of the mobile device does not change much in a short period of time.

[0088] S3-5, construct an optimization problem based on point-surface residuals, position consistency residuals and velocity consistency residuals, and solve the optimization problem to obtain the position and posture of each frame of lidar data, such as Figure 3 As shown, the formula for constructing the optimization problem in this scheme is as follows:

[0089] ;

[0090] in and is the set weight factor;

[0091] Solving the optimization problem, the pose corresponding to each frame of lidar data is .

[0092] S4. Perform the third processing on the global coordinate system laser radar data:

[0093] S4-1: Traverse all laser points in the current frame of lidar data, calculate the index of each laser point in the multi-resolution hash map based on the pose of the current frame of lidar data, and update the multi-resolution hash map.

[0094] like Figure 4 As shown, for each laser point, the hash map index of the current laser point in the multi-resolution hash map is calculated. If there is no laser point data in the current hash map index, the laser point data is inserted; if there is laser point data in the current hash map index and the existing laser point data does not exceed a certain threshold, the current laser point data is inserted.

[0095] It should be noted that the certain threshold here refers to the data of only the laser point data stored in the voxel corresponding to each hash index, such as 20.

[0096] S4-2: Calculate the Euclidean distance between the data corresponding to each hash map index of the current multi-resolution hash map and the pose of the current frame lidar data. If the Euclidean distance exceeds a certain threshold, remove the data in the multi-resolution hash map.

[0097] Embodiment 2

[0098] This embodiment also provides an electronic device, referring to Figure 6 , including a memory 404 and a processor 402, wherein the memory 404 stores a computer program, and the processor 402 is configured to run the computer program to execute the steps in any of the above-mentioned embodiments of the continuous-time pose estimation method based on a multi-resolution hash map.

[0099] Specifically, the processor 402 may include a central processing unit (CPU), or an application specific integrated circuit (ASIC), or may be configured to implement one or more integrated circuits of the embodiments of the present application.

[0100] Among them, the memory 404 may include a large capacity memory 404 for data or instructions. For example, but not limitation, the memory 404 may include a hard disk drive (HDD), a floppy disk drive, a solid state drive (SSD), a flash memory, an optical disk, a magneto-optical disk, a magnetic tape, or a universal serial bus (USB) drive or a combination of two or more of these. In appropriate cases, the memory 404 may include a removable or non-removable (or fixed) medium. In appropriate cases, the memory 404 may be inside or outside the data processing device. In a specific embodiment, the memory 404 is a non-volatile memory. The memory 404 can be used to store or cache various data files that need to be processed and / or used for communication, as well as possible computer program instructions executed by the processor 402.

[0101] The processor 402 implements any one of the continuous-time pose estimation methods based on multi-resolution hash maps in the above embodiments by reading and executing computer program instructions stored in the memory 404.

[0102] Optionally, the electronic device may further include a transmission device 406 and an input / output device 408 , wherein the transmission device 406 is connected to the processor 402 , and the input / output device 408 is connected to the processor 402 .

[0103] The transmission device 406 can be used to receive or send data via a network. Specific examples of the above-mentioned network may include a wired or wireless network provided by a communication provider of the electronic device. In one example, the transmission device includes a network adapter (Network Interface Controller, referred to as NIC), which can be connected to other network devices through a base station so as to communicate with the Internet. In one example, the transmission device 406 can be a radio frequency (Radio Frequency, referred to as RF) module, which is used to communicate with the Internet wirelessly.

[0104] The input / output device 408 is used to input or output information. In this embodiment, the input information may be laser radar data, etc., and the output information may be estimated position and posture, etc.

[0105] Optionally, in this embodiment, the processor 402 may be configured to perform the following steps through a computer program:

[0106] S1. Get LiDAR data:

[0107] Get the laser radar data of the mobile device at the current moment;

[0108] S2. Perform the first processing on the laser radar data:

[0109] Each frame of laser radar data is split to obtain multiple frames of laser radar data, and the laser radar data to be processed is converted into global coordinate system laser radar data in a global coordinate system;

[0110] S3, performing a second process on the global coordinate system laser radar data:

[0111] Extract the key points of each frame of lidar data in the global coordinate system lidar data, determine the hash map index corresponding to the current key point in the multi-resolution hash map based on the key points, construct the point-surface residual based on the hash map of all key points of the current frame lidar data, construct the position consistency residual and speed consistency residual of the current frame lidar data based on the previous frame lidar data, construct an optimization problem based on the point-surface residual, position consistency residual and speed consistency residual, and obtain the pose corresponding to each frame of lidar data based on the solution of the optimization problem;

[0112] S4. Perform the third processing on the global coordinate system laser radar data:

[0113] Update the multi-resolution hash map based on the pose of each frame of lidar data, and remove the out-of-view data in the multi-resolution hash map.

[0114] It should be noted that the specific examples in this embodiment can refer to the examples described in the above embodiments and optional implementation modes, and this embodiment will not be described in detail here.

[0115] In general, various embodiments may be implemented in hardware or dedicated circuits, software, logic, or any combination thereof. Some aspects of the invention may be implemented in hardware, while other aspects may be implemented in firmware or software that may be executed by a controller, microprocessor, or other computing device, but the invention is not limited thereto. Although various aspects of the invention may be shown and described as block diagrams, flow charts, or using some other graphical representation, it should be understood that, as non-limiting examples, the boxes, devices, systems, techniques, or methods described herein may be implemented in hardware, software, firmware, dedicated circuits or logic, general-purpose hardware or controllers or other computing devices, or some combination thereof.

[0116] Embodiments of the present invention can be implemented by computer software, which is executable by a data processor of a mobile device, such as in a processor entity, or implemented by hardware, or implemented by a combination of software and hardware. Computer software or programs (also referred to as program products) including software routines, applets and / or macros can be stored in any device readable data storage medium, and they include program instructions for performing specific tasks. Computer program products can include one or more computer executable components configured to perform embodiments when the program is running. One or more computer executable components can be at least one software code or a part thereof. In addition, at this point, it should be noted that any box of the logic flow in the figure can represent a program step, or interconnected logic circuits, boxes and functions, or a combination of program steps and logic circuits, boxes and functions. Software can be stored in physical media such as memory chips or storage blocks implemented in processors, magnetic media such as hard disks or floppy disks, and optical media such as, for example, DVDs and data variants thereof, CDs. Physical media are non-transient media.

[0117] Those skilled in the art should understand that the technical features of the above embodiments may be arbitrarily combined. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0118] The above embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the present application. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the present application shall be subject to the attached claims.

Claims

1. A continuous-time pose estimation method based on multi-resolution hash maps, characterized in that: The following steps are involved: S1. Get LiDAR data: Get the laser radar data of the mobile device at the current moment; S2. Perform the first processing on the laser radar data: Each frame of laser radar data is split to obtain multiple frames of laser radar data, and the laser radar data to be processed is converted into global coordinate system laser radar data in a global coordinate system; S3, performing a second process on the global coordinate system laser radar data: Extract the key points of each frame of lidar data in the global coordinate system lidar data, determine the hash map index corresponding to the current key point in the multi-resolution hash map based on the key points, construct the point-surface residual based on the hash map of all key points of the current frame lidar data, construct the position consistency residual and speed consistency residual of the current frame lidar data based on the previous frame lidar data, construct an optimization problem based on the point-surface residual, position consistency residual and speed consistency residual, and obtain the pose corresponding to each frame of lidar data based on the solution of the optimization problem; S4. Perform the third processing on the global coordinate system laser radar data: Update the multi-resolution hash map based on the pose of each frame of lidar data, and remove the out-of-view data in the multi-resolution hash map.

2. The continuous-time pose estimation method based on multi-resolution hash maps according to claim 1, characterized in that: The uniform motion model of the mobile device is used to initialize the starting pose of the starting timestamp and the ending pose of the ending timestamp of each frame of lidar data to be processed, and then the laser pose of the current laser point is interpolated according to the timing information of each laser point. The laser poses of all laser points are converted to the global coordinate system to obtain the global coordinate system lidar data of the current frame lidar data.

3. The continuous-time pose estimation method based on multi-resolution hash maps according to claim 2, characterized in that: The timing information of each laser point in each frame of split lidar data is calculated based on the start timestamp and end timestamp of the current frame of lidar data and the timestamp of the current laser point. The calculation method is as follows: ； Where i is the index of the current frame lidar data, is the timestamp of the ith laser point, is the starting timestamp of the current frame of lidar data, The end timestamp of the current frame of lidar data.

4. The continuous-time pose estimation method based on multi-resolution hash maps according to claim 1, characterized in that: The current frame of the global coordinate system lidar data is randomly reordered, and each frame of the randomly reordered lidar data is downsampled with a voxel grid to extract some laser points as key points.

5. The continuous-time pose estimation method based on multi-resolution hash maps according to claim 1, characterized in that: The hash map index of each key point in the multi-resolution hash map is calculated according to the distance from the key point to the laser origin. The neighboring points of the current key point are searched in the corresponding hash map and the weight and normal vector are calculated. The point-surface residual of the current key point is calculated based on the weight and normal vector. The point-surface residuals of all key points are summarized to obtain the point-surface residuals of the current frame lidar data.

6. The method for continuous-time pose estimation based on multi-resolution hash maps according to claim 1, characterized in that: The position consistency residual is constructed by using the end position of the previous frame of lidar data and the starting position of the current frame of lidar data. The calculation formula is as follows: ； in is the end point position of the previous frame of lidar data, The starting point of the current frame of lidar data.

7. The method for continuous-time pose estimation based on multi-resolution hash maps according to claim 1, characterized in that: The speed consistency residual is constructed by using the speed of the previous frame of lidar data and the speed of the current frame of lidar data. The calculation formula is as follows: ; in are the starting and ending positions of the previous frame of lidar data, The starting and ending positions of the current frame of lidar data.

8. The method for continuous-time pose estimation based on multi-resolution hash maps according to claim 1, characterized in that: The formula for constructing the optimization problem is as follows: ； in and is the set weight factor; solving the optimization problem, the pose corresponding to each frame of lidar data is .

9. The method for continuous-time pose estimation based on multi-resolution hash maps according to claim 1, characterized in that: Traverse all laser points in the current frame lidar data, calculate the index of each laser point in the multi-resolution hash map based on the pose of the current frame lidar data, and update the multi-resolution hash map; calculate the Euclidean distance between the data corresponding to each hash map index of the current multi-resolution hash map and the pose of the current frame lidar data, and remove the data in the multi-resolution hash map if the Euclidean distance exceeds a certain threshold.

10. A readable storage medium, characterized in that: The readable storage medium stores a computer program, which includes a program code for controlling a process to execute a process, and the process includes a continuous-time pose estimation method based on a multi-resolution hash map according to any one of claims 1 to 9.

Images