Pose estimation method, readable storage medium, and intelligent device

Abstract

The present application relates to the technical field of autonomous driving. Specifically provided are a pose estimation method, a readable storage medium, and an intelligent device, which aim to solve the problem of how to obtain a more accurate pose of a point cloud frame, so as to further improve the precision of a LiDAR odometer. To this end, the present application involves: acquiring the current point cloud frame collected by LiDAR provided on an intelligent device; and performing registration on the current point cloud frame and a local point cloud map, so as to obtain a pose estimation result of the current point cloud frame, wherein the local point cloud map is obtained by means of stitching historical point cloud frames in a sliding window and using a factor graph to perform optimization, and optimization variables of the factor graph comprise poses and surface element features of the historical point cloud frames. Therefore, the poses and surface element features of the historical point cloud frames added to the sliding window can be optimized at the same time, and thus an accumulated error during point cloud registration can be effectively reduced, a more accurate pose estimation result can be obtained, and the precision of a LiDAR odometer can be improved.

Description

Position estimation method, readable storage medium and intelligent device

[0001] This application claims priority to Chinese patent application No. 202311728395.2, filed on December 14, 2023, entitled “Posture Estimation Method, Readable Storage Medium and Intelligent Device”. The entire contents of the above Chinese patent application are incorporated into this application by reference. Technical Field

[0002] The present application relates to the field of autonomous driving technology, and specifically provides a posture estimation method, a readable storage medium, and an intelligent device. Background Art

[0003] LiDAR odometry is becoming increasingly popular in autonomous driving technology. It estimates the transformation between consecutive point cloud frames to obtain the pose of the point cloud frames.

[0004] However, existing lidar odometry mainly uses the point cloud eigenvalues ​​observed at the current moment to estimate the current state of motion, and aligns the current frame point cloud with the local point cloud map. It does not consider the observation error and fixes the state of historical moments, which is prone to cumulative errors. It is not robust to some degraded scenarios, and there are problems with poor lidar odometry accuracy and easy drift.

[0005] Accordingly, this field requires a new pose estimation solution to solve the above problems.

[0006] Summary of the Invention

[0007] In order to overcome the above-mentioned defects, the present application is proposed to provide a solution or at least partially solve the problem of how to obtain a more accurate pose of the point cloud frame, thereby further improving the accuracy of the lidar odometry.

[0008] In a first aspect, the present application provides a pose estimation method, the method comprising:

[0009] Get the current point cloud frame collected by the LiDAR on the smart device;

[0010] Registering the current point cloud frame with the local point cloud map to obtain a pose estimation result of the current point cloud frame;

[0011] The local point cloud map is obtained by stitching together historical point cloud frames within a sliding window of a preset window size and optimizing it with a factor graph.

[0012] The optimization variables of the factor graph include the pose and surface element features of the historical point cloud frame.

[0013] In one technical solution of the above-mentioned pose estimation method, the method further includes obtaining the local point cloud map according to the following steps:

[0014] Obtaining the pose of the historical point cloud frame within the sliding window;

[0015] Obtaining an initial point cloud map according to the posture and the historical point cloud frame;

[0016] The factor graph is applied to optimize the pose and facet features of the historical point cloud frames contained in the initial point cloud map to obtain the local point cloud map.

[0017] In one technical solution of the above-mentioned pose estimation method, the method further includes constructing the factor graph according to the following steps:

[0018] Setting variable nodes of the factor graph; the variable nodes include a pose variable node corresponding to the pose of the historical point cloud frame and a facet feature variable node corresponding to the facet feature;

[0019] Setting constraints on the variable nodes; each of the pose variable nodes is respectively provided with at least one constraint for constraining the pose; each of the face feature variable nodes is respectively provided with at least one constraint for constraining the face feature;

[0020] The factor graph is constructed according to the variable nodes and the constraint items.

[0021] In one technical solution of the above-mentioned pose estimation method, applying the factor graph to optimize the pose and facet features of the historical point cloud frames contained in the initial point cloud map includes:

