Real-time odometry method, device, and storage medium

Abstract

This application relates to a real-time odometry method, device, and storage medium, belonging to the field of computer technology. The method includes: fitting a continuous sensor pose using a B-spline curve on a Lie group by acquiring point cloud data and inertial measurement unit (IMU) data; determining the sparse surface elements corresponding to the point cloud data; fitting a B-spline curve to the point cloud pose corresponding to the point cloud data, and using IMU data to constrain the fitting to obtain the B-spline curve; repeatedly matching adjacent sparse surface elements and optimizing the B-spline curve using a K-tree nearest neighbor algorithm until the difference between the final optimized loss function values ​​in the two matching and optimization processes is less than a preset loss function threshold, at which point convergence is determined. Real-time odometry is determined based on the converged B-spline curve, which can increase the robustness of instant localization and mapping (IMM) algorithms. This can solve the problem of ILM algorithm failure.

Description

Technical Field

[0001] This application relates to a real-time odometer method, device, and storage medium, belonging to the field of computer technology. Background Technology

[0002] With the adoption of multiple sensors and the development of Simultaneous Localization and Mapping (SLAM) technology, the demand for LiDAR-based real-time positioning is becoming increasingly significant in applications such as autonomous driving, robotics, building BIM models, fire protection, mapping, resource exploration, mining, and VR / AR. To achieve real-time positioning for various high-speed vehicles such as drones, robots, and automobiles, the corresponding algorithms must be able to process input radar point clouds and inertial measurement unit (IMU) data in real time.

[0003] Traditional odometry methods achieve odometry by using the original point cloud and point cloud geometric feature points to perform inter-frame point cloud registration and inter-frame pose transformation estimation.

[0004] However, traditional odometry methods estimate the pose between two point cloud frames by matching them. Due to noise, when the pose estimation between the two frames is not accurate enough, it may cause the estimated pose to drift, which may lead to the failure of the real-time localization and mapping algorithm. Summary of the Invention

[0005] This application provides a real-time odometry method, device, and storage medium, which can solve the problem of failure in real-time positioning and map building algorithms. This application provides the following technical solution:

[0006] A first aspect is a real-time odometer method, the method comprising:

[0007] Acquire point cloud data and inertial measurement unit (IMU) data; the IMU data includes acceleration, acceleration deviation, angular velocity, and angular velocity deviation.

[0008] Determine the sparse surface elements corresponding to the point cloud data;

[0009] The point cloud pose corresponding to the point cloud data is fitted, and the fitting is constrained using the inertial measurement unit data to obtain the first B-spline curve;

[0010] The first matching result is obtained by sequentially matching the sparse surface elements of two adjacent frames using the K-dimensional tree nearest neighbor algorithm.

[0011] Based on the first matching result, the first B-spline curve, and the preset loss function, the value of the first loss function is determined;

[0012] The first B-spline curve is iteratively optimized using a preset iterative algorithm to obtain the second B-spline curve;

[0013] The K-dimensional tree nearest neighbor algorithm is used to match the sparse surface elements of two adjacent frames sequentially to obtain the second matching result;

[0014] Based on the second matching result, the second B-spline curve, and the preset loss function, the value of the second loss function is determined;

[0015] If the difference between the second loss function value and the first loss function value is less than a preset loss function threshold, the real-time odometer is determined based on the second B-spline curve.

[0016] Optionally, the point cloud data includes N frames of point cloud data; determining the sparse surface elements corresponding to the point cloud data includes:

[0017] Obtain the point cloud data of the Yth frame; where Y is an integer greater than 0 and less than or equal to N;

[0018] Determine the sparse surface element of the Yth frame corresponding to the Yth frame point cloud data;

[0019] If Y is less than N, let Y = Y + 1, and execute the step of acquiring the point cloud data of the Yth frame.

[0020] Optionally, the step of sequentially matching sparse surface elements of two adjacent frames using the K-dimensional tree nearest neighbor algorithm to obtain a first matching result includes:

[0021] The sparse surface elements of frame P are matched sequentially with the sparse surface elements of frame P+1 to obtain the first matching result; P is a positive integer from 1 to N.

[0022] Optionally, determining the sparse surface elements of the Yth frame corresponding to the Yth frame point cloud data includes:

[0023] The mean point cloud value of each voxel in the voxel grid corresponding to the Y-th frame point cloud data is determined as the mean sparse surface element value.

[0024] The normal vector corresponding to the minimum eigenvalue of the point cloud variance of each voxel is determined and used as the sparse surface element normal vector.

[0025] The point cloud time corresponding to the point cloud data of the Yth frame is taken as the sparse surface time.

[0026] Optionally, before determining the point cloud mean of each voxel in the voxel grid corresponding to the Y-th frame point cloud data, the method further includes:

[0027] Determine the current measurement range of the lidar;

[0028] Obtain the size information of a single voxel;

[0029] The current measurement range is divided according to the size information to obtain the voxel grid.

[0030] Optionally, the first B-spline curve includes M control points and at least one time window; each time window includes the point cloud pose corresponding to H frames of point cloud data; H is a positive integer greater than 1 and less than or equal to N;

[0031] After determining the sparse surface elements of the Yth frame corresponding to the Yth frame point cloud data, the method further includes:

[0032] When the current time window includes the point cloud pose corresponding to H frame point cloud data, the time window is moved; where H is a positive integer greater than 1 and less than or equal to N;

[0033] The point cloud poses in the current time window are saved in chronological order and output as odometry results.

[0034] Optionally, the preset loss function includes a first loss function, a second loss function, and a third loss function; the first loss function is used to determine the matching loss function value corresponding to sparse surface element matching; the second loss function is used to determine the acceleration loss function value; and the third loss function is used to determine the angular velocity loss function value.

[0035] The step of determining the value of the first loss function based on the first matching result, the first B-spline curve, and the preset loss function includes:

[0036] Based on the first matching result and the first loss function, the value of the matching loss function is determined;

[0037] Based on the first B-spline curve and the second loss function, determine the value of the acceleration loss function;

[0038] Based on the first B-spline curve and the third loss function, determine the value of the angular velocity loss function;

[0039] The first loss function value is obtained by adding the matching loss function value, the acceleration loss function value, and the angular velocity loss function value.

[0040] Optionally, the iterative optimization of the first B-spline curve using a preset iterative algorithm includes:

[0041] The control point, the angular velocity deviation, and the acceleration deviation are iteratively optimized using the preset iterative algorithm.

[0042] In a second aspect, an electronic device is provided, the device including a processor and a memory; the memory stores a program, the program being loaded and executed by the processor to implement the real-time odometer method provided in the first aspect.

[0043] Thirdly, a computer-readable storage medium is provided, wherein a program is stored in the storage medium, and the program, when executed by a processor, is used to implement the real-time odometer method provided in the first aspect.

[0044] The beneficial effects of this application include at least the following: acquiring point cloud data and inertial measurement unit (IMU) data; determining the sparse surface elements corresponding to the point cloud data; fitting the point cloud pose corresponding to the point cloud data and using IMU data to constrain the fitting to obtain a first B-spline curve; sequentially matching adjacent sparse surface elements in two frames using a K-tree nearest neighbor algorithm to obtain a first matching result; determining a first loss function value based on the first matching result, the first B-spline curve, and a preset loss function; iteratively optimizing the first B-spline curve using a preset iterative algorithm to obtain a second B-spline curve; sequentially matching adjacent sparse surface elements in two frames using a K-tree nearest neighbor algorithm to obtain a second matching result; determining a second loss function value based on the second matching result, the second B-spline curve, and the preset loss function; and determining the real-time odometry based on the second B-spline curve when the difference between the second loss function value and the first loss function value is less than a preset loss function threshold. This can solve the problem of failure in instant localization and map building algorithms. By extracting sparse surface element information from the input point cloud for multi-frame sparse surface element matching, real-time odometry estimation of LiDAR and inertial measurement unit is achieved. B-spline fitting is used to fit the global pose within a time window. Even if the pose estimation between two frames fails, the constraint of the B-spline curve can minimize the drift of the instantaneous localization and mapping algorithm. At the same time, the B-spline curve is considered to have converged when the difference between the final optimized loss function values ​​in the two matching and optimization processes is less than the preset loss function threshold. The real-time odometry is determined based on the converged B-spline curve, which can increase the robustness of the instantaneous localization and mapping algorithm and thus avoid the failure of the instantaneous localization and mapping algorithm.

