Position and posture estimation method, readable storage medium and intelligent device

Abstract

The present application relates to the field of autonomous driving technology, and specifically provides a pose estimation method, a readable storage medium, and an intelligent device, aiming to solve the problem of how to obtain a more accurate pose of a point cloud frame, thereby further improving the accuracy of the laser radar odometer. To this end, the present application obtains the current point cloud frame collected by the laser radar set on the intelligent 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, which can simultaneously optimize the pose and facet features of the historical point cloud frames added to the sliding window, and can effectively reduce the cumulative error during point cloud registration, obtain more accurate pose estimation results, and improve the accuracy of the laser radar odometer.

Description

Technical Field

[0001] 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

[0002] In autonomous driving technology, LiDAR odometry is increasingly used. LiDAR odometry estimates the transformation between consecutive point cloud frames to obtain the pose of the point cloud frame.

[0003] However, the existing lidar odometry mainly uses the point cloud feature values ​​observed at the current moment to estimate the motion state at the current moment, and aligns the current frame point cloud with the local point cloud map. It does not take into account the observation error and fixes the state at historical moments, which is prone to cumulative errors. It is not robust to some degraded scenarios, and the lidar odometry has poor accuracy and is prone to drift.

[0004] Accordingly, the field needs a new pose estimation solution to solve the above problems. Summary of the invention

[0005] 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 a point cloud frame, thereby further improving the accuracy of the lidar odometer.

[0006] In a first aspect, the present application provides a posture estimation method, the method comprising:

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

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

[0009] Wherein, 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 by applying a factor graph;

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

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

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

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

[0014] 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.

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

[0016] 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;

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

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

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

[0020] Based on the factor graph, sliding window optimization is performed on the pose of the historical point cloud frame and the face element features contained in the initial point cloud map, and each constraint item is optimized respectively after the sliding optimization to form an optimized factor graph, and the pose and the face element features in the sliding window can be optimized based on the optimized factor graph when the next sliding window optimization is performed.

[0021] In a technical solution of the above-mentioned pose estimation method,

[0022] The step of optimizing each constraint item separately to form an optimized factor graph includes:

[0023] Get the residual of each constraint;

[0024] According to the residual of each constraint item, each constraint item is optimized respectively to form the optimization factor graph.

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

[0026] According to the residual of each constraint item, obtaining the Jacobian matrix of the residual with respect to the optimization variable;

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

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

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

[0030] In a technical solution of the above-mentioned posture estimation method, obtaining an initial point cloud map according to the posture and the historical point cloud frame includes:

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

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

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

[0034] In a technical solution of the above-mentioned pose estimation method, the step of obtaining the pose of the historical point cloud frame in the sliding window includes:

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

[0036] According to the initial posture, the historical point cloud frame is registered with the local point cloud map to obtain the distance residual between the facet features of the historical point cloud frame and the local point cloud map;

[0037] 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.

[0038] In a technical solution of the above-mentioned pose estimation method, the method further includes obtaining the face element features according to the following steps:

[0039] The 3D coordinates of the point on the face element that is closest to the origin of the current coordinate system are used as the face element feature of the face element.

[0040] In a technical solution of the above-mentioned posture estimation method, the method further includes:

[0041] 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.

[0042] 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.

[0043] In a third aspect, a smart device is provided, characterized in that it includes:

[0044] at least one processor;

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

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

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

[0048] 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

[0049] The disclosure of the present application will become easier to understand with reference to the accompanying drawings. It is easy for those skilled in the art to understand that these drawings are only for illustrative purposes and are not intended to limit the scope of protection of the present application. Among them:

[0050] Figure 1 is a schematic flow chart of the main steps of a method for posture estimation according to an embodiment of the present application;

[0051] Figure 2 is a schematic diagram of facet parameterization according to an implementation of an embodiment of the present application;

[0052] Figure 3 It is a schematic diagram of the main steps of obtaining a local point cloud map according to an implementation of an embodiment of the present application;

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

[0054] 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 protection scope of the present application.

[0055] In the description of the present application, "module" and "processor" may include hardware, software or a combination of the two. A module may include hardware circuits, various suitable sensors, communication ports, memory, and may also include software parts, such as program code, or a combination of software and hardware. The processor may be a central processing unit, a microprocessor, an image processor, a digital signal processor or any other suitable processor. The processor has data and / or signal processing functions. The processor may be implemented in software, hardware or a combination of the two. Non-temporary computer-readable storage media include any suitable medium that can store program code, such as a disk, a hard disk, an optical disk, a flash memory, a read-only memory, a random access memory, etc. The term "A and / or B" means all possible combinations of A and B, such as only A, only B or A and B. The term "at least one A or B" or "at least one of A and B" has a similar meaning to "A and / or B" and may include only A, only B or A and B. The singular terms "one" and "the" may also include plural forms.