[0022] Based on the factor graph, a sliding window optimization is performed on the pose of the historical point cloud frame and the facet features contained in the initial point cloud map, and after the sliding optimization, each constraint item is optimized separately to form an optimized factor graph. When the next sliding window optimization is performed, the pose and the facet features within the sliding window can be optimized based on the optimized factor graph.

[0023] In one technical solution of the above-mentioned pose estimation method,

[0024] Optimizing each constraint item separately to form an optimized factor graph includes:

[0025] Get the residual of each constraint term;

[0026] Each constraint item is optimized according to the residual of each constraint item to form the optimized factor graph.

[0027] In one technical solution of the above-mentioned pose estimation method, optimizing each constraint item separately according to the residual of each constraint item includes:

[0028] Obtaining a Jacobian matrix of the residual with respect to the optimization variable according to the residual of each constraint item;

[0029] Each constraint item is optimized according to the residual and the Jacobian matrix.

[0030] In a technical solution of the above-mentioned pose estimation method, optimizing each constraint item according to the residual and the Jacobian matrix includes:

[0031] A least squares method is applied according to the residual and the Jacobian matrix to optimize each constraint item.

[0032] In one technical solution of the above-mentioned pose estimation method, obtaining an initial point cloud map based on the pose and the historical point cloud frame includes:

[0033] splicing the historical point cloud frames according to the pose to obtain a spliced ​​point cloud map;

[0034] voxelize the spliced ​​point cloud map to obtain a voxelized point cloud map;

[0035] An octree is constructed according to the voxelized point cloud map to obtain the initial point cloud map.

[0036] In one technical solution of the above-mentioned pose estimation method, obtaining the pose of the historical point cloud frame within the sliding window includes:

[0037] Obtaining an initial pose of the historical point cloud frame according to other odometers of the smart device;

[0038] According to the initial pose, the historical point cloud frame is registered with the local point cloud map to obtain the distance residual between the surface element features of the historical point cloud frame and the local point cloud map;

[0039] The initial pose is optimized according to the distance residual to obtain the pose of the historical point cloud frame, and the historical point cloud frame is added to the sliding window according to the pose.

[0040] In one technical solution of the above-mentioned pose estimation method, the method further includes obtaining the facet features according to the following steps:

[0041] The 3D coordinates of the point on the surface element closest to the origin of the current coordinate system are used as the surface element feature of the surface element.

[0042] In one technical solution of the above-mentioned pose estimation method, the method further includes:

[0043] When the number of historical point cloud frames in the sliding window is greater than the preset window size, the oldest historical point cloud frame is deleted.

[0044] In a second aspect, a computer-readable storage medium is provided, which stores a plurality of program codes, wherein the program codes are suitable for being loaded and run by a processor to execute the posture estimation method described in any one of the technical solutions of the above-mentioned posture estimation method.

[0045] In a third aspect, a smart device is provided, comprising:

[0046] at least one processor;

[0047] and, a memory communicatively coupled to the at least one processor;

[0048] The memory stores a computer program, and when the computer program is executed by the at least one processor, the pose estimation method described in the pose estimation method technical solution is implemented.

[0049] The above one or more technical solutions of this application have at least one or more of the following beneficial effects:

[0050] In the technical solution for implementing the present application, the present application obtains the current point cloud frame collected by the laser radar set on the smart device, aligns the current point cloud frame with the local point cloud map, and obtains the pose estimation result of the current point cloud frame. The local point cloud map is based on the splicing of historical point cloud frames in the sliding window and is obtained by applying a factor graph for optimization. The optimization variables of the factor graph are the pose and facet features of the historical point cloud frames. Through the above configuration, the present application can simultaneously optimize the pose and facet features of the historical point cloud frames added to the sliding window, so that when the current point cloud frame is aligned with the local point cloud map, it can effectively reduce the cumulative error, obtain a more accurate pose estimation result, and improve the accuracy of the laser radar odometer. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] The disclosure of this application will become more easily understood with reference to the accompanying drawings. Those skilled in the art will readily appreciate that these drawings are for illustrative purposes only and are not intended to limit the scope of protection of this application. Among them:

[0052] FIG1 is a schematic flow chart of the main steps of a pose estimation method according to an embodiment of the present application;

[0053] FIG2 is a schematic diagram of facet parameterization according to an embodiment of the present application;

[0054] FIG3 is a flowchart illustrating the main steps of obtaining a local point cloud map according to an implementation of an embodiment of the present application;

[0055] FIG4 is a schematic diagram of a connection relationship between a memory and a processor in a smart device according to an embodiment of the present application. DETAILED DESCRIPTION

[0056] Some embodiments of the present application are described below with reference to the accompanying drawings. Those skilled in the art should understand that these embodiments are only used to explain the technical principles of the present application and are not intended to limit the scope of protection of the present application.

[0057] In the description of this application, "module" and "processor" may include hardware, software, or a combination of both. A module may include hardware circuitry, various suitable sensors, communication ports, and memory. It may also include software components, such as program code, or a combination of software and hardware. A processor may be a central processing unit, a microprocessor, an image processor, a digital signal processor, or any other suitable processor. A processor has data and / or signal processing capabilities. A processor may be implemented in software, hardware, or a combination of both. Non-transitory computer-readable storage media include any suitable medium capable of storing program code, such as magnetic disks, hard disks, optical disks, flash memory, read-only memory, random access memory, etc. The term "A and / or B" refers to all possible combinations of A and B, such as only A, only B, or both A and B. The terms "at least one of A or B" or "at least one of A and B" have similar meanings to "A and / or B" and may include only A, only B, or both A and B. The singular forms "a" and "the" may also include the plural forms.

[0058] An automated driving system (ADS) is a system that continuously performs all dynamic driving tasks (DDT) within its operational domain design (ODD). Specifically, the system is only allowed to fully assume the task of autonomous vehicle control under specified appropriate driving scenarios. When the vehicle meets the ODD conditions, the system is activated, replacing the human driver as the vehicle's primary driver. The DDT refers to the continuous lateral (left and right steering) and longitudinal motion control (acceleration, deceleration, and constant speed) of the vehicle, as well as the detection and response to objects and events in the vehicle's driving environment. The ODD refers to the conditions under which the automated driving system can operate safely. These conditions can include geographic location, road type, speed range, weather, time of day, and national and local traffic laws and regulations.

[0059] Referring to FIG1 , FIG1 is a flow chart showing the main steps of a pose estimation method according to an embodiment of the present application. As shown in FIG1 , the pose estimation method in the embodiment of the present application mainly includes the following steps S101 to S102.

[0060] Step S101: Acquire a current point cloud frame collected by a laser radar set on a smart device.

[0061] In this embodiment, the current point cloud frame collected by the laser radar set on the smart device can be obtained.

[0062] In one embodiment, the smart device may be a driving device, a smart car, a robot, or the like.

[0063] Step S102: Align the current point cloud frame with the local point cloud map to obtain the pose estimation result of the current point cloud frame; wherein, the local point cloud map is obtained by splicing historical point cloud frames within a sliding window of a preset window size and applying a factor graph for optimization; the optimization variables of the factor graph include the pose and facet features of the historical point cloud frames.

[0064] In this embodiment, the lidar odometry can align the current point cloud frame with a local point cloud map to obtain a pose estimate for the current point cloud frame. The local point cloud map is spliced ​​based on the pose of the historical point cloud frame collected by the lidar and the historical point cloud. This is obtained by optimizing a factor graph based on a sliding window. The optimization variables of the factor graph are the pose and facet features of the historical point cloud frame. In other words, the historical point cloud frame can be modeled as multiple facets, and the parameters representing each facet are used as the facet features of the historical point cloud frame. Those skilled in the art can set the preset window size based on the needs of the actual application.