[0045] In addition, since the number of extracted sparse surface elements is significantly reduced compared to the original point cloud data, the power consumption and computing resource requirements of the algorithm can be reduced.

[0046] The above description is only an overview of the technical solution of this application. In order to better understand the technical means of this application and to implement it in accordance with the contents of the specification, the preferred embodiments of this application are described in detail below with reference to the accompanying drawings. Attached Figure Description

[0047] Figure 1This is a flowchart of a real-time odometer method provided in one embodiment of this application;

[0048] Figure 2 This is a block diagram of a real-time odometer device provided in one embodiment of this application;

[0049] Figure 3 This is a block diagram of an electronic device provided in one embodiment of this application. Detailed Implementation

[0050] The specific embodiments of this application will be described in further detail below with reference to the accompanying drawings and examples. The following examples are used to illustrate this application, but are not intended to limit the scope of this application.

[0051] First, let me introduce some of the terms used in this application.

[0052] B-spline curves are a special form of representation in numerical analysis, a subfield of mathematics. They are linear combinations of B-spline basis curves. They were created by Isaac Jacob Schoenberg.

[0053] Lie Group: A Lie group is a relatively simple algebraic structure with only one operation; it is a fundamental structure that can be used to build many other algebraic systems.

[0054] In mathematics, a Lie group is a real or complex manifold with group structure, and the addition and inverse operations in the group are analytic mappings in the manifold. Lie groups play a very important role in mathematical analysis, physics, and geometry.

[0055] Figure 1 This is a flowchart of a real-time odometer method provided in one embodiment of this application, which includes at least the following steps:

[0056] Step 101: Acquire point cloud data and inertial measurement unit (IMU) data; wherein, the IMU data includes acceleration, acceleration deviation, angular velocity, and angular velocity deviation.

[0057] Step 102: Determine the sparse surface elements corresponding to the point cloud data.

[0058] In this embodiment, the point cloud data includes N frames of point cloud data. Accordingly, determining the sparse surface elements corresponding to the point cloud data includes: acquiring the Y-th frame of point cloud data; Y is an integer greater than 0 and less than or equal to N; determining the Y-th frame of sparse surface elements corresponding to the Y-th frame of point cloud data; if Y is less than N, let Y = Y + 1, and execute the step of acquiring the Y-th frame of point cloud data.

[0059] Specifically, determining the sparse surface element corresponding to the Y-th frame point cloud data includes: determining the point cloud mean of each voxel in the voxel grid corresponding to the Y-th frame point cloud data, as the sparse surface element mean; determining the normal vector corresponding to the minimum eigenvalue of the point cloud variance of each voxel, as the sparse surface element normal vector; and taking the point cloud time corresponding to the Y-th frame point cloud data as the sparse surface element time.

[0060] In this embodiment, the sparse surface element has a position, a normal vector, and a generation time. In the LiDAR coordinate system, the current measurement space, i.e., the cube corresponding to the maximum measurement range of the LiDAR that can rotate 360 ​​degrees up, down, left, and right, is used as the cube for voxel partitioning. According to the length of a single voxel, the cube is divided into individual voxels to obtain a voxel mesh. The mean value of the point cloud of each voxel is used as the mean value of the sparse surface element, the normal vector corresponding to the minimum eigenvalue of the variance is used as the normal vector of the sparse surface element, and the point cloud time is used as the sparse surface element time.