[0056] An automated driving system (ADS) refers to a system that will continuously perform all dynamic driving tasks (DDT) within its own operational domain design (ODD). That is, the machine system is allowed to fully assume the task of automatic vehicle control only under the specified appropriate driving scenario conditions, that is, the system is activated when the vehicle meets the ODD conditions, and the system replaces the human driver as the driving subject of the vehicle. Among them, the dynamic driving task DDT refers to the continuous lateral (left and right steering) and longitudinal motion control (acceleration, deceleration, constant speed) of the vehicle, as well as the detection and response to targets and events in the vehicle's driving environment. The design operating range ODD refers to the conditions under which the automated driving system can operate safely, and the conditions set may include geographic location, road type, speed range, weather, time, national and local traffic laws and regulations, etc.

[0057] See attached Figure 1 , Figure 1 FIG. 1 is a flow chart of the main steps of a method for posture estimation according to an embodiment of the present application. Figure 1 As shown, the posture estimation method in the embodiment of the present application mainly includes the following steps S101-S102.

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

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

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

[0061] 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 the historical point cloud frames within a sliding window of a preset window size and optimizing them with a factor graph; the optimization variables of the factor graph include the pose and facet features of the historical point cloud frames.

[0062] In this embodiment, the laser radar odometer can align the current point cloud frame with the local point cloud map to obtain the pose estimation result of the current point cloud frame. Among them, the local point cloud map is spliced ​​according to the historical frame point cloud collected by the laser radar and the pose of the historical frame point cloud, and is obtained by optimizing the factor graph based on the sliding window, and the optimization variables of the factor graph are the pose and face element features of the historical point cloud frame. That is, the historical point cloud frame can be modeled as multiple face elements, and the parameters representing each face element are used as the face element features of the historical point cloud frame. Those skilled in the art can set the value of the preset window size according to the needs of the actual application.

[0063] In one implementation, the facet feature may be a plane equation of each facet.

[0064] In one embodiment, please refer to the attached Figure 2 , Figure 2 FIG. 1 is a schematic diagram of a facet parameterization according to an implementation of an embodiment of the present application. Figure 2 As shown, the 3D coordinates of the closest point in the face element to the origin of the current coordinate system can be obtained, that is, Figure 2 In As the face element feature of the face element, the face element parameterization is realized. In this way, the parameter form can be minimized and the amount of calculation in the calculation process can be reduced while ensuring that the face element feature has real physical meaning.

[0065] 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.

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

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

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

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

[0070] Step S2011: Obtain the initial pose of the historical point cloud frame according to other odometers of the smart device.

[0071] In this embodiment, the initial position and posture of the historical point cloud frame can be obtained according to other odometers of the smart device, where the other odometers can be visual odometers, wheel odometers, etc.

[0072] Step S2012: According to the initial posture, the historical point cloud frame is aligned 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.

[0073] 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 posture to obtain the distance residual between the face element features of the current historical point cloud frame and the face element features of the local point cloud map.

[0074] 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.

[0075] In this embodiment, the distance residual can be applied to optimize the initial pose, so as to obtain the pose of the historical point cloud frame, and then add the historical point cloud frame to the sliding window.

[0076] Step S202: Obtain an initial point cloud map according to the posture and historical point cloud frames.

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

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

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

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

[0081] 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 indexes to obtain an initial point cloud map.

[0082] 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.

[0083] In this embodiment, the factor graph can be applied to optimize the pose and facet features of the historical point cloud frames in the initial point cloud map to obtain a local point cloud map. The conventional factor graph-based optimization method in the field of autonomous driving technology can be used to optimize the pose and facet features of each historical point cloud frame in the sliding window based on the above factor graph, which is not specifically limited in the embodiment of the present application.

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

[0085] 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 face feature variable nodes corresponding to the face feature.

[0086] Step 302: setting constraint items of 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.

[0087] Step 303: construct a factor graph according to the variable nodes and constraint items.

[0088] In this embodiment, the variable nodes of the factor graph can be pose variable nodes corresponding to the pose of the historical point cloud frame and 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.

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

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

[0091] In this embodiment, the posture and face element 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 face element features within the sliding window can be optimized based on the optimization factor graph during the next sliding window optimization.

[0092] 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.

[0093] In one embodiment, the Jacobian matrix of the residual with respect to the optimization variable can be calculated, and each constraint item can be optimized according to the residual and the Jacobian matrix. Among them, the optimization method based on the residual and the Jacobian matrix commonly used in the art can be applied to achieve the optimization of the constraint item, which is not limited in this application.

[0094] In one implementation, the least squares method may 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.

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

[0096] In one embodiment, please refer to the attached Figure 3 , Figure 3 FIG. 1 is a flow chart of the main steps of obtaining a local point cloud map according to an implementation of an embodiment of the present application. Figure 3As shown in the figure, 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. It is determined 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, and 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 frames, and continue to obtain new historical point cloud frames.

[0097] 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 can understand that in order to achieve the effect of the present application, different steps do not have to be executed in such an order. They can be executed simultaneously (in parallel) or in other orders. These changes are within the scope of protection of the present application.

[0098] It is understood by those skilled in the art that all or part of the processes in the method for implementing the above-mentioned embodiment of the present application can also be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a computer-readable storage medium. When the computer program is executed by the processor, the steps of each of the above-mentioned method embodiments can be implemented. Among them, the computer program includes computer program code, and the computer program code can be in source code form, object code form, executable file or some intermediate form. The computer-readable storage medium may include: any entity or device, medium, U disk, mobile hard disk, 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.