[0065] In one embodiment, the surface element feature may be a plane equation of each surface element.

[0066] In one embodiment, please refer to FIG2, which is a schematic diagram of the facet parameterization according to an embodiment of the present application. As shown in FIG2, the 3D coordinates of the closest point in the facet to the origin of the current coordinate system, i.e. ... As the surface element feature of the surface element, the surface element parameterization is realized. This can minimize the parameter form and reduce the amount of calculation in the calculation process while ensuring that the surface element feature has real physical meaning.

[0067] Based on the above steps S101-S102, the embodiment of the present application obtains the current point cloud frame collected by the laser radar set on the smart device, aligns the current point cloud frame with the local point cloud map, and obtains the pose estimation result of the current point cloud frame. The local point cloud map is based on the splicing of historical point cloud frames in the sliding window and is obtained by applying a factor graph for optimization. The optimization variables of the factor graph are the pose and facet features of the historical point cloud frames. Through the above configuration, the embodiment of the present application can simultaneously optimize the pose and facet features of the historical point cloud frames added to the sliding window, so that when the current point cloud frame is aligned with the local point cloud map, it can effectively reduce the cumulative error, obtain a more accurate pose estimation result, and improve the accuracy of the laser radar odometer.

[0068] The process of obtaining a local point cloud map is further explained below.

[0069] In one implementation of the embodiment of the present application, a local point cloud map may be obtained according to the following steps S201 to S203:

[0070] Step S201: Obtain the pose of the historical point cloud frame within the sliding window.

[0071] In this embodiment, step S201 may further include the following steps S2011 to S2013:

[0072] Step S2011: Obtain the initial pose of the historical point cloud frame based on other odometers of the smart device.

[0073] In this embodiment, the initial pose of the historical point cloud frame can be obtained based on other odometry of the smart device, such as a visual odometry, a wheel odometry, etc.

[0074] Step S2012: According to the initial pose, the historical point cloud frame is registered with the local point cloud map to obtain the distance residual between the surface element features between the historical point cloud frame and the local point cloud map.

[0075] In this embodiment, the current historical point cloud frame can be matched with the local point cloud map (scan to map) according to the initial pose to obtain the distance residual between the surface element features of the current historical point cloud frame and the surface element features of the local point cloud map.

[0076] Step S2013: Optimize the initial pose according to the distance residual to obtain the pose of the historical point cloud frame, and add the historical point cloud frame to the sliding window according to the pose.

[0077] In this embodiment, the distance residual can be applied to optimize the initial pose, thereby obtaining the pose of the historical point cloud frame, and the historical point cloud frame is added to the sliding window.

[0078] Step S202: Obtain an initial point cloud map based on the pose and historical point cloud frames.

[0079] In this embodiment, step S202 may further include the following steps S2021 to S2023:

[0080] Step S2021: stitching the historical point cloud frames according to the pose to obtain a stitched point cloud map.

[0081] Step S2022: voxelize the spliced ​​point cloud map to obtain a voxelized point cloud map.

[0082] Step S2023: construct an octree based on the voxelized point cloud map to obtain an initial point cloud map.

[0083] In this embodiment, the historical point cloud frames can be spliced ​​together according to the posture, and the spliced ​​point cloud map can be voxelized to obtain a voxelized point cloud map. Based on the voxelized point cloud map, an octree is constructed to facilitate subsequent search indexing to obtain an initial point cloud map.

[0084] Step S203: Apply the factor graph to optimize the pose and facet features of the historical point cloud frames contained in the initial point cloud map to obtain a local point cloud map.

[0085] In this embodiment, a factor graph can be applied to optimize the pose and surfel features of historical point cloud frames in the initial point cloud map to obtain a local point cloud map. Conventional factor graph-based optimization methods in the field of autonomous driving can be used to optimize the pose and surfel features of each historical point cloud frame within the sliding window based on the factor graph, which is not specifically limited in this embodiment.

[0086] In one embodiment, a factor graph may be constructed according to the following steps 301 to S303:

[0087] Step 301: setting variable nodes of the factor graph; the variable nodes include pose variable nodes corresponding to the pose of the historical point cloud frame and surface element feature variable nodes corresponding to the surface element features.

[0088] Step 302: setting the constraint items of the variable nodes; each pose variable node is respectively provided with at least one constraint item for constraining the pose; each face feature variable node is respectively provided with at least one constraint item for constraining the face feature.

[0089] Step 303: Construct a factor graph based on the variable nodes and constraint items.

[0090] In this embodiment, the variable nodes of the factor graph can be the pose variable nodes corresponding to the pose of the historical point cloud frame and the face feature variable nodes corresponding to the face feature, and at least one constraint item is set for each variable node, so as to construct a factor graph based on the variable nodes and constraint items.

[0091] In one implementation, step S203 may be further configured as follows:

[0092] Based on the factor graph, a sliding window optimization is performed on the pose and surface element features of the historical point cloud frames contained in the initial point cloud map. After the sliding optimization, each constraint item is optimized separately to form an optimized factor graph. When the next sliding window optimization is performed, the pose and surface element features within the sliding window can be optimized based on the optimized factor graph.

[0093] In this embodiment, the posture and facet features within the sliding window can be optimized, and each constraint item can be optimized to form an optimization factor graph, and the posture and facet features within the sliding window can be optimized based on the optimization factor graph during the next sliding window optimization.

[0094] In one implementation, the residual of each constraint item may be obtained, and the constraint item may be optimized to form an optimized factor graph.

[0095] In one embodiment, the Jacobian matrix of the residual with respect to the optimization variable can be calculated, and each constraint term can be optimized based on the residual and the Jacobian matrix. The optimization of the constraint terms can be achieved by applying commonly used methods in the art for optimizing based on residuals and Jacobian matrices, which are not limited in this application.

[0096] In one embodiment, the least squares method can be applied to optimize each constraint item based on the residual and the Jacobian matrix, thereby optimizing the pose and facet features within the sliding window.

[0097] In one embodiment, when the number of historical point cloud frames in the sliding window is greater than the preset window size, the oldest historical point cloud frame can be deleted and a new historical point cloud frame can be added to repeat the above-mentioned local point cloud map acquisition process.

[0098] In one embodiment, please refer to Figure 3, which is a flowchart of the main steps of obtaining a local point cloud map according to an embodiment of the present application. As shown in Figure 3, when obtaining a local point cloud map, the initial pose of the historical point cloud frame can be obtained based on the odometer from other sources, and the pose of the current frame (current historical point cloud frame) can be optimized based on the distance residual between the current historical point cloud frame and the map, and the optimized current frame is added to the sliding window. Determine whether the number of frames in the sliding window reaches the threshold (preset sliding window size). If not, continue to obtain new historical point cloud frames. If so, splice the historical point cloud frames in the sliding window into a local map, construct an octree map, construct a factor graph that simultaneously optimizes the pose and facet features of multiple frames of historical point cloud frames in the sliding window, perform factor graph optimization, delete the oldest frame of historical point cloud frame, and continue to obtain new historical point cloud frames.

[0099] It should be pointed out that although the various steps in the above embodiments are described in a specific order, those skilled in the art will understand that in order to achieve the effect of the present application, different steps do not have to be performed in such an order. They can be performed simultaneously (in parallel) or in other orders. These changes are within the scope of protection of the present application.

[0100] It will be understood by those skilled in the art that all or part of the processes in the method for implementing the above embodiment of the present application can also be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a computer-readable storage medium. When the computer program is executed by a processor, it can implement the steps of each of the above method embodiments. The computer program includes computer program code, which can be in source code form, object code form, executable file or some intermediate form. The computer-readable storage medium can include: any entity or device, medium, USB flash drive, mobile hard disk, magnetic disk, optical disk, computer memory, read-only memory, random access memory, electric carrier signal, telecommunication signal and software distribution medium that can carry the computer program code. It should be noted that the content contained in the computer-readable storage medium can be appropriately increased or decreased according to the requirements of legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, computer-readable storage media do not include electric carrier signals and telecommunication signals.