[0061] At this point, the sparse surface element is located in the lidar coordinate system. The global pose of the lidar can be fitted by B-spline curves, thereby transforming the coordinates of the sparse surface element to the global coordinate system. Then, the K-Nearest Neighbour (KNN) algorithm can be used to match the sparse surface elements of adjacent frames sequentially.

[0062] The global coordinate system refers to the coordinate system constructed with the position of the first frame of point cloud data acquired by the lidar as the origin.

[0063] Specifically, before determining the point cloud mean of each voxel in the voxel grid corresponding to the point cloud data of the Yth frame, the process also includes: determining the current measurement range of the lidar; obtaining the size information of a single voxel; and dividing the current measurement range according to the size information to obtain the voxel grid.

[0064] Step 103: Fit the point cloud pose corresponding to the point cloud data, and use inertial measurement unit data to constrain the fitting to obtain the first B-spline curve.

[0065] The first B-spline curve includes at least one control point and at least one time window; each time window includes the point cloud pose corresponding to H frames of point cloud data; H is a positive integer greater than 1 and less than or equal to N.

[0066] In this embodiment, the Lie group SE(3) is used as the space where the continuous global pose is located, and the time range of the B-spline curve fitting is [0, tmax), where tmax is determined by the number of control points and the time interval between adjacent control points.

[0067] In this embodiment, the global pose at any time t can be represented by the following formula:

[0068]

[0069] In the formula, B i，k The coefficients are B-spline coefficients and can be used as known coefficients; Q i The control points of the B-spline curve determine the state of the global pose of the B-spline curve fitting in the Lie group SE(3), and are also a component of the subsequent optimization variables; k represents the number of control points; i is used to indicate the i-th control point.

[0070] Using B-spline curves, the pose p(t), angular velocity w(t), and acceleration a(t) corresponding to the lidar coordinate system at a certain time t can be calculated by differentiation. The sparse surface elements can be transformed to the global coordinate system using the pose p(t) corresponding to time t, so that adjacent sparse surface elements can be matched sequentially using the K-dimensional tree nearest neighbor algorithm.

[0071] Because the time window range for B-spline curve fitting is [0, tmax), the number of control points cannot be set too high to achieve real-time performance. Therefore, in this embodiment, the B-spline curve is moved for sliding window optimization. Each time window has a timestamp tB. The timestamp of the point cloud is converted into the B-spline curve time t, which can be expressed by the following formula:

[0072] t = tP - tB

[0073] In the formula, tB is the timestamp corresponding to the time window; tP is the point cloud time, i.e., the sparse surface time.

[0074] If the time of the sparse element corresponding to the current sparse element exceeds the maximum representation time of the time window, in other words, if the time window can only include the pose corresponding to H frame point cloud data, and the current time window already includes the pose corresponding to H frame point cloud data, then move the time window and save the point cloud pose that is longest away from the current sparse element in chronological order as the odometry output result.

[0075] Specifically, after determining the sparse surface element corresponding to the Y-th frame point cloud data, the process further includes: moving the time window if the current time window includes the point cloud pose corresponding to the H-th frame point cloud data; H is a positive integer greater than 1 and less than or equal to N; saving the point cloud pose in the current time window in chronological order and outputting it as the odometry result.

[0076] In this embodiment, the angular velocity and acceleration of the global pose fitted by the B-spline curve are limited by the inertial measurement unit data between frames, so as to make the obtained pose estimation more accurate. At the same time, the optimization parameters include the angular velocity deviation and acceleration deviation of the inertial measurement unit data.

[0077] Among them, the optimization parameters refer to the parameters that need to be iteratively optimized through a preset iterative algorithm. The optimization function also includes control points. The first B-spline curve is iteratively optimized through a preset iterative algorithm, including: iterative optimization of control points, angular velocity deviation and acceleration deviation through a preset iterative algorithm.

