Laser odometer pose estimation method and device, computing equipment, vehicle and storage medium
By introducing the first and second attributes into the target raster map, incremental updates are realized when the NDT algorithm frame matches the map, redundant rasterization problem is solved, and real-time and calculation of laser odometers are improved efficiency.
Patent Information
- Application Number
- CN202310623700.5
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2023-05-30
- Publication Date
- 2024-12-03
AI Technical Summary
In the prior art, when using the NDT algorithm to match frames and maps, it is necessary to rasterize the overall point cloud map and recalculate the normal distribution parameters, resulting in redundant time consumption and affecting the real-time nature of the laser odometer.
By introducing the first and second attributes in the target raster map, it is used to characterize the raster parameters of the added points and the cache queue of the points to be added, only the target raster updated incrementally is calculated to avoid the overall point Redundant rasterization and parameter updates for cloud maps.
The real-time performance of the laser odometer position estimation calculation method is improved, the calculation amount and processing amount are reduced, and the normal distribution parameter update frequency after point cloud matching is reduced.
Smart Images

Figure CN119063719A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of autonomous driving positioning, and in particular to a laser odometer posture estimation method, device, computing equipment, vehicle, and storage medium. Background Art
[0002] LiDAR Odometry (LO) is a track recursive positioning method that uses laser radar for pose estimation. Its core is the point cloud matching algorithm, which is to make the matching degree of two frames of point clouds as high as possible by minimizing the cost function, and estimate the relative pose between the coordinate systems of the two frames of point clouds. The NDT (Normal Distributions Transform) algorithm is a point cloud matching algorithm widely used in the field of autonomous driving positioning. The core idea of the NDT algorithm is to rasterize the target point cloud, use normal distribution to describe the distribution of point clouds in the grid, convert the current frame point cloud to the target point cloud grid, and optimize the pose to maximize the joint probability density of the current frame point cloud in the target point cloud grid.
[0003] At present, there are two main implementation schemes for laser odometry. One is to match the current frame point cloud with the historical frame point cloud to obtain the position increment, and then add the current frame point cloud to the point cloud map for incremental mapping. This is called frame to frame (scan to scan) matching; the other is to match the current frame point cloud with the point cloud map to obtain the position increment, and then add the current frame point cloud to the point cloud map for incremental mapping. This is called frame to map (scan to map) matching. Since the point cloud map is composed of multiple frame point cloud maps, it is denser than a single frame point cloud. The dense point cloud is more accurate in describing geometric features, and the normal distribution parameters fitted using the dense point cloud are also more accurate. Matching the current frame with the point cloud map makes full use of the point cloud of the historical frame to obtain better point cloud map quality. Therefore, laser odometry with frame to map matching has been widely used in engineering.
[0004] In the laser odometry that uses the NDT algorithm for frame-map matching, the point cloud map must be rasterized as a whole and the normal distribution parameters of each grid must be recalculated each time a match is made. However, only a portion of the grids in the point cloud map actually have points added. For other grids, rasterization is redundant, which will result in additional time consumption and affect the real-time performance of the laser odometry. Summary of the invention
[0005] The embodiments of the present invention provide a laser odometer pose estimation method, apparatus, computing device, vehicle, storage medium and computer program product to solve the redundancy problem of the prior art that the entire point cloud map needs to be rasterized each time when the NDT algorithm is used for frame and map matching, thereby improving the real-time performance of the laser odometer that uses the NDT algorithm for frame and map matching.
[0006] In a first aspect, an embodiment of the present invention provides a laser odometer pose estimation method, comprising:
[0007] Obtain the current frame point cloud, and determine the target grid to be calculated for the first attribute based on the initial pose value and the target grid map corresponding to the current frame point cloud, wherein each grid of the target grid map includes a first attribute and a second attribute, wherein the first attribute is a grid parameter characterized by the points added to the corresponding grid, and the second attribute is a cache queue for caching the points to be added in the corresponding grid, wherein the points to be added are the points in the previous frame point cloud that fall into the corresponding grid;
[0008] A pose estimate corresponding to the current frame point cloud is determined according to the first attribute and the second attribute of the target grid.
[0009] In a second aspect, an embodiment of the present invention provides a laser odometer posture estimation device, comprising:
[0010] A target grid determination module is used to obtain the current frame point cloud, and determine the target grid to be calculated for the first attribute based on the initial value of the pose corresponding to the current frame point cloud and the target grid map, wherein each grid of the target grid map includes a first attribute and a second attribute, wherein the first attribute is a grid parameter characterized by the points added to the corresponding grid, and the second attribute is a cache queue for caching the points to be added in the corresponding grid, and the points to be added are the points that fall into the corresponding grid in the previous frame point cloud;
[0011] A pose estimation module is used to determine a pose estimation corresponding to a point cloud of a current frame according to a first attribute and a second attribute of the target grid.
[0012] In a third aspect, an embodiment of the present invention provides a computing device, including:
[0013] A memory for storing executable instructions; and
[0014] A processor is used to execute executable instructions stored in a memory, wherein the executable instructions, when executed by the processor, implement the steps of the laser odometer pose estimation method of the first aspect.
[0015] In a fourth aspect, an embodiment of the present invention provides a vehicle, comprising the laser odometer posture estimation device of the second aspect or the computing device of the third aspect.
[0016] In a fifth aspect, an embodiment of the present invention provides a storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the laser odometer pose estimation method of the first aspect described above.
[0017] In a sixth aspect, an embodiment of the present invention provides a computer program product, comprising a computer program stored on a non-volatile computer-readable storage medium, wherein the computer program comprises program instructions, which, when executed by a computer, cause the computer to execute the steps of the laser odometer pose estimation method of the first aspect.
[0018] The method provided by the embodiment of the present invention uses the first attribute and the second attribute to describe each grid in the target grid map, thereby realizing the separation of the newly added points in each grid and the incremental update of the grid parameters, so that when the NDT algorithm is used for frame and map matching, it is not necessary to rasterize the entire point cloud map and recalculate the normal distribution parameters of all grids each time the current frame point cloud is acquired. Instead, the first attribute and the second attribute and the current frame point cloud are used to determine the target grid, and only the first attribute and the second attribute of the target grid in the target grid map are incrementally updated, and the first attribute and the second attribute of the incrementally updated target grid are used to calculate the pose estimation of the current point cloud, thereby avoiding redundant rasterization and incremental update of the point cloud map, and improving the real-time performance of the laser odometer pose estimation algorithm. Furthermore, since the points in the current frame point cloud are usually disordered, there may be multiple points falling into the same grid, and there may also be no points falling into a certain grid. Therefore, the embodiment of the present invention sets the second attribute so that the points to be added in each grid can be temporarily stored through the cache queue. This not only avoids the situation where the grid parameters are updated every time a point is added to the grid, reducing the processing volume in the single-frame point cloud processing, but also enables the incremental update of the first attribute of the target grids participating in the cost function construction only when it is determined that the corresponding grid participates in the cost function construction, avoiding the updating of the normal distribution parameters after each point cloud match, and reducing the amount of calculation in the multi-frame point cloud processing. Therefore, the method of the present invention can also significantly reduce the amount of calculation of the laser odometer pose estimation algorithm, and further improve the real-time performance of the algorithm. BRIEF DESCRIPTION OF THE DRAWINGS
[0019] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings required for use in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other accompanying drawings can be obtained based on these accompanying drawings without paying any creative work.
[0020] Figure 1A flowchart of a laser odometer pose estimation method according to an embodiment of the present invention;
[0021] Figure 2 This is a method flow chart of step S11 in the laser odometer pose estimation method according to one embodiment of the present invention;
[0022] Figure 3 This is a method flow chart of step S12 in the laser odometer pose estimation method according to one embodiment of the present invention;
[0023] Figure 4 A flowchart of a laser odometer pose estimation method according to another embodiment of the present invention;
[0024] Figure 5 This is a method flow chart of step S13 in a laser odometer pose estimation method according to another embodiment of the present invention;
[0025] Figure 6 A schematic diagram of a target grid map when the target grid map is updated in step S13 of the laser odometer pose estimation method according to an embodiment of the present invention;
[0026] Figure 7 The overall flow chart of the laser odometer pose estimation method according to one embodiment of the present invention;
[0027] Figure 8 This is a principle block diagram of a laser odometer posture estimation device according to one embodiment of the present invention;
[0028] Fig. 9 This is a principle block diagram of a laser odometer position and posture estimation device according to another embodiment of the present invention;
[0029] Fig.10 A functional block diagram of a computing device according to an embodiment of the present invention;
[0030] Fig.11 It is a schematic structural diagram of an embodiment of an electronic device of the present invention. DETAILED DESCRIPTION
[0031] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.
[0032] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments may be combined with each other.
[0033] The present invention may be described in the general context of computer-executable instructions executed by a computer, such as program modules. Generally, program modules include routines, programs, objects, components, data structures, etc. that perform specific tasks or implement specific abstract data types. The present invention may also be practiced in distributed computing environments where tasks are performed by remote processing devices connected through a communication network. In a distributed computing environment, program modules may be located in local and remote computer storage media, including storage devices.
[0034] In the present invention, "module", "device", "system" and the like refer to related entities applied to computers, such as hardware, a combination of hardware and software, software or software in execution, etc. In detail, for example, an element can be, but is not limited to, a process, a processor, an object, an executable element, an execution thread, a program and / or a computer running on a processor. In addition, an application or script program running on a server, a server can all be an element. One or more elements can be in an execution process and / or thread, and an element can be localized on a computer and / or distributed between two or more computers, and can be operated by various computer-readable media. An element can also communicate through local and / or remote processes according to a signal with one or more data packets, for example, a signal from a data that interacts with another element in a local system, a distributed system, and / or a network on the Internet through a signal to interact with other systems.
[0035] Finally, it should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include" and "comprise" include not only those elements, but also other elements that are not explicitly listed, or also include elements inherent to such processes, methods, articles or equipment. In the absence of further restrictions, the elements defined by the statement "include..." do not exclude the existence of other identical elements in the process, method, article or equipment that includes the elements.
[0036] The present invention will be further described in detail below in conjunction with the accompanying drawings.
[0037] Figure 1The process of the laser odometer pose estimation method of one embodiment of the present invention is schematically shown. The execution subject of the method can be the laser odometer pose estimation device described below, or it can be an intelligent device such as a smart phone, a personal computer, a cloud server, or an intelligent cleaning mobile platform such as an unmanned cleaning vehicle, an unmanned sweeping vehicle, or a sweeping robot. The embodiment of the present invention does not limit this, so that these devices or equipment or mobile platforms can use the laser odometer pose estimation method of the embodiment of the present invention to perform pose estimation on the acquired current frame point cloud with less computation and better algorithm real-time performance. Figure 1 As shown, the method comprises the following steps:
[0038] Step S11: obtaining the current frame point cloud, and determining the target grid to be calculated for the first attribute based on the initial pose value and the target grid map corresponding to the current frame point cloud, wherein each grid of the target grid map includes a first attribute and a second attribute, wherein the first attribute is a grid parameter characterized by the points added to the corresponding grid, and the second attribute is a cache queue for caching the points to be added in the corresponding grid, and the points to be added are the points that fall into the corresponding grid in the previous frame point cloud;
[0039] Step S12: Determine a pose estimate corresponding to the current frame point cloud according to the first attribute and the second attribute of the target grid.
[0040] In step S11, it is necessary to first obtain the current frame point cloud. The way and method of obtaining the current frame point cloud will not affect the implementation of the present invention, and can be designed according to actual conditions. In the present embodiment, it can be set to be obtained by laser radar. In an embodiment of the present invention, the target grid map is a map represented by a data structure of a grid and a point cloud corresponding to the grid, which contains the distribution of historical frame point clouds before the current frame point cloud in the grid, wherein the initial target grid map is obtained based on rasterizing the first frame point cloud obtained. The initial value of the pose of the current frame point cloud refers to the initial value of the relative pose between the current frame laser radar coordinate system and the map coordinate system at the moment of obtaining the current frame point cloud, which can be obtained by pose estimation of other sensors, or by estimating the historical pose according to the uniform velocity model. Through the initial pose value corresponding to the current frame point cloud and the target grid map, it is possible to determine which grids in the target grid map each point in the current frame point cloud falls into, so as to determine the target grid for calculating the first attribute, wherein, in the embodiment of the present invention, the target grid refers to the grid in the target grid map where the points in the current frame point cloud fall into. The embodiment of the present invention determines the grid in which the points in the current frame point cloud fall into as the target grid, so that only the target grid in the target grid map is updated, thereby avoiding redundant rasterization and incremental updates of the point cloud map, and improving the real-time performance of the laser odometer pose estimation algorithm.
[0041] As a possible preferred implementation, in the present invention, the point cloud data in each grid in the target grid map is described by the first attribute and the second attribute, that is, the embodiment of the present invention describes the data structure of the point cloud corresponding to each grid by the first attribute and the second attribute. The first attribute is used to represent the grid parameter represented by the point added in the corresponding grid, and the second attribute is used to cache the cache queue of the point to be added in the corresponding grid, and the point to be added is the point that falls into the corresponding grid in the point cloud of the previous frame. Therefore, when the first frame point cloud is obtained, the first frame point cloud can be rasterized and the correspondence between the points in the first frame point cloud and the grids can be obtained based on the initial value of the pose of the first frame point cloud. Then, based on the correspondence between the points in the first frame point cloud and the grids, a grid including the first attribute and the second attribute corresponding to the points in the first frame point cloud can be constructed by adding a grid. All the constructed grids including the first attribute and the second attribute corresponding to the points in the first frame point cloud are used as the content of the initialized target grid map, so as to use the initialized target grid map to perform steps S11 and S12, thereby determining the pose estimate of the first frame point cloud. After determining the position estimate of the first frame point cloud, the initialized target grid map can be incrementally updated based on the determined pose estimate of the first frame point cloud according to the method described below in an embodiment of the present invention, so as to obtain the latest target grid map. Therefore, when the second frame point cloud is obtained, the processing of steps S11 and S12 can be performed based on the latest target grid map incrementally updated based on the pose estimate of the first frame point cloud to determine the pose estimate of the second frame point cloud, and so on. Therefore, the target grid map used in step S11 is the latest target grid map corresponding to when the current frame point cloud is obtained, that is, the target grid map formed after the target grid map is incrementally updated based on the pose estimate of the previous frame point cloud. In the example of the present invention, the first attribute corresponding to the grid is used to represent the grid parameters represented by the points added in the corresponding grid, and the second attribute is a cache queue for caching the points to be added in the corresponding grid. The points to be added are the points that fall into the corresponding grid in the previous frame of the point cloud. Therefore, the points that fall into the corresponding grid in the previous frame of the point cloud are not directly calculated into the first attribute first, but are first cached into the second attribute, that is, the data structure used to represent the point cloud data in a certain grid includes a second attribute for caching the points that fall into the corresponding grid in the previous frame of the point cloud, and a first attribute of the grid parameters represented by the added points before the points that fall into the corresponding grid in the previous frame of the point cloud are added. Therefore, the embodiment of the present invention separates the update of the newly added points in each grid and the update of the grid parameters corresponding to each grid through the data structure design of the target grid map, so that only the target grid can be incrementally updated according to needs to avoid redundant rasterization and unnecessary parameter updates of the point cloud map.Based on the data structure design of the target grid map, in step S11, the target grid to be calculated for the first attribute is only the target grid for which the points to be added cached in the second attribute need to be added to the first attribute, rather than all grids included in the entire target grid map. As a possible implementation, the first attribute includes a number of points n for indicating the number of points added in the corresponding grid and a sum of the point positions for indicating the number of points added in the corresponding grid. and the sum of the quadratic forms representing the point positions of the added points in the corresponding grid The number of points n added to the corresponding grid, used to represent the sum of the point positions of the points added to the corresponding grid and the sum of the quadratic forms representing the point positions of the added points in the corresponding grid It can represent the data of the points accumulated in the corresponding grid, and can also be used to calculate the mean and covariance of the points in the corresponding grid, so as to quickly determine the normal distribution probability density function corresponding to the corresponding grid. At the same time, the first attribute is used to represent the data of the points accumulated in the corresponding grid. When updating the data of the corresponding grid, the grid data can be updated only in an incremental manner, which can effectively save the processing amount when updating the grid data.
[0042] Reference Figure 2 As shown, Figure 2 The method flow of step S11 in the laser odometer pose estimation method according to one embodiment of the present invention is schematically shown. Figure 2 As shown, as a possible implementation, the method can be implemented as including the following steps:
[0043] Step S21: determining the initial coordinate position of each point in the current frame point cloud in the target grid map according to the initial pose value corresponding to the current frame point cloud;
[0044] Step S22: determining the initial value correspondence between each point in the current frame point cloud and each grid in the target grid map according to the initial value coordinate position of each point in the current frame point cloud in the target grid map;
[0045] Step S23: Determine the grid in the target grid map that has an initial value corresponding relationship with the point in the current frame point cloud as the target grid.
[0046] Step S21 is a step of determining the initial coordinate position of each point in the current frame point cloud in the target grid map. In step S21, the initial coordinate position of each point in the current frame point cloud in the target grid map is calculated based on the initial pose value corresponding to the current frame point cloud and the grid size of the target grid map.
[0047] Step S22 is a step of determining the initial value correspondence between each point in the current frame point cloud and each grid in the target grid map. In step S22, since the initial value coordinate position of each point in the current frame point cloud has been determined, it is possible to determine whether each point falls into each grid in the target grid map based on the initial value coordinate position of each point in the current frame point cloud, thereby forming an initial value correspondence between each point in the current frame point cloud and each grid in the target grid map.
[0048] Step S23 is a step of determining the target grid. The target grid is the grid in the target grid map whose first attribute is to be calculated. Since the points in the current frame point cloud may fall into the constructed grid in the target grid map, or may fall outside the constructed grid in the target grid map, for the points falling into the constructed grid in the target grid map, the corresponding grid may change during the process of updating the target grid map, so the grid in the target grid map that has an initial value corresponding relationship with the points in the current frame point cloud is determined as the target grid.
[0049] For example, the method of determining the target grid using a hash table is used as an example to illustrate steps S21 to S23. In this method, the grids in the target grid map are stored in a hash table. The coordinates of each point in the current frame point cloud are marked as p = [p x ,p y ,p z ] T The initial coordinate position of the corresponding point in the target grid map is determined as v = [v x ,v y ,v z ] T , at this time, the initial coordinate position of each point can be calculated by the following formula:
[0050]
[0051] Where s is the grid size. Then use the spatial hash function Hash() to calculate the hash key id corresponding to the grid coordinate v v , which can be calculated by the following formula:
[0052] id v =Hash(v)=(v x n x )xor(v y n y )xor(v z n z ))modN
[0053] Among them, n x 、ny 、n z is a large prime number. In this embodiment, n x =73856093、n y =471943, n z =83492791, xor is the exclusive or operator, mod is the modulus operator, and N is the size of the hash table. Finally, the hash key id corresponding to the current point is found in the hash table. v , it is possible to determine in which grid the current point falls in the target grid map, form the initial value correspondence between each point in the current frame point cloud and each grid in the target grid map, and determine the target grid.
[0054] After the target grid for calculating the first attribute is determined, step S12 is continued.
[0055] Step S12 is a step of determining the pose estimation corresponding to the current frame point cloud according to the first attribute and the second attribute of the target grid. Since the target grid is the grid in the target grid map where the points in the current frame point cloud fall, the normal distribution probability density function corresponding to the corresponding grid can be calculated according to the first attribute and the second attribute of the corresponding grid, and the cost function corresponding to each point in the current frame point cloud is constructed to determine the pose estimation corresponding to the current frame point cloud. Figure 3 The method flow of step S12 in the laser odometer pose estimation method according to one embodiment of the present invention is schematically shown. Figure 3 As shown, the method can be specifically implemented by comprising the following steps:
[0056] Step S31: incrementally updating the first attribute of the corresponding target grid according to the second attribute of each target grid, and clearing the cached points to be added in the second attribute of the corresponding target grid;
[0057] Step S32: determining the mean and covariance of the points in the corresponding target grid according to the first attribute after the incremental update of each target grid, and then determining the normal distribution probability density function corresponding to the corresponding target grid;
[0058] Step S33: Determine the residual of each point in the current frame point cloud according to the relative pose between the current frame point cloud and the target grid map and the mean of the points in each target grid, and construct the cost function of each point in the current frame point cloud according to the residual of each point in the current frame point cloud and the normal distribution probability density function corresponding to each target grid, and find the optimal solution for the cost function according to the preset optimization target to determine the pose estimate corresponding to the current frame point cloud.
[0059] When determining the pose estimate corresponding to the point cloud of the current frame, first incrementally update the first attribute of the target grid according to step S31. It can be understood that in step S31, the incremental update of the first attribute of the target grid is performed based on the points to be added in the second attribute of the corresponding target grid. Therefore, after the incremental update of the first attribute of the target grid, it is necessary to clear the cached points to be added in the second attribute of the target grid. At this time, there is actually no substantial change in the point cloud data in each target grid, but its form of expression has changed, that is, the points in the previous frame of point cloud that fall into the corresponding target grid are changed from being cached in the second attribute to being added to the first attribute. Exemplarily, before the incremental update of a certain target grid, the number of points added in its first attribute is n, and the sum of the point positions of the points added in the corresponding target grid is The sum of the point positions of the added points in the corresponding target grid is In its second attribute, the cached points to be added are {q1,q2,q3...q m} as an example, at this time, the target grid is incrementally updated, that is, the second attribute of the target grid is cleared, and the number of points in the first attribute of the target grid is incrementally updated to n+m, and the sum of the point positions is incrementally updated to The sum of the quadratic form of the point position is incrementally updated as
[0060] In step S32, after each target grid is incrementally updated, the mean and covariance of the points in the corresponding target grid can be determined according to the first attribute corresponding to each target grid after the incremental update, and the normal distribution probability density function corresponding to the corresponding target grid can be determined according to the mean and covariance of the points in the corresponding target grid. Exemplarily, the above example is continued to explain that the mean of the points in the corresponding target grid can be obtained by the following formula:
[0061]
[0062] Among them, p i is the coordinate of the point in the corresponding target grid, i is used to represent the i-th point, i=1,...,N, and the mean μ of the points in the corresponding target grid can be calculated by the above formula, and the mean μ describes the center position of the point in the target grid.
[0063] The covariance of the points in the corresponding target grid can be obtained using the following formula:
[0064]
[0065] Among them, p iis the coordinate of the point in the corresponding target grid, i is used to represent the i-th point, i=1,...,N, and T is the transposed matrix with the rows and columns of the matrix swapped. The covariance Σ of the points in the corresponding target grid can be calculated by the above formula, and the covariance Σ describes the distribution of the points in the target grid.
[0066] After determining the mean μ and covariance Σ of the points in the corresponding target grid, the normal distribution probability density function P(x) corresponding to the corresponding target grid can be determined according to the mean μ and covariance Σ of the points, which can be obtained by the following formula:
[0067]
[0068] Wherein, D is the dimension of the multidimensional normal distribution variable. For a point in three-dimensional space, D=3.
[0069] After determining the mean μ of the points in each target grid, in step S33, the residual e of each point in the current frame point cloud is determined based on the relative pose between the current frame point cloud and the target grid map and the mean of the points in each target grid. i Among them, the relative pose between the current frame point cloud and the target grid map is R∈SO(3), R is the relative rotation, t is the relative translation, and the residual e of each point in the point cloud of the current frame i It can be obtained by the following formula:
[0070] e i =Rq i +t-μ
[0071] Among them, q i is the coordinate of the i-th point in the current frame point cloud, μ is the coordinate of point q i The mean value of the points corresponding to the target grid. The residual e of each point in the current frame point cloud is calculated. i After that, the residual e of each point in the point cloud of the current frame can be used to calculate i The normal distribution probability density function P(x) of each target grid corresponding to each point is used to construct the cost function of each point in the current frame point cloud. Through this cost function, the optimal solution of the cost function is obtained according to the preset optimization goal to determine the pose estimation corresponding to the current frame point cloud. i When constructing the cost function, the initial value of the pose of the point cloud of the current frame (the method of obtaining it can refer to the relevant instructions in the previous article) is used as the initial relative pose to determine the residual e i And the cost function, then the constructed cost function will be solved according to the preset optimization goal to determine the relative pose R∈SO(3) between the current frame point cloud and the target grid map. Then the residual e is further determined based on the calculated relative pose. i And construct the cost function, solve the constructed cost function according to the preset optimization goal, and perform iterative calculations in this way until the calculated relative pose meets the optimization goal, that is, the point q in the point cloud of the current frame is i The joint probability density in the target grid is the largest, and the corresponding relative posture is the determined posture estimate. Specifically, according to the normal distribution probability density function P(x) corresponding to each target grid, the maximum likelihood estimation can be transformed into a least squares problem, and then the following cost function is constructed:
[0072]
[0073] Solving by Gauss-Newton method, we further get the following formula:
[0074]
[0075] Residual e i The Jacobian of the pose is as follows:
[0076]
[0077]
[0078] Among them, ∧ is an antisymmetric operator, which is used to convert a vector into an antisymmetric matrix skew-symmetric matrix). The cost function is optimized and solved by the above method, the Jacobian matrix is calculated, and the cost function is minimized using a numerical optimization method, and finally the pose estimation corresponding to the current frame point cloud is obtained.
[0079] As a possible implementation, after step S12, the target grid map can also be updated and maintained according to the pose estimation corresponding to the current frame point cloud determined in step S12, so that when obtaining the position estimation corresponding to the next frame point cloud, the above steps S11 and S12 can be continued to be used for acquisition. Figure 4 As shown, in this embodiment, after determining the pose estimation corresponding to the point cloud of the current frame, step S12 further includes:
[0080] Step S13: Update the target grid map according to the pose estimation corresponding to the point cloud of the current frame.
[0081] Step S13 is a step of updating the target grid map according to the pose estimation corresponding to the current frame point cloud. Since the pose estimation corresponding to the current frame point cloud has been determined, the coordinate position of each point in the current frame point cloud in the target grid map can be determined, and each point in the current frame point cloud falls into which grids in the target grid map, so as to update the point cloud data in these grids, thereby realizing the incremental update and dynamic maintenance of the target grid map, avoiding the need to update all grids of the overall target grid map every time the pose estimation is obtained by the method of the present invention, and effectively reducing the amount of calculation.
[0082] Figure 5 The method flow of step S13 in the laser odometer pose estimation method according to one embodiment of the present invention is schematically shown. Figure 5 As shown, the method can be specifically implemented by comprising the following steps:
[0083] Step S41: Determine the coordinate position of each point in the target grid map according to the pose estimation corresponding to the point cloud of the current frame;
[0084] Step S42: determining the correspondence between each point in the current frame point cloud and each grid in the target grid map according to the coordinate position of each point in the current frame point cloud in the target grid map;
[0085] Step S43: Determine the grid to be updated according to the correspondence between each point in the current frame point cloud and each grid in the target grid map, and update the points in the current frame point cloud that fall into the corresponding grid to be updated as points to be added to the second attribute of the corresponding grid to be updated.
[0086] First, according to step S41, the coordinate position of each point in the current frame point cloud in the target grid map is determined. Among them, for step S41, its implementation method can refer to the relevant implementation method of determining the initial value coordinate position of each point in the current frame point cloud in the target grid map in step S21. The difference is that in this step, when determining the coordinate position of each point in the current frame point cloud in the target grid map, the coordinate position of each point in the current frame point cloud in the target grid map is calculated based on the pose estimation corresponding to the current frame point cloud and the grid size of the target grid map.
[0087] After determining the coordinate position of each point in the current frame point cloud in the target grid map, the corresponding relationship between each point in the current frame point cloud and each grid in the target grid map is determined according to step S42. According to the coordinate position of each point in the current frame point cloud in the target grid map, it can be determined whether each point in the current frame point cloud falls into each grid in the target grid map to form a corresponding relationship between each point in the current frame point cloud and each grid in the target grid map. Among them, for step S42, its implementation method can refer to the relevant implementation method of determining the initial value of each point in the current frame point cloud and each grid in the target grid map in step S22.
[0088] Step S43 is a step of determining the grid to be updated and updating the grid to be updated. The grid to be updated is a grid in the target grid map that needs to update the point cloud data in the corresponding grid according to the points in the current frame point cloud that fall into the corresponding grid. By updating each point in the current frame point cloud to the point cloud data of the grid it falls into, the incremental update of the target grid map is completed. Among them, for the points in the current frame point cloud, it may fall into the constructed grid in the target grid map, or it may fall outside the constructed grid in the target grid map. Therefore, for the points that fall into the constructed grid in the target grid map, the corresponding grid is recorded as the grid to be updated. For the grid to be updated, the point cloud data of the grid needs to be updated according to the points that fall into the grid in the current frame point cloud. Since in the present invention, the point cloud data of the grid in the target grid map is represented by the first attribute and the second attribute, the points in the current frame point cloud in the point cloud data that need to be newly added to the grid will be cached in the second attribute as points to be added, and will not be updated into the first attribute for the time being. Thus, the separation of the update of the first attribute and the second attribute of each grid is achieved, so that the first attribute of the target grid can be incrementally updated according to needs and expectations, thereby reducing the amount of calculation when updating the target grid map and calculating the pose estimation of the current frame point cloud.
[0089] According to the example of steps S21 to S23 in the previous text, taking the example of each grid being stored in a hash table, by searching the hash key id corresponding to the point of the current frame point cloud in the hash table, v , it is possible to determine which grid the current point falls into in the target grid map, form the corresponding relationship between each point in the current frame point cloud and each grid in the target grid map, and determine the grid to be updated. When updating the grid to be updated, the points in the current frame that have a corresponding relationship with the corresponding grid to be updated are cached in the second attribute of the corresponding grid as points to be added, so as to complete the step of updating the grid to be updated.
[0090] As a possible implementation, since all points in the current frame point cloud need to be added to the target grid map to complete the update of the target grid map, in step S42, when there are points without corresponding grids in the current frame point cloud, that is, when at least some points in the current frame fall outside the constructed grids in the target grid map, it is necessary to add a new grid in the target grid map so that all points in the current frame point cloud can fall into the grids in the target grid map to achieve the update of the target grid map. Specifically, a new grid including a first attribute and a second attribute can be constructed based on the coordinate position of the points without corresponding grids in the current frame point cloud and the target grid map, and then the points in the current frame point cloud corresponding to the new grid are cached as points to be added in the second attribute of the corresponding grid.
[0091] For the target grid map, since the current frame is constantly acquired and new grids are added and updated to the target grid map, the target grid map will become larger and larger as time goes by. In order to reduce the amount of calculation and storage of the target grid map to be maintained, as a preferred implementation, the embodiment of the present invention can be based on the preset number of grids as a standard to limit the size of the target grid map to avoid the target grid map being too large, resulting in excessive calculation and storage of the target grid map to be maintained. At this time, as a possible implementation, in the present invention, step S13 also includes: based on the number of grids in the target grid map and the parameter information of each grid, determining the grid to be deleted, and deleting the determined grid to be deleted from the target grid map. Specifically, since the grid of the target grid map of the present invention uses the first attribute and the second attribute to represent the point cloud data of the grid, the number of grids stored in the target grid map is an important factor affecting the size of the target grid map. When the target grid map is updated according to the pose estimation corresponding to each point in the current frame point cloud, when a new grid is needed, the size of the target grid map will be affected. Therefore, when the number of grids in the target grid map is greater than the set number threshold, it is necessary to determine the grids to be deleted and delete the determined grids to be deleted from the target grid map. Among them, the grids to be deleted are determined based on the parameter information of each grid, and the parameter information of each grid includes the distance between the corresponding grid and the current frame point cloud and / or the update time of the first attribute of the corresponding grid. It can be understood that for a grid in the target grid map, the farther it is from the current frame point cloud, the lower the probability of updating the grid when acquiring a new frame point cloud and updating the target grid map according to the acquired new frame point cloud. Similarly, for a grid in the target grid map, the earlier the update time of the first attribute of its corresponding grid, the longer the grid has not been updated, and the lower the probability of updating the grid when acquiring a new frame of point cloud and updating the target grid map according to the acquired new frame of point cloud. Therefore, as a possible implementation method, when the number of grids is greater than a set number threshold, the grid farthest from the current frame of point cloud and / or the grid with the earliest update time of the first attribute of the corresponding grid is determined as the grid to be deleted, so as to maintain the size of the target grid map.
[0092] Figure 6 The following schematically shows a target grid map diagram when updating the target grid map in step S13 of the laser odometer pose estimation method according to an embodiment of the present invention. Figure 6In the example, some points of the current frame point cloud fall into the constructed grids in the target grid map, and some points of the current frame point cloud fall outside the constructed grids of the target grid map. At this time, for the grids in the target grid map that have a corresponding relationship with the points of the current frame point cloud, the points in the current frame point cloud that fall into the corresponding grids are cached as points to be updated in the second attribute of the corresponding grids. For the coordinate positions where the points in the current frame point cloud fall outside the constructed grids in the target grid map, a new grid is created at the corresponding position according to the grid construction rules of the target grid map (i.e. Figure 6 The rightmost dotted grid in the grid is selected, and then the newly created grids are updated according to the points in the current frame point cloud that fall into the corresponding grids. At the same time, since there are newly created grids in the target grid map, the number of grids will be greater than the preset threshold. Therefore, the grids to be deleted (i.e. Figure 6 The leftmost column of grids in the target grid map is selected, and these grids to be deleted are the grids farthest from the current frame point cloud and / or the grids with the earliest update time of the first attribute of the corresponding grid, and these grids to be deleted are deleted from the target grid map, thereby completing the incremental update and dynamic maintenance of the target grid map, so as to avoid redundant rasterization and incremental update of the point cloud map.
[0093] Figure 7 The overall process of the laser odometer pose estimation method according to one embodiment of the present invention is schematically shown. Figure 7As shown, the present invention includes two parts: point cloud matching and grid maintenance. After obtaining the current frame point cloud, point cloud matching is performed first. Specifically, a grid search is performed first to determine which grids in the target grid map the points in the current frame point cloud fall into, so as to determine these grids as target grids. Then, the target grid in the target grid map is incrementally updated, wherein the incremental update is to add the points to be added cached according to the second attribute in the target grid to the first attribute of the corresponding target grid, that is, only the expression form of the point cloud data in the target grid is updated, and the point cloud data in the target grid is not changed. After the target grid is incrementally updated, a cost function is constructed according to the first attribute of the target grid, the relative pose of the current frame point cloud and the target grid map, and the cost function is optimized and solved to obtain the pose estimation corresponding to the current frame point cloud. After the point cloud matching is completed, the target grid map is grid maintained and incrementally updated. Specifically, the position of each point in the current frame point cloud in the target grid map is first determined according to the pose estimation of the current frame point cloud. Then, a grid search is performed to determine the grid to be updated whose point cloud data needs to be updated. At the same time, it is determined whether there are points outside the constructed grid in the target grid map, and a new grid is created based on the coordinate position of the points outside the constructed grid in the target grid map and the target grid map, and the grid to be deleted is determined. For the newly created grid and the grid to be updated, the point cache value that falls into the corresponding grid in the current frame point cloud is added to the second attribute of the corresponding grid, and the grid to be deleted is deleted, thereby completing the incremental update of the target grid map, so as to better use the method of the point cloud matching part to determine the pose estimation of the newly acquired current frame point cloud.
[0094] The method provided by the embodiment of the present invention uses the first attribute and the second attribute to describe each grid in the target grid map, thereby realizing the separation of the newly added points in each grid and the incremental update of the grid parameters, so that when the NDT algorithm is used for frame and map matching, it is not necessary to rasterize the entire point cloud map and recalculate the normal distribution parameters of all grids each time the current frame point cloud is acquired. Instead, the first attribute and the second attribute and the current frame point cloud are used to determine the target grid, and only the first attribute and the second attribute of the target grid in the target grid map are incrementally updated, and the first attribute and the second attribute of the incrementally updated target grid are used to calculate the pose estimation of the current point cloud, thereby avoiding redundant rasterization and incremental update of the point cloud map, and improving the real-time performance of the laser odometer pose estimation algorithm. Furthermore, since the points in the current frame point cloud are usually disordered, there may be multiple points falling into the same grid, and there may also be no points falling into a certain grid. Therefore, the embodiment of the present invention sets the second attribute so that the points to be added in each grid can be temporarily stored through the cache queue. This not only avoids the situation where the grid parameters are updated every time a point is added to the grid, reducing the processing amount in the single-frame point cloud processing, but also enables the incremental update of the first attribute of the target grids participating in the cost function construction only when it is determined that the corresponding grid participates in the cost function construction, avoiding the updating of the normal distribution parameters after each point cloud match, and reducing the amount of calculation in the multi-frame point cloud processing. Therefore, the method of the present invention can also significantly reduce the amount of calculation of the laser odometer pose estimation algorithm, and further improve the real-time performance of the algorithm.
[0095] Figure 8 The principle block diagram of the laser odometer pose estimation device according to one embodiment of the present invention is schematically shown. Figure 8 As shown, the laser odometer posture estimation device includes:
[0096] The target grid determination module 1 is used to obtain the current frame point cloud, and determine the target grid to be calculated for the first attribute based on the initial value of the pose corresponding to the current frame point cloud and the target grid map, wherein each grid of the target grid map includes a first attribute and a second attribute, wherein the first attribute is a grid parameter represented by the points added in the corresponding grid, and the second attribute is a cache queue for caching the points to be added in the corresponding grid, and the points to be added are the points in the previous frame point cloud that fall into the corresponding grid;
[0097] The pose estimation module 2 is used to determine the pose estimation corresponding to the current frame point cloud according to the first attribute and the second attribute of the target grid.
[0098] It should be noted that the implementation process and implementation principle of the laser odometer pose estimation device of the embodiment of the present invention can be specifically referred to the corresponding description of the above method embodiment, such as the corresponding description of the method embodiment part on determining the target grid, the pose estimation of each point in the current frame point cloud, etc., so they are not repeated here. Exemplarily, the laser odometer pose estimation device of the embodiment of the present invention can be any intelligent device with a processor, including but not limited to computers, smart phones, personal computers, robots, cloud servers, etc.
[0099] Fig. 9 The principle block diagram of the laser odometer pose estimation device according to another embodiment of the present invention is schematically shown. Fig. 9 As shown in FIG. 1 , the laser odometer pose estimation device is Figure 8 Based on the laser odometer pose estimation device, it also includes:
[0100] The grid map updating module 3 is used to update the target grid map according to the pose estimation corresponding to each point in the point cloud of the current frame.
[0101] It should be noted that the implementation process and implementation principle of the laser odometer pose estimation device of the embodiment of the present invention can be specifically referred to the corresponding description of the above method embodiment, such as the corresponding description of updating the target grid map in the method embodiment, so it will not be repeated here. Exemplarily, the laser odometer pose estimation device of the embodiment of the present invention can be any intelligent device with a processor, including but not limited to computers, smart phones, personal computers, robots, cloud servers, etc.
[0102] Fig.10 A block diagram schematically shows a computing device according to an embodiment of the present invention. Fig.10 As shown, the device includes:
[0103] Memory 4, used to store executable instructions; and
[0104] The processor 5 is used to execute the executable instructions stored in the memory 4. When the executable instructions are executed by the processor 5, the steps of the laser odometer pose estimation method in the above embodiment are implemented.
[0105] It should be noted that the computing device in the embodiment of the present invention can be any intelligent device with a processor, including but not limited to computers, smart phones, personal computers, robots, cloud servers, etc., and can also be an intelligent device set on any movable platform. These movable platforms can be unmanned cleaning vehicles with automatic cleaning functions, unmanned cleaning machines, cleaning robots, etc. The specific implementation process and implementation principles of the laser odometer pose estimation method involved in the computing device of the embodiment of the present invention can be found in the corresponding description of the above-mentioned method embodiment, and will not be repeated here.
[0106] In some embodiments, an embodiment of the present invention provides a non-volatile computer-readable storage medium, which stores one or more programs including execution instructions, and the execution instructions can be read and executed by an electronic device (including but not limited to a computer, a server, or a network device, etc.) to execute the laser odometer pose estimation method of any of the above embodiments of the present invention.
[0107] In some embodiments, an embodiment of the present invention further provides a computer program product, which includes a computer program stored on a non-volatile computer-readable storage medium, and the computer program includes program instructions. When the program instructions are executed by a computer, the computer executes the laser odometer pose estimation method of any one of the above embodiments.
[0108] In some embodiments, an embodiment of the present invention further provides an electronic device, comprising: at least one processor, and a memory communicatively connected to the at least one processor, wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the laser odometer pose estimation method of any of the above embodiments.
[0109] In some embodiments, an embodiment of the present invention further provides a storage medium having a computer program stored thereon, characterized in that when the program is executed by a processor, the laser odometer pose estimation method of any of the above embodiments is implemented.
[0110] Fig.11 FIG. 1 is a schematic diagram of the hardware structure of an electronic device for executing a method for estimating a laser odometer posture according to another embodiment of the present invention. Fig.11 As shown, the device includes:
[0111] One or more processors 610 and memory 620, Fig.11 A processor 610 is taken as an example.
[0112] The device for executing the laser odometer pose estimation method may further include: an input device 630 and an output device 640 .
[0113] The processor 610, the memory 620, the input device 630 and the output device 640 may be connected via a bus or other means. Fig.11 The example of connecting through bus is taken in the following.
[0114] The memory 620, as a non-volatile computer-readable storage medium, can be used to store non-volatile software programs, non-volatile computer executable programs and modules, such as program instructions / modules corresponding to the laser odometer pose estimation method in the embodiment of the present invention. The processor 610 executes various functional applications and data processing of the server by running the non-volatile software programs, instructions and modules stored in the memory 620, that is, the laser odometer pose estimation method of the above method embodiment is implemented.
[0115] The memory 620 may include a program storage area and a data storage area, wherein the program storage area may store an operating system and applications required by at least one function; the data storage area may store data created according to the use of the laser odometer pose estimation method, etc. In addition, the memory 620 may include a high-speed random access memory, and may also include a non-volatile memory, such as at least one disk storage device, a flash memory device, or other non-volatile solid-state storage device. In some embodiments, the memory 620 may optionally include a memory remotely arranged relative to the processor 610, and these remote memories may be connected to the electronic device via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.
[0116] The input device 630 may receive input digital or character information and generate signals related to user settings and function control of the image processing device. The output device 640 may include a display device such as a display screen.
[0117] The one or more modules are stored in the memory 620, and when executed by the one or more processors 610, the laser odometer pose estimation method in any of the above method embodiments is executed.
[0118] The above product can execute the method provided by the embodiment of the present invention, and has the functional modules and beneficial effects corresponding to the execution method. For technical details not described in detail in this embodiment, please refer to the method provided by the embodiment of the present invention.
[0119] The electronic devices of the embodiments of the present invention exist in various forms, including but not limited to:
[0120] (1) Mobile communication devices: These devices are characterized by their mobile communication functions and their main purpose is to provide voice and data communications. These terminals include: smart phones (such as iPhone), multimedia phones, functional phones, and low-end phones.
[0121] (2) Ultra-mobile personal computer devices: These devices fall into the category of personal computers, have computing and processing capabilities, and generally also have mobile Internet access features. These terminals include: PDA, MID and UMPC devices, such as iPad.
[0122] (3) Portable entertainment devices: These devices can display and play multimedia content. They include audio and video players (such as iPods), handheld game consoles, e-books, as well as smart toys and portable car navigation devices.
[0123] (4) Server: A device that provides computing services. The server consists of a processor, hard disk, memory, system bus, etc. The server is similar to a general computer architecture, but because it needs to provide highly reliable services, it has higher requirements in terms of processing power, stability, reliability, security, scalability, and manageability.
[0124] (5) Other electronic devices with data interaction functions.
[0125] The device embodiments described above are merely illustrative, wherein the units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of this embodiment.
[0126] Through the description of the above implementation methods, those skilled in the art can clearly understand that each implementation method can be implemented by means of software plus a general hardware platform, and of course, by hardware. Based on this understanding, the above technical solution is essentially or the part that contributes to the relevant technology can be embodied in the form of a software product, and the computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a disk, an optical disk, etc., including a number of instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in each embodiment or some parts of the embodiments.
[0127] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.
Claims
1. A laser odometer pose estimation method, characterized in that: include: Obtain the current frame point cloud, and determine the target grid to be calculated for the first attribute based on the initial pose value and the target grid map corresponding to the current frame point cloud, wherein each grid of the target grid map includes a first attribute and a second attribute, wherein the first attribute is a grid parameter characterized by the points added to the corresponding grid, and the second attribute is a cache queue for caching the points to be added in the corresponding grid, wherein the points to be added are the points in the previous frame point cloud that fall into the corresponding grid; A pose estimate corresponding to the current frame point cloud is determined according to the first attribute and the second attribute of the target grid.
2. The method according to claim 1, characterized in that Also includes: The target grid map is updated according to the pose estimation corresponding to the point cloud of the current frame.
3. The method according to claim 1, characterized in that The first attributes include a number of points representing the points added in the corresponding grid, a sum of point positions representing the points added in the corresponding grid, and a sum of quadratic forms representing the point positions of the points added in the corresponding grid.
4. The method according to claim 1, characterized in that: Determining a pose estimate corresponding to the current frame point cloud according to the first attribute and the second attribute of the target grid includes: Incrementally update the first attribute of the corresponding target grid according to the second attribute of each target grid, and clear the cached points to be added in the second attribute of the corresponding target grid; Determine the mean and covariance of the points in the corresponding target grid according to the first attribute after the incremental update of each target grid, and then determine the normal distribution probability density function corresponding to the corresponding target grid; According to the relative pose between the current frame point cloud and the target grid map and the mean of the points in each target grid, the residual of each point in the current frame point cloud is determined, and the cost function of each point in the current frame point cloud is constructed according to the residual of each point in the current frame point cloud and the normal distribution probability density function corresponding to each target grid, and the optimal solution of the cost function is obtained according to the preset optimization target to determine the pose estimate corresponding to the current frame point cloud.
5. The method according to claim 2, characterized in that: The updating of the target grid map according to the pose estimation corresponding to the point cloud of the current frame includes: Determine the coordinate position of each point in the target grid map based on the pose estimation corresponding to the point cloud of the current frame; Determine the correspondence between each point in the current frame point cloud and each grid in the target grid map according to the coordinate position of each point in the current frame point cloud in the target grid map; The grid to be updated is determined according to the correspondence between each point in the current frame point cloud and each grid in the target grid map, and the points in the current frame point cloud that fall into the corresponding grid to be updated are updated as points to be added to the second attribute of the corresponding grid to be updated.
6. The method according to claim 5, characterized in that The updating of the target grid map according to the pose estimation corresponding to the point cloud of the current frame also includes: According to the correspondence between each point in the current frame point cloud and each grid in the target grid map, when there are points without corresponding grids in the current frame point cloud, a new grid including the first attribute and the second attribute is constructed based on the coordinate position of the point without corresponding grid in the target grid map and the target grid map.
7. The method according to claim 6, characterized in that The updating of the target grid map according to the pose estimation corresponding to the point cloud of the current frame also includes: Based on the number of grids in the target grid map and the parameter information of each grid, the grid to be deleted is determined, and the determined grid to be deleted is deleted from the target grid map, wherein the parameter information of each grid includes the distance between the corresponding grid and the current frame point cloud and / or the update time of the first attribute of the corresponding grid.
8. The method according to claim 7, characterized in that The step of determining the grid to be deleted based on the number of grids in the target grid map and parameter information of each grid includes: When the number of grids is greater than a set number threshold, the grid farthest from the current frame point cloud and / or the grid with the earliest update time of the first attribute of the corresponding grid is determined as the grid to be deleted.
9. The method according to claim 1, characterized in that: The step of determining the target grid to be calculated for the first attribute based on the initial pose value corresponding to the point cloud of the current frame and the target grid map includes: Determine the initial coordinate position of each point in the current frame point cloud in the target grid map according to the initial pose value corresponding to the current frame point cloud; Determine the initial value correspondence between each point in the current frame point cloud and each grid in the target grid map according to the initial value coordinate position of each point in the current frame point cloud in the target grid map; The grid in the target grid map that has an initial value corresponding relationship with the point in the current frame point cloud is determined as the target grid.
10. A laser odometer posture estimation device, characterized in that: include: A target grid determination module is used to obtain the current frame point cloud, and determine the target grid to be calculated for the first attribute based on the initial value of the pose corresponding to the current frame point cloud and the target grid map, wherein each grid of the target grid map includes a first attribute and a second attribute, wherein the first attribute is a grid parameter characterized by the points added to the corresponding grid, and the second attribute is a cache queue for caching the points to be added in the corresponding grid, and the points to be added are the points that fall into the corresponding grid in the previous frame point cloud; A pose estimation module is used to determine a pose estimation corresponding to a point cloud of a current frame according to a first attribute and a second attribute of the target grid.
11. A computing device, characterized in that include: A memory for storing executable instructions; as well as A processor, configured to execute executable instructions stored in a memory, wherein the executable instructions, when executed by the processor, implement the steps of the method according to any one of claims 1 to 9.
12. A vehicle, characterized in that It includes the laser odometer pose estimation device as described in claim 10 or the computing device as described in claim 11.
13. A storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the steps of the method described in any one of claims 1 to 9 are implemented.