[0101] Furthermore, the present application also provides a computer-readable storage medium. In a computer-readable storage medium embodiment according to the present application, the computer-readable storage medium can be configured to store a program for executing the pose estimation method of the above-mentioned method embodiment, and the program can be loaded and run by the processor to implement the above-mentioned pose estimation method. For ease of explanation, only the parts related to the embodiment of the present application are shown. For specific technical details not disclosed, please refer to the method part of the embodiment of the present application. The computer-readable storage medium can be a storage device formed by various electronic devices. Optionally, the computer-readable storage medium in the embodiment of the present application is a non-temporary computer-readable storage medium.

[0102] Furthermore, the present application also provides an intelligent device, which may include at least one processor; and a memory communicatively connected to the at least one processor; wherein a computer program is stored in the memory, and when the computer program is executed by the at least one processor, the method described in any of the above embodiments is implemented. The intelligent device of the present application may include driving devices, smart cars, robots and other devices. Referring to Figure 4, Figure 4 is a schematic diagram of the connection relationship between the memory and the processor in the intelligent device according to an embodiment of the present application. Figure 4 exemplarily shows that the memory and the processor are communicatively connected via a bus.

[0103] In some embodiments of the present application, the smart device further includes at least one sensor for sensing information. The sensor is communicatively coupled to any of the types of processors described herein. Optionally, the smart device further includes an autonomous driving system for guiding the smart device to drive autonomously or with assistance. The processor communicates with the sensor and / or autonomous driving system to perform the method described in any of the above embodiments.

[0104] Furthermore, it should be understood that since the configuration of each module is merely for the purpose of illustrating the functional units of the apparatus of the present application, the physical devices corresponding to these modules may be the processor itself, or a portion of the software in the processor, a portion of the hardware, or a combination of software and hardware. Therefore, the number of modules in the figure is merely illustrative.

[0105] Those skilled in the art will appreciate that the various modules in the device can be adaptively split or merged. Such splitting or merging of specific modules will not cause the technical solution to deviate from the principles of this application. Therefore, the technical solutions after splitting or merging will fall within the scope of protection of this application.

[0106] The relevant user personal information that may be involved in the various embodiments of this application is strictly in accordance with the requirements of laws and regulations, following the principles of legality, legitimacy and necessity, and based on the reasonable purposes of business scenarios, to process the personal information that users actively provide during the use of products / services or generated due to the use of products / services, as well as the personal information obtained with the user's authorization.

[0107] The user personal information processed in this application will vary depending on the specific product / service scenario and will be based on the specific scenario in which the user uses the product / service. This may involve the user's account information, device information, driving information, vehicle information, or other related information. The applicant will treat the user's personal information and its processing with a high degree of diligence.

[0108] This application attaches great importance to the security of user personal information and has taken reasonable and feasible security protection measures that comply with industry standards to protect user information and prevent personal information from being accessed, disclosed, used, modified, damaged or lost without authorization.

[0109] Thus far, the technical solutions of the present application have been described in conjunction with the preferred embodiments shown in the accompanying drawings. However, it is readily understood by those skilled in the art that the scope of protection of the present application is obviously not limited to these specific embodiments. Without departing from the principles of the present application, those skilled in the art may make equivalent changes or substitutions to the relevant technical features, and the technical solutions after such changes or substitutions will fall within the scope of protection of the present application.

Claims

1. A pose estimation method, characterized in that, The method includes: Obtaining a current point cloud frame collected by a lidar set on an intelligent device; Registering the current point cloud frame with a local point cloud map to obtain a pose estimation result of the current point cloud frame; Wherein, the local point cloud map is obtained by stitching historical point cloud frames within a sliding window based on a preset window size and optimizing using a factor graph; The optimization variables of the factor graph include the poses and surface element features of the historical point cloud frames.