[0078] Optionally, the preset iterative algorithm includes a numerical gradient algorithm or an analytical gradient algorithm.

[0079] The first B-spline curve is iteratively optimized using a preset iterative algorithm, including iterative optimization of control points, angular velocity deviations, and acceleration deviations.

[0080] Step 104: The K-dimensional tree nearest neighbor algorithm is used to match the sparse surface elements of two adjacent frames in turn to obtain the first matching result.

[0081] Specifically, the K-dimensional tree nearest neighbor algorithm is used to match the sparse surface elements of two adjacent frames in turn to obtain the first matching result, including: matching the sparse surface elements of the Pth frame with the sparse surface elements of the P+1th frame in turn to obtain the first matching result; P is a positive integer from 1 to N-1 in turn.

[0082] For example, if the number of frames N in a time window is 4, then the K-dimensional tree nearest neighbor algorithm is used to match the first and second frames, the second and third frames, and the third and fourth frames in sequence.

[0083] In this embodiment, the nearest neighbor search space of the K-dimensional tree nearest neighbor algorithm is a 7-dimensional vector space of sparse facets, including position, normal vector, and generation time. The matching output is all matching results, with the matching form being that a sparse facet in frame P corresponds to another sparse facet in frame P+1.

[0084] Step 105: Determine the value of the first loss function based on the first matching result, the first B-spline curve, and the preset loss function.

[0085] In this embodiment, the preset loss function includes a first loss function, a second loss function, and a third loss function; the first loss function is used to determine the matching loss function value corresponding to sparse surface element matching; the second loss function is used to determine the acceleration loss function value; and the third loss function is used to determine the angular velocity loss function value.

[0086] Specifically, the first loss function can be expressed by the following formula:

[0087]

[0088] In the formula, and These represent the positions of sparse surface element a and sparse surface element b in the matched surface elements, respectively. This represents the rotational portion of the global pose of the frame containing sparse surface element a. This represents the translation of the global pose of the frame containing sparse surface element a. This represents the rotational portion of the global pose of the frame containing the sparse surface element b. n represents the translation of the global pose of the frame containing sparse element b. ab This is the average normal vector.

[0089] Where, n ab It can be expressed by the following formula:

[0090] n ab =(n a +n b ) / |n a +n b ||

[0091] In the formula, n a Let n represent the normal vector of sparse surface element a. b Let b represent the normal vector of the sparse surface element.

[0092] The second loss function can be expressed by the following formula:

[0093]

[0094] The third loss function can be expressed by the following formula:

[0095]

[0096] In the formula, a T and ω T These are the acceleration and angular velocity from the inertial measurement unit data, b a and b ω ω represents the acceleration and angular velocity deviations corresponding to the inertial measurement unit, g is the gravitational acceleration, R, t, and ω correspond to the B-spline interpolation rotation, translation, and angular velocity of the frame containing the sparse surface element, respectively; T is the generation time of the sparse surface element.

[0097] Based on the first matching result, the first B-spline curve, and the preset loss function, the first loss function value is determined, including: determining the matching loss function value based on the first matching result and the first loss function; determining the acceleration loss function value based on the first B-spline curve and the second loss function; determining the angular velocity loss function value based on the first B-spline curve and the third loss function; and adding the matching loss function value, the acceleration loss function value, and the angular velocity loss function value to obtain the first loss function value.

[0098] Step 106: Iteratively optimize the first B-spline curve using a preset iterative algorithm to obtain the second B-spline curve.

[0099] Step 107: The K-dimensional tree nearest neighbor algorithm is used to match the sparse surface elements of two adjacent frames in turn to obtain the second matching result.

[0100] Step 108: Determine the value of the second loss function based on the second matching result, the second B-spline curve, and the preset loss function.