[0099] 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.

[0100] Furthermore, the present application also provides an intelligent device, which may include at least one processor; and a memory 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 a driving device, a smart car, a robot, and other devices. Figure 4 , Figure 4 It is a schematic diagram of the connection relationship between the memory and the processor in a smart device according to an embodiment of the present application. Figure 4 The memory and the processor are exemplarily shown to be communicatively connected via a bus.

[0101] In some embodiments of the present application, the smart device further includes at least one sensor, which is used to sense information. The sensor is communicatively connected to any type of processor mentioned in the present application. Optionally, the smart device further includes an autonomous driving system, which is used to guide the smart device to drive itself or assist in driving. The processor communicates with the sensor and / or the autonomous driving system to complete the method described in any of the above embodiments.

[0102] Further, it should be understood that since the setting of each module is only for illustrating the functional units of the device of the present application, the physical devices corresponding to these modules may be the processor itself, or a part of the software in the processor, a part of the hardware, or a part of the combination of software and hardware. Therefore, the number of each module in the figure is only schematic.

[0103] It is understood by those skilled in the art that each module 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 principle of the present application, and therefore, the technical solutions after splitting or merging will fall within the protection scope of the present application.

[0104] 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, 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.

[0105] The user personal information processed by 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. It 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.

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

[0107] So 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 easy for those skilled in the art to understand that the protection scope 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 can make equivalent changes or substitutions to the relevant technical features, and the technical solutions after these changes or substitutions will fall within the protection scope of the present application.

Claims

1. A pose estimation method, characterized in that: The method comprises: Get the current point cloud frame collected by the LiDAR set on the smart device; Aligning the current point cloud frame with the 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 together historical point cloud frames within a sliding window of a preset window size and optimizing it by applying a factor graph; The optimization variables of the factor graph include the pose and facet features of the historical point cloud frame; The local point cloud map is obtained according to the following steps: Obtaining the pose of the historical point cloud frame within the sliding window; Obtaining an initial point cloud map according to the posture and the historical point cloud frame; Applying the factor graph, optimizing 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; The factor graph is constructed according to the following steps: 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; Setting constraints on the variable nodes; each of the pose variable nodes is respectively provided with at least one constraint item for constraining the pose; each of the face element feature variable nodes is respectively provided with at least one constraint item for constraining the face element feature; The factor graph is constructed according to the variable nodes and the constraint items.

2. The method for posture estimation according to claim 1, characterized in that: The 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: Based on the factor graph, sliding window optimization is performed on the pose of the historical point cloud frame and the face element features contained in the initial point cloud map, and each constraint item is optimized respectively after the sliding optimization to form an optimized factor graph, and the pose and the face element features in the sliding window can be optimized based on the optimized factor graph when the next sliding window optimization is performed.

3. The method for posture estimation according to claim 2, characterized in that: The step of optimizing each constraint item separately to form an optimized factor graph includes: Get the residual of each constraint; According to the residual of each constraint item, each constraint item is optimized respectively to form the optimization factor graph.

4. The method for posture estimation according to claim 3, characterized in that: The step of optimizing each constraint item according to the residual of each constraint item comprises: According to the residual of each constraint item, obtaining the Jacobian matrix of the residual with respect to the optimization variable; Each constraint item is optimized according to the residual and the Jacobian matrix.

5. The method for posture estimation according to claim 4, characterized in that: The step of optimizing each constraint item according to the residual and the Jacobian matrix includes: A least square method is applied according to the residual and the Jacobian matrix to optimize each constraint item.

6. The method for posture estimation according to claim 1, characterized in that: The step of obtaining an initial point cloud map according to the posture and the historical point cloud frame comprises: splicing the historical point cloud frames according to the posture to obtain a spliced ​​point cloud map; voxelize the spliced ​​point cloud map to obtain a voxelized point cloud map; An octree is constructed according to the voxelized point cloud map to obtain the initial point cloud map.

7. The method for posture estimation according to claim 1, characterized in that: The obtaining the pose of the historical point cloud frame in the sliding window includes: Obtaining an initial pose of the historical point cloud frame according to other odometers of the smart device; According to the initial posture, the historical point cloud frame is registered with the local point cloud map to obtain the distance residual between the facet features of the historical point cloud frame and the local point cloud map; 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.

8. The method for posture estimation according to any one of claims 1 to 7, characterized in that: The method further comprises obtaining the facet features according to the following steps: The 3D coordinates of the point on the face element that is closest to the origin of the current coordinate system are used as the face element feature of the face element.

9. The method for posture estimation according to any one of claims 1 to 8, characterized in that: The method further comprises: 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.

10. A computer-readable storage medium storing a plurality of program codes, characterized in that: The program code is suitable for being loaded and run by a processor to execute the pose estimation method according to any one of claims 1 to 9.

11. A smart device, characterized in that: include: at least one processor; and, a memory communicatively coupled 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 posture estimation method described in any one of claims 1 to 9 is implemented.

Images