2. The pose estimation method according to claim 1, characterized in that, The method further includes obtaining the local point cloud map according to the following steps: Obtaining the poses of the historical point cloud frames within the sliding window; Obtaining an initial point cloud map according to the poses and the historical point cloud frames; Applying the factor graph to optimize the poses and surface element features of the historical point cloud frames included in the initial point cloud map to obtain the local point cloud map.

3. The pose estimation method according to claim 2, characterized in that, The method further includes constructing the factor graph according to the following steps: Setting variable nodes of the factor graph; the variable nodes include pose variable nodes corresponding to the poses of the historical point cloud frames and surface element feature variable nodes corresponding to the surface element features; Setting constraint terms of the variable nodes; each pose variable node is respectively provided with at least one constraint term for constraining the pose; each surface element feature variable node is respectively provided with at least one constraint term for constraining the surface element feature; Constructing the factor graph according to the variable nodes and the constraint terms.

4. The pose estimation method according to claim 3, characterized in that, The applying the factor graph to optimize the poses and surface element features of the historical point cloud frames included in the initial point cloud map includes: Based on the factor graph, for the historical point cloud frames included in the initial point cloud map Performing sliding window optimization on the poses and the surface element features, and respectively optimizing each constraint term after the sliding optimization to form an optimized factor graph, and being able to optimize the poses and the surface element features within the sliding window based on the optimized factor graph when performing the next sliding window optimization.

5. The pose estimation method according to claim 4, characterized in that, The respectively optimizing each constraint term to form an optimized factor graph includes: Obtaining the residuals of each constraint term; Respectively optimizing each constraint term according to the residuals of each constraint term to form the optimized factor graph.

6. The pose estimation method according to claim 5, characterized in that, The respectively optimizing each constraint term according to the residuals of each constraint term includes: Obtaining the Jacobian matrix of the residuals with respect to the optimization variables according to the residuals of each constraint term; Optimizing each constraint term according to the residuals and the Jacobian matrix.

7. The pose estimation method according to claim 6, characterized in that, The optimizing each constraint term according to the residuals and the Jacobian matrix includes: Applying the least squares method to optimize each constraint term according to the residuals and the Jacobian matrix.

8. The pose estimation method according to claim 2, characterized in that, The obtaining an initial point cloud map according to the poses and the historical point cloud frames includes: Stitching the historical point cloud frames according to the poses to obtain a stitched point cloud map; Voxelizing the stitched point cloud map to obtain a voxelized point cloud map; Constructing an octree according to the voxelized point cloud map to obtain the initial point cloud map.

9. The pose estimation method according to claim 2, wherein, Obtaining the pose of the historical point cloud frame within the sliding window includes: Obtaining an initial pose of the historical point cloud frame according to other odometers of the intelligent device; Registering the historical point cloud frame with the local point cloud map according to the initial pose, and obtaining a distance residual between surface element features between the historical point cloud frame and the local point cloud map; Optimizing the initial pose according to the distance residual to obtain the pose of the historical point cloud frame, and adding the historical point cloud frame to the sliding window according to the pose.

10. The pose estimation method according to any one of claims 1 to 9, wherein, The method further includes obtaining the surface element features according to the following steps: Taking the 3D coordinates of the nearest point on the surface element to the origin of the current coordinate system as the surface element feature of the surface element.

11. The pose estimation method according to any one of claims 1 to 10, wherein, The method further includes: When the number of historical point cloud frames in the sliding window is greater than the preset window size, deleting the oldest historical point cloud frame.

12. A computer-readable storage medium, in which multiple program codes are stored, wherein, The program code is adapted to be loaded and run by a processor to execute the pose estimation method according to any one of claims 1 to 11.

13. An intelligent device, wherein, Including: At least one processor; And a memory communicatively connected to the at least one processor; Wherein, a computer program is stored in the memory, and when the computer program is executed by the at least one processor, the pose estimation method according to any one of claims 1 to 11 is implemented.

Images