[0101] Since the acceleration and angular velocity deviations corresponding to the control points and inertial measurement units of the B-spline curve change after each optimization, the global pose of the corresponding frame also changes. Consequently, the matching results obtained by matching sparse surface elements between adjacent frames using the K-tree nearest neighbor algorithm also change. Therefore, we repeat the matching and optimization of the B-spline curve until the difference between the final optimized loss function values ​​in the two matching and optimization processes is less than a preset loss function threshold, at which point convergence is considered achieved.

[0102] Step 109: If the difference between the second loss function value and the first loss function value is less than the preset loss function threshold, determine the real-time odometer based on the second B-spline curve.

[0103] If the difference between the second loss function value and the first loss function value is greater than or equal to the preset loss function threshold, then the B-spline curve matching and optimization continue.

[0104] In summary, the real-time odometry method provided in this embodiment acquires point cloud data and inertial measurement unit (IMU) data; determines the sparse surface elements corresponding to the point cloud data; fits the point cloud pose corresponding to the point cloud data and uses IMU data to constrain the fitting to obtain a first B-spline curve; sequentially matches adjacent sparse surface elements in two frames using a K-tree nearest neighbor algorithm to obtain a first matching result; determines a first loss function value based on the first matching result, the first B-spline curve, and a preset loss function; iteratively optimizes the first B-spline curve using a preset iterative algorithm to obtain a second B-spline curve; sequentially matches adjacent sparse surface elements in two frames using a K-tree nearest neighbor algorithm to obtain a second matching result; determines a second loss function value based on the second matching result, the second B-spline curve, and the preset loss function; and determines the real-time odometry based on the second B-spline curve when the difference between the second loss function value and the first loss function value is less than a preset loss function threshold. This method can solve the problem of failure in instant localization and map building algorithms. By extracting sparse surface element information from the input point cloud for multi-frame sparse surface element matching, real-time odometry estimation for LiDAR and inertial measurement units is achieved. B-spline fitting is used to fit the global pose within a time window. Even if pose estimation fails between two frames, the constraints of the B-spline curve minimize the drift of the instantaneous localization and mapping (SMR) algorithm. Furthermore, the B-spline curve is considered converged when the difference between the final optimized loss function values ​​from two consecutive matching and optimization processes is less than a preset loss function threshold. Determining the real-time odometry based on the converged B-spline curve increases the robustness of the SMR algorithm, thereby preventing its failure.

[0105] In addition, since the number of extracted sparse surface elements is significantly reduced compared to the original point cloud data, the power consumption and computing resource requirements of the algorithm can be reduced.

[0106] Figure 2 This is a block diagram of a real-time odometer device provided in one embodiment of this application. The device includes at least the following modules: a data acquisition module 210, a surface element determination module 220, a first matching module 230, a pose fitting module 240, a first determination module 250, a curve optimization module 260, a second matching module 270, a second determination module 280, and a third determination module 290.

[0107] The data acquisition module 210 is used to acquire point cloud data and inertial measurement unit (IMU) data; the IMU data includes acceleration, acceleration deviation, angular velocity, and angular velocity deviation.

[0108] The surface element determination module 220 is used to determine the sparse surface elements corresponding to the point cloud data;

[0109] The first matching module 230 is used to match the sparse surface elements of two adjacent frames sequentially using the K-dimensional tree nearest neighbor algorithm to obtain the first matching result;

[0110] The pose fitting module 240 is used to fit the pose of the point cloud data, and to use the inertial measurement unit data to constrain the fitting to obtain the first B-spline curve.

[0111] The first determining module 250 is used to determine the value of the first loss function based on the first matching result, the first B-spline curve, and the preset loss function;

[0112] The curve optimization module 260 is used to iteratively optimize the first B-spline curve using a preset iterative algorithm to obtain the second B-spline curve;

[0113] The second matching module 270 is used to match the sparse surface elements of two adjacent frames sequentially using the K-dimensional tree nearest neighbor algorithm to obtain the second matching result;

[0114] The second determining module 280 is used to determine the value of the second loss function based on the second matching result, the second B-spline curve, and the preset loss function;

[0115] The third determining module 290 is used to determine the real-time odometer based on the second B-spline curve when the difference between the second loss function value and the first loss function value is less than a preset loss function threshold.

[0116] For relevant details, please refer to the above method implementation examples.

[0117] It should be noted that the real-time odometer device provided in the above embodiments is only illustrated by the division of the above functional modules when performing real-time odometer calculations. In practical applications, the above functions can be assigned to different functional modules as needed, that is, the internal structure of the real-time odometer device can be divided into different functional modules to complete all or part of the functions described above. In addition, the real-time odometer device and the real-time odometer method embodiments provided in the above embodiments belong to the same concept, and the specific implementation process can be found in the method embodiments, which will not be repeated here.

[0118] Figure 3 This is a block diagram of an electronic device provided in one embodiment of this application. The device includes at least a processor 301 and a memory 302.

[0119] Processor 301 may include one or more processing cores, such as a quad-core processor or an octa-core processor. Processor 301 may be implemented using at least one hardware form selected from DSP (Digital Signal Processing), FPGA (Field-Programmable Gate Array), and PLA (Programmable Logic Array). Processor 301 may also include a main processor and a coprocessor. The main processor, also known as a CPU (Central Processing Unit), is used to process data in the wake-up state; the coprocessor is a low-power processor used to process data in the standby state. In some embodiments, processor 301 may integrate a GPU (Graphics Processing Unit), which is responsible for rendering and drawing the content to be displayed on the screen. In some embodiments, processor 301 may also include an AI (Artificial Intelligence) processor, which is used to handle computational operations related to machine learning.

[0120] The memory 302 may include one or more computer-readable storage media, which may be non-transitory. The memory 302 may also include high-speed random access memory and non-volatile memory, such as one or more disk storage devices or flash memory devices. In some embodiments, the non-transitory computer-readable storage media in the memory 302 are used to store at least one instruction, which is executed by the processor 301 to implement the real-time odometer method provided in the method embodiments of this application.

[0121] In some embodiments, the electronic device may also optionally include: a peripheral device interface and at least one peripheral device. The processor 301, memory 302, and peripheral device interface can be connected via a bus or signal line. Each peripheral device can be connected to the peripheral device interface via a bus, signal line, or circuit board. Indicatively, peripheral devices include, but are not limited to: radio frequency circuitry, a touch display screen, audio circuitry, and a power supply.

[0122] Of course, electronic devices may also include fewer or more components, and this embodiment does not limit this.

[0123] Optionally, this application also provides a computer-readable storage medium storing a program that is loaded and executed by a processor to implement the real-time odometer method of the above method embodiments.

[0124] Optionally, this application also provides a computer product including a computer-readable storage medium storing a program that is loaded and executed by a processor to implement the real-time odometer method of the above method embodiments.

[0125] The technical features of the above embodiments can be combined in any way. For the sake of brevity, 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.

[0126] The embodiments described above are merely illustrative of several implementation methods of this application, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the invention patent. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these all fall within the protection scope of this application. Therefore, the protection scope of this patent application should be determined by the appended claims.

Claims

1. A real-time odometer method, characterized in that, The method includes: Acquire point cloud data and inertial measurement unit (IMU) data; the IMU data includes acceleration, acceleration deviation, angular velocity, and angular velocity deviation. Determine the sparse surface elements corresponding to the point cloud data; The point cloud pose corresponding to the point cloud data is fitted, and the fitting is constrained using the inertial measurement unit data to obtain the first B-spline curve; The first matching result is obtained by sequentially matching the sparse surface elements of two adjacent frames using the K-dimensional tree nearest neighbor algorithm. Based on the first matching result, the first B-spline curve, and the preset loss function, the value of the first loss function is determined; The first B-spline curve is iteratively optimized using a preset iterative algorithm to obtain the second B-spline curve; The K-dimensional tree nearest neighbor algorithm is used to match the sparse surface elements of two adjacent frames sequentially to obtain the second matching result; Based on the second matching result, the second B-spline curve, and the preset loss function, the value of the second loss function is determined; If the difference between the second loss function value and the first loss function value is less than a preset loss function threshold, the real-time odometer is determined based on the second B-spline curve.

2. The method according to claim 1, characterized in that, The point cloud data includes N frames of point cloud data; determining the sparse surface elements corresponding to the point cloud data includes: Obtain the point cloud data of the Yth frame; where Y is an integer greater than 0 and less than or equal to N; Determine the sparse surface element of the Yth frame corresponding to the Yth frame point cloud data; If Y is less than N, let Y = Y + 1, and execute the step of acquiring the point cloud data of the Yth frame.

3. The method according to claim 1, characterized in that, The first matching result obtained by sequentially matching sparse surface elements of two adjacent frames using the K-dimensional tree nearest neighbor algorithm includes: The sparse surface elements of frame P are matched sequentially with the sparse surface elements of frame P+1 to obtain the first matching result; P is a positive integer from 1 to N.

4. The method according to claim 2, characterized in that, Determining the sparse surface elements corresponding to the Y-th frame point cloud data includes: The mean point cloud value of each voxel in the voxel grid corresponding to the Y-th frame point cloud data is determined as the mean sparse surface element value. The normal vector corresponding to the minimum eigenvalue of the point cloud variance of each voxel is determined and used as the sparse surface element normal vector. The point cloud time corresponding to the point cloud data of the Yth frame is taken as the sparse surface time.

5. The method according to claim 4, characterized in that, Before determining the point cloud mean of each voxel in the voxel grid corresponding to the Y-th frame point cloud data, the method further includes: Determine the current measurement range of the lidar; Obtain the size information of a single voxel; The current measurement range is divided according to the size information to obtain the voxel grid.

6. The method according to claim 2, characterized in that, The first B-spline curve includes at least one control point and at least one time window; each time window includes the point cloud pose corresponding to H frames of point cloud data; H is a positive integer greater than 1 and less than or equal to N; After determining the sparse surface elements of the Yth frame corresponding to the Yth frame point cloud data, the method further includes: When the current time window includes the point cloud pose corresponding to H frame point cloud data, the time window is moved; where H is a positive integer greater than 1 and less than or equal to N; The point cloud poses in the current time window are saved in chronological order and output as odometry results.

7. The method according to claim 1, characterized in that, The preset loss function includes a first loss function, a second loss function, and a third loss function; the first loss function is used to determine the matching loss function value corresponding to sparse surface element matching; the second loss function is used to determine the acceleration loss function value; and the third loss function is used to determine the angular velocity loss function value. The step of determining the value of the first loss function based on the first matching result, the first B-spline curve, and the preset loss function includes: Based on the first matching result and the first loss function, the value of the matching loss function is determined; Based on the first B-spline curve and the second loss function, determine the value of the acceleration loss function; Based on the first B-spline curve and the third loss function, determine the value of the angular velocity loss function; The first loss function value is obtained by adding the matching loss function value, the acceleration loss function value, and the angular velocity loss function value.

8. The method according to claim 7, characterized in that, The iterative optimization of the first B-spline curve using a preset iterative algorithm includes: The control point, the angular velocity deviation, and the acceleration deviation are iteratively optimized using the preset iterative algorithm.

9. An electronic device, characterized in that, The device includes a processor and a memory; the memory stores a program that is loaded and executed by the processor to implement the real-time odometer method as described in any one of claims 1 to 8.

10. A computer-readable storage medium, characterized in that, The storage medium stores a program that, when executed by a processor, is used to implement the real-time odometer method as described in any one of claims 1 to 8.

Images