Pose estimation method, related device and storage medium

CN116929343BActive Publication Date: 2026-08-07BEIJING SEMIDRIVE TECHNOLOGY CO LTD
View PDF 2 Cites 0 Cited by

Patent Information

Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
BEIJING SEMIDRIVE TECHNOLOGY CO LTD
Filing Date
2023-06-30
Publication Date
2026-08-07

AI Technical Summary

Technical Problem

而往往运算能力强的硬件的造价高,不利于支出成本的节省

Benefits of technology

[0021] Compared with related technologies, the pose estimation algorithm in this application is easy to implement, does not require high computing power, is a lightweight algorithm, and can effectively avoid the problem of increased costs caused by purchasing hardware with high computing power, thus saving costs.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN116929343B_ABST
    Figure CN116929343B_ABST
Patent Text Reader

Abstract

The application discloses a pose estimation method, related equipment and a storage medium, wherein the method comprises the following steps: obtaining a relative pose quantity of a to-be-measured object at a tth acquisition time based on inertial measurement unit (IMU) data of the to-be-measured object at the tth acquisition time and at a previous time; obtaining a pose prior quantity at the tth acquisition time based on the relative pose quantity and a pose optimization quantity at a pth acquisition time; wherein t and p are positive numbers, and the pth acquisition time is a previous acquisition time of the tth acquisition time; obtaining a pose estimation quantity of the to-be-measured object at the tth acquisition time based on the pose prior quantity at the tth acquisition time and an observation quantity at the tth acquisition time; and positioning the to-be-measured object based on the pose estimation quantity of the to-be-measured object at the tth acquisition time. Lightweight pose estimation can be realized.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] This application relates to the field of positioning, and more particularly to a pose estimation method, related equipment, and storage medium. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) can solve the problem of localization of objects (such as robots, drones, and logistics vehicles) in unknown environments. Specifically, it constructs a map of the surrounding environment using data collected by sensors, while simultaneously estimating the object's pose. The pose estimation algorithms in related technologies require high computing power, necessitating powerful hardware to support their normal operation. However, such powerful hardware is often expensive, hindering cost savings. Summary of the Invention

[0003] This application provides a pose estimation method, related equipment, and storage medium to at least solve the above-mentioned technical problems existing in the prior art.

[0004] According to a first aspect of this application, a pose estimation method is provided, comprising:

[0005] Based on the inertial measurement unit (IMU) data of the object under test at acquisition time t and the time before it, the relative pose of the object under test at acquisition time t is obtained.

[0006] Based on the relative pose and the pose optimization at time p, the pose prior at time t is obtained; where t and p are both positive numbers, and time p is the previous time before time t.

[0007] Based on the prior pose at acquisition time t and the observations at acquisition time t, the pose estimate of the object under test at acquisition time t is obtained.

[0008] Based on the pose estimate of the object under test at time t, the object under test is located.

[0009] Wherein, the pose optimization amount at the p-th acquisition time is obtained by, when the optimization conditions are met, based on the target optimization amount of at least one of the pose estimates at M acquisition times and the relative pose amount between the at least one pose estimate and the pose estimate at the p-th acquisition time; wherein, M is a positive integer greater than or equal to 1, and the M acquisition times are earlier than the p-th acquisition time; the target optimization amount is obtained by optimizing the at least one pose estimate.

[0010] According to a second aspect of this application, a pose estimation device is provided, comprising:

[0011] The first acquisition unit is used to obtain the relative pose of the object under test at acquisition time t based on the inertial measurement unit (IMU) data of the object under test at acquisition time t and the data of the object at previous times.

[0012] The second acquisition unit is used to obtain the prior pose value at acquisition time t based on the relative pose value and the pose optimization value at acquisition time p; where t and p are both positive numbers, and acquisition time p is the acquisition time before acquisition time t.

[0013] The third acquisition unit is used to obtain the pose estimate of the object under test at the acquisition time t based on the pose prior at the acquisition time t and the observation at the acquisition time t.

[0014] The localization unit is used to locate the object under test based on the pose estimate of the object under test at acquisition time t.

[0015] Wherein, the pose optimization amount at the p-th acquisition time is obtained by, when the optimization conditions are met, based on the target optimization amount of at least one of the pose estimates at M acquisition times and the relative pose amount between the at least one pose estimate and the pose estimate at the p-th acquisition time; wherein, M is a positive integer greater than or equal to 1, and the M acquisition times are earlier than the p-th acquisition time; the target optimization amount is obtained by optimizing the at least one pose estimate.

[0016] According to a third aspect of this application, an electronic device is provided, comprising:

[0017] At least one processor; and

[0018] A memory communicatively connected to the at least one processor; wherein,

[0019] The memory stores instructions that can be executed by the at least one processor to enable the at least one processor to perform the method described in this application.

[0020] According to a fourth aspect of this application, a non-transitory computer-readable storage medium is provided storing computer instructions for causing the computer to perform the methods described in this application.

[0021] Compared with related technologies, the pose estimation algorithm in this application is easy to implement, does not require high computing power, is a lightweight algorithm, and can effectively avoid the problem of increased costs caused by purchasing hardware with high computing power, thus saving costs.

[0022] It should be understood that the description in this section is not intended to identify key or essential features of the embodiments of this application, nor is it intended to limit the scope of this application. Other features of this application will become readily apparent from the following description. Attached Figure Description

[0023] The above and other objects, features, and advantages of exemplary embodiments of this application will become readily apparent from the following detailed description taken in conjunction with the accompanying drawings. Several embodiments of this application are illustrated in the drawings by way of example and not limitation, in which:

[0024] In the accompanying drawings, the same or corresponding reference numerals indicate the same or corresponding parts.

[0025] Figure 1 This paper illustrates the implementation flow of the pose estimation method in the embodiments of this application. Figure 1 ;

[0026] Figure 2 This paper illustrates the implementation flow of the pose estimation method in the embodiments of this application. Figure 2 ;

[0027] Figure 3 This paper illustrates the implementation flow of the pose estimation method in the embodiments of this application. Figure 3 ;

[0028] Figure 4 This paper illustrates the implementation flow of the pose estimation method in the embodiments of this application. Figure 4 ;

[0029] Figure 5 This application shows a rendering of the map in an embodiment of the map.

[0030] Figure 6 A schematic diagram of the composition structure of the pose estimation device in an embodiment of this application is shown;

[0031] Figure 7 A schematic diagram of the composition structure of the electronic device in an embodiment of this application is shown. Detailed Implementation

[0032] To make the objectives, features, and advantages of this application more apparent and understandable, the technical solutions in the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0033] To make the objectives, technical solutions, and advantages of this application clearer, the application will be further described in detail below with reference to the accompanying drawings. The described embodiments should not be regarded as limitations on this application. All other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0034] In the following description, references are made to “some embodiments,” which describe a subset of all possible embodiments. However, it is understood that “some embodiments” may be the same subset or different subsets of all possible embodiments and may be combined with each other without conflict.

[0035] In the following description, the terms "first" and "second" are used merely to distinguish similar objects and do not represent a specific ordering of objects. It is understood that "first" and "second" may be interchanged in a specific order or sequence where permitted, so that the embodiments of this application described herein can be implemented in an order other than that illustrated or described herein.

[0036] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs. The terminology used herein is for the purpose of describing embodiments of this application only and is not intended to limit this application.

[0037] It should be understood that in the various embodiments of this application, the sequence number of each implementation process does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of this application.

[0038] The pose estimation method in this application can be applied to any device that needs to estimate pose, such as robots, drones, logistics vehicles, underwater exploration robots, and driving equipment such as automobiles. The pose estimation algorithm in this application combines the relative pose at acquisition time t and the optimized pose at acquisition time p to obtain the prior pose at acquisition time t. It then combines the prior pose at acquisition time t with the observed values ​​at acquisition time t to obtain the estimated pose of the object at acquisition time t. Compared with related pose estimation algorithms that require high computational power, the pose estimation algorithm in this application is lightweight, easy to implement, and has low computational power requirements. It also effectively avoids the increased costs caused by purchasing high-performance hardware, thus saving costs.

[0039] Figure 1 This paper illustrates the implementation flow of the pose estimation method in the embodiments of this application. Figure 1 .like Figure 1 As shown, the method includes:

[0040] S101: Based on the inertial measurement unit (IMU) data of the object under test at acquisition time t and the time before it, obtain the relative pose of the object under test at acquisition time t.

[0041] The object to be measured can be any object capable of pose estimation, such as a robot, drone, logistics vehicle, underwater exploration robot, or driving equipment like a car. Driving equipment is a preferred choice for the object to be measured in this application.

[0042] Typically, an Inertial Measurement Unit (IMU) is installed on the object under test. The IMU has a specific acquisition frequency. Data is acquired according to this frequency. The reciprocal of the acquisition frequency is the acquisition period. The time within each acquisition period is considered the acquisition time. In this application, the t-th acquisition time refers to any acquisition time other than the initial acquisition time.

[0043] In this step, the IMU data acquired at each acquisition time is recorded. The IMU data of the object under test at acquisition time t is obtained by acquiring IMU data at acquisition time t. The IMU data of the object under test at times preceding acquisition time t is obtained by reading the recorded IMU data from all times prior to acquisition time t.

[0044] Typically, an IMU (Integrated Measurement Unit) includes an accelerometer and a gyroscope. The accelerometer measures the acceleration of the object in space, while the gyroscope measures its angular velocity in three-dimensional space. Using the principles of the IMU, the relative pose at acquisition time t is obtained by integrating the acceleration and angular velocity over time over the previous time steps. For a detailed explanation of the IMU principle, please refer to the relevant documentation.

[0045] It can be understood that the relative pose at acquisition time t is a relative quantity, which refers to the relative value of the pose at acquisition time t relative to the previous time.

[0046] By combining the IMU data acquired at time t with the IMU data from previous times, an accurate relative pose can be obtained.

[0047] S102: Based on the relative pose quantity and the pose optimization quantity at the p-th acquisition time, obtain the pose prior quantity at the t-th acquisition time; where t and p are both positive numbers, and the p-th acquisition time is the acquisition time preceding the t-th acquisition time; the pose optimization quantity at the p-th acquisition time is obtained by satisfying the optimization conditions, based on the target optimization quantity of at least one of the pose estimates at M acquisition times, and the relative pose quantity between the at least one pose estimate and the pose estimate at the p-th acquisition time; where M is a positive integer greater than or equal to 1, and the M acquisition times are earlier than the p-th acquisition time; the target optimization quantity is obtained by optimizing the at least one pose estimate.

[0048] In practice, the relative pose at acquisition time t and the pose optimization at acquisition time p (the acquisition time preceding acquisition time t) can be added together to obtain the pose prior at acquisition time t. The calculation of the pose prior is simple and requires low computing power.

[0049] In this application, satisfying the optimization condition means performing optimization once every q acquisition times, where q is a positive integer greater than or equal to 2. For example, optimization is performed every 30 (e.g., t1-t) acquisition times. 30 ) or 50 (e.g., t1-t) 30 The optimization is performed once for each set of pose estimates obtained at each acquisition time. In other words, the optimization conditions define the optimization cycle; optimization does not begin immediately after calculating the pose estimate at each acquisition time. This constraint on the optimization conditions improves optimization efficiency and avoids the resource burden caused by frequent optimizations.

[0050] The pose optimization amount at acquisition time p is obtained by optimizing the pose estimate at acquisition time p when the optimization conditions are met. Further, the target optimization amount of at least one of the pose estimates at M acquisition times, and the relative pose amount (between the at least one pose estimate and the pose estimate at acquisition time p) are added together, and the result of this addition is used as the pose optimization amount at acquisition time p.

[0051] The target optimization quantity in this application is obtained by optimizing at least one of the pose estimates using the GeorgiaTech Smoothing and Mapping (GTSAM) method, which is based on smoothing and mapping. Using the GTSAM method ensures the accuracy of the target optimization quantity, thereby guaranteeing the accuracy of the pose optimization quantity at acquisition time p, and ultimately enabling accurate calculation of the pose prior quantity at acquisition time t.

[0052] S103: Based on the prior pose at acquisition time t and the observations at acquisition time t, the pose estimate of the object under test at acquisition time t is obtained.

[0053] In practice, the prior pose at acquisition time t and the observation at acquisition time t are input into the error iterative Kalman filter (ESKF) system to obtain the pose estimate of the object under test at acquisition time t.

[0054] In this system, the ESKF calculates the Jacobian matrix based on the error state when determining the pose estimate. Compared to pose estimation algorithms in related technologies, the Jacobian matrix based on the error state is simpler to calculate and requires less computational power. This makes the pose estimate calculated by the error iterative Kalman filter algorithm a lightweight estimation algorithm, effectively saving computational and expenditure costs.

[0055] S104: Based on the pose estimate of the object under test at acquisition time t, the object under test is located.

[0056] In this application, pose includes position and orientation. Calculating the pose estimate of the object under test at acquisition time t can be considered as calculating the position and orientation of the object under test in the environment at acquisition time t, thereby realizing the localization of the object under test in its environment.

[0057] In S101 to S104, the scheme of combining the relative pose at acquisition time t and the pose optimization at acquisition time p to obtain the pose prior at acquisition time t; and the scheme of combining the pose prior at acquisition time t and the observation at acquisition time t to obtain the pose estimate of the object at acquisition time t, are algorithms with low computational requirements. Compared with pose estimation algorithms in related technologies that have high computational requirements, the pose estimation algorithm in this application has low computational requirements, is a lightweight algorithm, is easy to implement, and can effectively avoid the cost increase caused by purchasing hardware with high computing power, thus saving costs.

[0058] Furthermore, by employing accurate pose optimization parameters at acquisition time p and relative pose parameters at acquisition time t, accurate pose prior parameters at acquisition time t can be obtained. Based on these accurate pose prior parameters and accurate observations, accurate pose estimates are obtained, thereby achieving accurate estimation of the pose of the object under test.

[0059] In some embodiments, the aforementioned S104 method for locating the object under test based on the pose estimate of the object at acquisition time t is implemented through... Figure 2 The solution shown is to achieve this:

[0060] S201: Update the map based on the target optimization of the pose estimates of multiple previous times at the acquisition time t, wherein the map is established for the environment in which the object to be measured is located at multiple previous times.

[0061] In practice, the object under test is equipped with either radar or lidar. Lidar can collect point cloud data of the environment in which the object is located and construct an environment map based on this data. For example, the map can be constructed based on point cloud data collected at multiple previous acquisition times at acquisition time t. Since the object is in motion, its environment changes over time. Before acquisition time t arrives, the GTSAM method is used to optimize the pose estimates from multiple previous acquisition times, obtaining the target optimization values ​​for each previous pose estimate. At or before acquisition time t arrives, the map is updated based on the target optimization values ​​of the pose estimates from multiple previous acquisition times. For example, the pose estimates from previous acquisition times that were originally displayed on the map are adjusted to their target optimization values, so that the object under test is displayed on the map with the optimized pose before or at acquisition time t. If the map with the optimized pose is considered as the updated map, then before or at acquisition time t, the pose of the object under test at each previous time step is displayed in the updated map as the optimized pose (target optimization amount of the pose estimate) at each previous time step.

[0062] S202: Based on the pose estimate at acquisition time t and the updated map, locate the object to be measured.

[0063] In this step, the position and orientation of the object under test in the environment at acquisition time t are calculated, thus realizing the localization of the object under test in its environment. Furthermore, because this application can map the environment in which the object under test is located and update the map, after calculating the pose estimate of the object under test at acquisition time t, this pose estimate can also be displayed in the latest map (updated map) obtained before or at acquisition time t, thus displaying the pose at acquisition time t.

[0064] As can be seen from S201 to S202, the technical solution of this application is both a scheme for locating the pose of an object under test and a scheme for constructing a map. Therefore, the technical solution of this application can be regarded as a SLAM scheme, which can solve the problem of locating an object under test in an unknown environment. This scheme achieves real-time map updates and optimization of pose estimation, thereby achieving accurate positioning of the object under test.

[0065] In the SLAM scheme of this application, an error iterative Kalman filter system is used to calculate the pose estimate, achieving lightweight pose estimation. Simultaneously, the GTSAM method is used to optimize the pose estimate, improving optimization accuracy. Therefore, the SLAM scheme of this application is a low-computing-power, high-accuracy, and robust real-time localization and mapping (SLAM) solution.

[0066] In some embodiments, before performing step S103 to obtain the pose estimate of the object at acquisition time t based on the pose prior at acquisition time t and the observations at acquisition time t, it is also necessary to obtain the observations at acquisition time t. Combined with Figure 3 As shown, the methods for obtaining the observations at time t include:

[0067] S301: Obtain the target point cloud data at acquisition time t;

[0068] In this application, radar or lidar is used to scan point cloud data of the environment surrounding the object under test. The radar or lidar has a specific scanning frequency. The point cloud data is scanned according to this scanning frequency. The reciprocal of the scanning frequency is the scanning cycle. The time of each scanning cycle is considered the scanning time.

[0069] During implementation, initial point cloud data at acquisition time t, obtained from radar scanning, is acquired. For example, the point cloud data acquired by the radar at acquisition time t is read and used as the initial point cloud data at acquisition time t. Point cloud preprocessing and distortion correction are then performed on the initial point cloud data at acquisition time t to obtain the target point cloud data at acquisition time t. Point cloud preprocessing includes point cloud data normalization, filtering (to remove noise), and downsampling. Point cloud distortion correction includes point cloud distortion detection and distortion correction.

[0070] In this application, point cloud data that has undergone point cloud preprocessing and distortion correction is used to construct a map. Accurate map construction can be achieved using the point cloud data after preprocessing and distortion correction.

[0071] Alternatively, the initial point cloud data at acquisition time t can be preprocessed to obtain the first point cloud data. Based on the pose prior at acquisition time t, the second point cloud data can be selected from the first point cloud data. Point cloud distortion processing can then be applied to the second point cloud data to obtain the target point cloud data at acquisition time t.

[0072] In this application, the object under test is in motion, and the radar or lidar is also in motion. The first point cloud data includes point clouds for the moving object under test and point clouds for the moving radar. The point cloud of the moving object under test will affect map construction, so it is necessary to delete the point cloud data for the moving object under test from the first point cloud data. The point cloud data of the moving object under test is the point cloud data that can reflect the pose prior at acquisition time t. By deleting the point cloud data that can reflect the pose prior at acquisition time t from the first point cloud data, the point cloud data for the moving radar is obtained, that is, the second point cloud data is obtained.

[0073] S302: For any point in the target point cloud data, determine multiple expected points corresponding to the arbitrary point from the map; wherein the map is established at a time earlier than the t-th acquisition time for the environment of the object under test, and the expected points are points in the map that satisfy a first condition and a second condition with respect to the arbitrary point. The first condition includes that the distance between each expected point and the arbitrary point is less than or equal to a first threshold, and the second condition includes that the multiple expected points can form a plane.

[0074] From the map constructed before time t, find multiple desired points whose distances from any point are less than or equal to a first threshold and which can form a plane.

[0075] S303: Based on each point in the target point cloud data and the multiple expected points corresponding to each point, obtain the observation at the t-th acquisition time.

[0076] The distance from each point in the target point cloud data to the plane formed by the multiple expected points corresponding to each point is taken as the observation at the t-th acquisition time.

[0077] The observation acquisition methods shown in S301 to S303 are easy to implement in engineering and highly feasible. They can provide a certain degree of assistance to the lightweight pose estimation scheme of this application.

[0078] The following is combined with Figure 4 and Figure 5 The technical solution of this application will be further explained.

[0079] For clarity, this application scenario uses the object under test as a driving device and the optimization condition as performing optimization once every 5 data acquisition times when the pose estimate is calculated. This example is merely illustrative; in actual applications, the value of q may be much larger than 5, but the implementation will be similar. Understanding the concept is sufficient.

[0080] IMU measures driving equipment at t q =Acceleration and angular velocity values ​​at time t5. Read the acceleration and angular velocity values ​​at time t5. q=Acceleration and angular velocity values ​​of all acquisition times (t1 to t4) before acquisition time t5. According to the IMU principle, the relative pose at acquisition time t5 can be obtained by integrating the acceleration and angular velocity at the previous acquisition times over time.

[0081] The relative pose at time t5 and the t-th time... p =t q-1 The pose optimization values ​​at acquisition time t4 are summed, and the sum is used as the pose prior value at acquisition time t5. This calculation of the pose prior value is simple and provides some support for the lightweight pose estimation scheme of this application. Since the optimization condition is set to perform optimization once every q = 5 acquisition times, the pose estimate at acquisition time t4 has not yet reached the optimization time and has not been optimized. Based on this, the pose optimization value at acquisition time t4 is taken as the pose estimate at acquisition time t4 to complete the calculation of the pose prior value at acquisition time t5. For example, the pose estimate at acquisition time t4 and the relative pose value at acquisition time t5 are summed, and the sum is used as the pose prior value at acquisition time t5.

[0082] The process of obtaining the above pose prior quantities is as follows: Figure 4 The process executed by the forward propagation module in the process. Because the prior quantities of the object under test can reflect the motion information of the object under test, through... Figure 4 The backpropagation module in the system transmits the motion information of the object under test (IMU) from the IMU side to the radar side at a given acquisition time. This allows the IMU to remove the point cloud data representing the motion information of the IMU from the radar-scanned and pre-processed point cloud data based on the motion information of the IMU at a given acquisition time, thus avoiding the influence of the IMU's motion on the radar's motion.

[0083] The pose prior and the observation at acquisition time t5 are input into the ESKF system. It can be understood that the ESKF system is an iterative computation, through which the pose estimate is calculated. For the update process of the observation equation and the calculation error state of the ESKF system, please refer to formulas (1) to (4).

[0084] Assume the observation equation of the ESKF system is an abstract h(x):

[0085] z=h(x)+vv~N(0,V) (1)

[0086] Where z is the observation; v is the observation noise, which follows a random distribution with probability N; and V is the covariance matrix of the noise.

[0087] The update process for the error state calculation in the ESKF system is as follows:

[0088] K = P pred H T HP pred H T +V) -1 (2)

[0089] Δx=K(zh(x t (3)

[0090] P=(I-KH)P pred (4)

[0091] Where K is the Kalman gain; H is the Jacobian matrix of the observation equation relative to the error state, calculated using the chain rule. pred Let P be the system's covariance matrix. P is the predicted system covariance matrix. pred The covariance matrix of the system after correction.

[0092] In this application, the ESKF system can calculate the system's covariance matrix P based on the IMU data at acquisition time t and previous acquisition times. pred And the Jacobian matrix H. Then calculate the covariance matrix P. pred Given H and V, the Kalman gain is calculated according to formula (2). The pose prior at acquisition time t5 is used as h(x) in formula (3). t The observation at time t5 is used as z in formula (3), and the system state variable Δx is updated according to formula (3). At the same time, the system covariance matrix is ​​updated according to formula (4). The quaternion obtained by calculating the system state variable Δx is normalized and then converted into Euler angles for convenient pose display, which represent the pose state of the object under test. The ESFK system outputs the pose state as the pose estimate.

[0093] That is, when the prior pose and the observation at time t5 are input into the ESKF system, the ESKF system calculates and outputs the pose estimate at time t5 through the above process.

[0094] It can be seen that when calculating the pose estimate using the error iterative Kalman filter system, the Jacobian matrix based on the error state is calculated. Compared with pose estimation algorithms in related technologies, the Jacobian matrix based on the error state is simpler to calculate and requires less computing power. Therefore, the pose estimate calculated by the error iterative Kalman filter algorithm is a lightweight estimation algorithm, effectively saving computing power and expenditure costs.

[0095] In the aforementioned scheme, the process of obtaining the observations at time t5 is described below:

[0096] It is understandable that before the arrival of acquisition time t5, for the initial point cloud data acquired by the radar at acquisition time t1, the initial point cloud data at that acquisition time undergoes point cloud preprocessing, removal of moving point clouds of other moving objects besides the radar (such as the object under test), and point cloud distortion processing, before a map of the environment in which the object under test is located is constructed. For example, the point cloud data obtained at acquisition time t1 after the above steps is mapped to the global coordinate system to construct a map of the environment in which the object under test is located. In addition, the calculated pose estimate at acquisition time t1 can be displayed on the map to achieve the localization of the object under test at acquisition time t1.

[0097] When the t5 data collection time arrives, the map used can be either the map constructed based on the data collected at the t1 data collection time after the above steps, or it can be an updated map constructed at the t1 data collection time using point cloud data from the t2 to t4 data collection times. Let's take the example where the map used when the t5 data collection time arrives is the map constructed based on the data collected at the t1 data collection time after the above steps.

[0098] The radar scans point cloud data according to its scanning frequency. From the point cloud data scanned by the radar, the point cloud data at acquisition time t5 is read as the initial point cloud data at acquisition time t5. The initial point cloud data is preprocessed to obtain the first point cloud data at acquisition time t5. The pose prior at acquisition time t5 can reflect the motion state of the object under test. From the first point cloud data, point cloud data that can reflect the motion state of the object under test are deleted, thus obtaining the (second) point cloud data for the moving radar. For each point cloud in the second point cloud data at acquisition time t5, a certain number of expected points are found for each point cloud from the map that can be used when acquisition time t5 arrives. Taking point cloud 1 in the second point cloud data as an example, searching or finding 5 expected points, these 5 expected points need to be points in the map that meet the following two conditions (the first condition and the second condition). Specifically, each of the 5 expected points is the point closest to point cloud 1. That is, the distance between each point and point cloud 1 is less than or equal to the first threshold (satisfying the first condition). The 5 desired points can form a plane (satisfying the second condition).

[0099] Find 5 such desired points for each point cloud in the second point cloud data. Take the distance from each point cloud to the plane formed by the 5 desired points corresponding to each point cloud as the observation at the acquisition time t, and input it into the ESKF system.

[0100] Figure 4The residual calculation module is used to calculate the distance from each point cloud to the plane formed by the five expected points corresponding to each point cloud, and input the calculated distance as the residual into the ESKF system.

[0101] The pose estimate of the object under test is calculated at time t5 to determine its position and orientation within its environment. Alternatively, the pose estimate at time t5 can be displayed on a map to present the pose and orientation of the object under test, thus achieving localization of the object.

[0102] The aforementioned scheme is for estimating the pose of the object under test at acquisition time t5. When using the ESKF system to estimate the pose, the ESKF system calculates the Jacobian matrix based on the error state, which is simple to calculate and does not require high computing power, thus providing support for the lightweight calculation of pose estimation in this application.

[0103] Furthermore, due to the stability and accuracy of the ESKF system, the accuracy of the pose estimation can be guaranteed, thereby achieving precise positioning of the object under test.

[0104] Based on the optimization conditions or constraints, and using the aforementioned scheme to calculate the pose estimate at acquisition time t5, the pose estimates at acquisition times t1 to t5 can be optimized.

[0105] The pose estimates calculated at acquisition times t1 to t4 are cached. Given the pose estimate at acquisition time t5, the GTSAM method is used to optimize both the calculated pose estimate at acquisition time t5 and the previously cached pose estimates from acquisition times t1 to t4.

[0106] It is understandable that the GTSAM method is an optimization based on factor graphs. Factor graphs involve the concepts of nodes and edges. In this scheme, the pose estimates at each acquisition time can be considered as nodes in the factor graph. The constraint equations between the pose estimates at acquisition time can be considered as edges in the factor graph. These constraint equations include, but are not limited to: the pose estimates at two or more adjacent acquisition times are relatively close, such as being less than or equal to a second threshold; and the covariance of the pose estimates at one or more acquisition times is relatively small, such as being less than or equal to a third threshold. The optimization using factor graphs aims to continuously adjust the pose estimates at each acquisition time so that they satisfy the constraints of the constraint equations. When the constraint equations are satisfied, the adjusted pose estimates at each acquisition time can be considered as the optimized pose estimates at each acquisition time. The covariance of the adjusted optimized pose estimates at each acquisition time can be considered as the optimized covariance at each acquisition time.

[0107] In this application, the pose estimates at acquisition times t1 to t5 are optimized using the aforementioned GTSAM method to obtain the optimized pose estimates at acquisition times t1 to t5. The optimized pose estimates at each acquisition time are adjusted based on the pose estimates displayed on the map to update the map. The first, second, and third thresholds in this application are flexibly set according to the actual situation.

[0108] When the object under test, such as a driving device, includes a front end and a back end, the pose estimation scheme using the ESKF system can be performed by the front end of the driving device. The scheme using the GTSAM method to optimize the pose estimate can be performed by the back end of the driving device. Thus, this application provides a scheme for lightweight pose calculation at the front end and lightweight pose optimization at the back end. This scheme has low computational requirements and can effectively save costs. Furthermore, considering the strong stability of the ESKF system and the GTSAM method, accurate pose estimation and optimization can be achieved using these two methods.

[0109] In layman's terms, in this application, the front-end estimates the pose, and the back-end optimizes the estimated pose. Compared to the time spent on the front-end performing pose estimation, the time spent on the back-end optimizing the pose estimation is longer. For example, during the time it takes for the back-end to perform one optimization, the front-end may complete the pose estimation for multiple acquisition times. For instance, while the back-end is optimizing the pose estimation for acquisition times t1 to t5, the front-end may be calculating the pose estimation for acquisition times t6 to t9.

[0110] like Figure 4 As shown, the backend records the pose estimates from the frontend at acquisition times t1 to t5, along with the timestamps corresponding to each pose estimate. Given the timestamps for each pose estimate and the time taken for one optimization, the backend estimates the pose estimates for the frontend at several acquisition times after one optimization is completed, and then returns the pose optimization result for the frontend at which acquisition time.

[0111] Understandably, the front-end should next calculate the t-th... 10 The pose estimate at time t. Calculate the pose estimate at time t. 10The pose estimation at acquisition time t9 requires the pose optimization at acquisition time t9. In this application, the pose optimization at acquisition time t9 is obtained by comparing the relative (pose) value between the pose estimates at acquisition time t9 and acquisition time t5 with the pose optimization at acquisition time t5. Specifically, given the pose estimates at acquisition time t9 and acquisition time t5, the backend calculates the difference between the pose estimates at these two times, which can be considered as the relative (pose) value between the two times. This difference is then added to the pose optimization at acquisition time t5, and the result is considered the pose optimization at acquisition time t9. The backend feeds back the pose optimization at acquisition time t9 and the optimized covariance at acquisition time t9 to the frontend.

[0112] The optimized covariance at acquisition time t9 is obtained as follows: the difference between the covariances of the pose estimates at the two acquisition times is added to the covariance of the pose optimization at acquisition time t5, and the result can be regarded as the optimized covariance at acquisition time t9.

[0113] like Figure 4 As shown, the ESKF system in this application includes a Kalman observation iterative update module and a Kalman state update module. The Kalman observation iterative update module is used to update the system state variable Δx. The Kalman state update module receives the optimized covariance from the backend and provides this optimized covariance to the forward propagation module for calculating the pose prior at the next time step.

[0114] The front end reads the t-th measurement from the IMU. 10 The acceleration and angular velocity values ​​at the acquisition time and all preceding acquisition times (t1 to t9). Based on IMU principles, for the t-th... 10 Integrating the acceleration and angular velocity at the acquisition moment and the previous moment over time yields the result for the t-th moment. 10 The relative pose at the acquisition time. The t-th... 10 The relative pose at acquisition time t9 and the pose optimization at acquisition time t9 are added together, and the result is used as the t-th time. 10 The prior pose at the acquisition time. The t-th... 10 The prior pose at the acquisition time and the t-th time 10 The observations at the acquisition time are input into the ESKF system. The ESKF system calculates and outputs the t-th time using formulas (1) to (4). 10 Pose estimate at the time of data acquisition.

[0115] Among them, the tth 10 For the process of obtaining the observations at the acquisition time, please refer to the above description of the process of obtaining the observations at acquisition time t5. Repeated descriptions will not be repeated.

[0116] It is understandable that the t-th term is calculated at the front end. 10 Given the pose estimation at each acquisition time, and satisfying the optimization conditions, the backend uses the GTSAM method to perform pose estimation from t6 to t7. 10 The pose estimate at each acquisition time is optimized to obtain the optimized pose estimate at each sampling time, and then the map is updated. In this way, the front end calculates the pose estimate at each acquisition time, and the back end optimizes the pose estimate under the condition of meeting the optimization requirements and updates the map so that the pose of the object under test at each sampling time is displayed on the map.

[0117] It is understandable that if the pose optimization at a certain sampling time is calculated, the map can display the pose optimization at that sampling time. If it is not calculated, the map can display the pose estimate at that sampling time.

[0118] Figure 4 The loop closure detection module in the GTSAM method identifies whether the object under test has returned to a previously traversed position at a given acquisition moment, thus implementing loop closure detection technology. If it is detected that the object has returned to a previously traversed position at a given acquisition moment, the distance between the previously traversed position and the current position at that acquisition moment can be minimized. This distance is then added as an additional constraint, in addition to the aforementioned constraints, to the factor graph of the GTSAM method. This allows for the calculation of pose optimization quantities through more constraints.

[0119] In this application, a map is constructed of the environment in which the object under test is located, and the constructed map is updated to display the real-time pose of the object under test in the real-time environment in which the object is located. The effect of the map constructed or updated using the technical solution of this application is as follows: Figure 5 As shown. Considering the finiteness of radar scanning angle, the constructed or updated map is a map of a certain range around the environment in which the object under test is located. As the object under test moves, the environment displayed on the map also changes accordingly. That is, at all times, the map displays the pose of the object under test in its real-time environment.

[0120] As can be seen from the foregoing scheme, this application employs the ESKF system for lightweight pose estimation at the front end and the GTSAM method for accurate pose optimization at the back end. Because this scheme is lightweight, its computational requirements are lower than those of related technologies, and it can be considered a low-computational-power solution. Since the ESKF system and the GTSAM method have strong stability, the pose estimation and optimization achieve high accuracy and robustness. Therefore, this application provides a low-computational-power, high-precision, and highly robust pose estimation and optimization scheme.

[0121] This application provides two threads: a first thread and a second thread. The first thread operates at the front end and is used for lightweight pose estimation using the ESKF system. The second thread operates at the back end and is used for accurate pose optimization using the GTSAM method, map construction and updating, and loop closure detection. The two threads are independent and do not interfere with each other, ensuring their respective accuracy.

[0122] In terms of hardware, implementing the low-computing-power, high-precision, and robust pose estimation and optimization scheme of this application requires the support of hardware such as graphics processing units (GPUs), central processing units (CPUs), deep learning accelerators, vision accelerators, video memory, and general-purpose memory. Since the technical solution of this application is a lightweight computing solution, the performance and quantity requirements of these hardware components are not high. Ordinary performance and / or a small number of these hardware components are sufficient to implement the technical solution of this application. From a hardware support perspective, the technical solution of this application can also be considered a lightweight solution, avoiding the increased purchase expenditure caused by purchasing high-performance and large quantities of hardware, thus saving costs.

[0123] When the technical solution of this application is applied to driving equipment, the front end can estimate and locate the position and orientation (pose) of the driving equipment during driving. For the pose estimated by the front end, optimization is performed each time an optimization condition is met to ensure the accuracy of map construction or updating. Simultaneously, it facilitates the front end to use the optimized pose from previous data acquisition moments to calculate the pose at subsequent acquisition moments, thereby improving the positioning accuracy of the driving equipment at different acquisition moments.

[0124] This application provides a pose estimation device, such as... Figure 6 As shown, the device includes:

[0125] The first acquisition unit 701 is used to obtain the relative pose of the object under test at acquisition time t based on the inertial measurement unit (IMU) data of the object under test at acquisition time t and the data at previous times.

[0126] The second acquisition unit 702 is used to obtain the prior pose value at acquisition time t based on the relative pose value and the pose optimization value at acquisition time p; where t and p are both positive numbers, and acquisition time p is the acquisition time before acquisition time t.

[0127] The third acquisition unit 703 is used to obtain the pose estimate of the object under test at the acquisition time t based on the pose prior quantity at the acquisition time t and the observation quantity at the acquisition time t.

[0128] The positioning unit 704 is used to locate the object under test based on the pose estimate of the object under test at acquisition time t.

[0129] Wherein, the pose optimization amount at the p-th acquisition time is obtained by, when the optimization conditions are met, based on the target optimization amount of at least one of the pose estimates at M acquisition times and the relative pose amount between the at least one pose estimate and the pose estimate at the p-th acquisition time; wherein, M is a positive integer greater than or equal to 1, and the M acquisition times are earlier than the p-th acquisition time; the target optimization amount is obtained by optimizing the at least one pose estimate.

[0130] In some embodiments, the positioning unit 704 is used for:

[0131] The map is updated based on the target optimization quantity of multiple pose estimates at the t-th acquisition time, wherein the map is established for the environment in which the object under test is located at the multiple previous times.

[0132] Based on the pose estimate at acquisition time t and the updated map, the object to be measured is located.

[0133] In some embodiments, the third obtaining unit 703 is configured to:

[0134] Obtain the target point cloud data at time t;

[0135] For any point in the target point cloud data, determine multiple desired points corresponding to the arbitrary point from the map;

[0136] The map is established at a time earlier than the t-th acquisition time for the environment of the object under test. The expected point is a point in the map that satisfies a first condition and a second condition with respect to any point. The first condition includes that the distance between each expected point and any point is less than or equal to a first threshold. The second condition includes that multiple expected points can form a plane.

[0137] Based on each point in the target point cloud data and the multiple expected points corresponding to each point, the observations at the t-th acquisition time are obtained.

[0138] In some embodiments, the third obtaining unit 703 is used to: obtain the initial point cloud data at the t-th acquisition time scanned by the radar;

[0139] The initial point cloud data at acquisition time t is preprocessed and the point cloud distortion is corrected to obtain the target point cloud data at acquisition time t.

[0140] In some embodiments, the third obtaining unit 703 is configured to:

[0141] The initial point cloud data at time t is preprocessed to obtain the first point cloud data.

[0142] Based on the pose prior at time t, the second point cloud data is selected from the first point cloud data.

[0143] Point cloud distortion processing is performed on the second point cloud data to obtain the target point cloud data at acquisition time t.

[0144] In some embodiments, the second obtaining unit 702 is configured to:

[0145] The result of adding the relative pose quantity and the pose optimization quantity at acquisition time p is used as the pose prior quantity at acquisition time t.

[0146] In some embodiments, the third obtaining unit 703 is configured to:

[0147] The prior pose and the observation at acquisition time t are input into the error iterative Kalman filter system to obtain the pose estimate of the object under test at acquisition time t.

[0148] In some embodiments, the target optimization quantity is obtained by optimizing at least one of the pose estimates using a factor graph optimization library (gtsam) method based on smoothing and graph construction.

[0149] It should be noted that the pose estimation device in this application embodiment solves the problem in a similar way to the aforementioned pose estimation method. Therefore, the implementation process and implementation principle of the pose estimation device can be found in the description of the implementation process and implementation principle of the aforementioned method, and the repeated parts will not be repeated.

[0150] According to embodiments of this application, this application also provides an electronic device and a readable storage medium.

[0151] The electronic device includes: 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, the instructions being executed by the at least one processor to enable the at least one processor to perform the aforementioned pose estimation method.

[0152] For a non-transitory computer-readable storage medium storing computer instructions, the computer instructions are used to cause the computer to perform the aforementioned pose estimation method.

[0153] This application provides a driving device, including the aforementioned pose estimation device.

[0154] Figure 7A schematic block diagram of an example electronic device 800 that can be used to implement embodiments of this application is shown. The electronic device is intended to represent various forms of digital computers, such as laptop computers, desktop computers, workstations, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. The electronic device may also represent various forms of mobile devices, such as personal digital processors, cellular phones, smartphones, wearable devices, and other similar computing devices. The components shown herein, their connections and relationships, and their functions are merely illustrative and are not intended to limit the implementation of the application described and / or claimed herein.

[0155] like Figure 7 As shown, device 800 includes a computing unit 801, which can perform various appropriate actions and processes based on a computer program stored in read-only memory (ROM) 802 or a computer program loaded from storage unit 808 into random access memory (RAM) 803. RAM 803 may also store various programs and data required for the operation of device 800. The computing unit 801, ROM 802, and RAM 803 are interconnected via bus 804. Input / output (I / O) interface 805 is also connected to bus 804.

[0156] Multiple components in device 800 are connected to I / O interface 805, including: input unit 806, such as keyboard, mouse, etc.; output unit 807, such as various types of monitors, speakers, etc.; storage unit 808, such as disk, optical disk, etc.; and communication unit 809, such as network card, modem, wireless transceiver, etc. Communication unit 809 allows device 800 to exchange information / data with other devices through computer networks such as the Internet and / or various telecommunications networks.

[0157] The computing unit 801 can be a variety of general-purpose and / or special-purpose processing components with processing and computing capabilities. Some examples of the computing unit 801 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various special-purpose artificial intelligence (AI) computing chips, various computing units running machine learning model algorithms, a digital signal processor (DSP), and any suitable processor, controller, microcontroller, etc. The computing unit 801 performs the various methods and processes described above, such as pose estimation methods. For example, in some embodiments, the pose estimation method may be implemented as a computer software program tangibly contained in a machine-readable medium, such as storage unit 808. In some embodiments, part or all of the computer program may be loaded and / or installed on device 800 via ROM 802 and / or communication unit 809. When the computer program is loaded into RAM 803 and executed by the computing unit 801, one or more steps of the pose estimation method described above may be performed. Alternatively, in other embodiments, the computing unit 801 may be configured to perform the pose estimation method by any other suitable means (e.g., by means of firmware).

[0158] Various embodiments of the systems and techniques described above herein can be implemented in digital electronic circuit systems, integrated circuit systems, field-programmable gate arrays (FPGAs), application-specific integrated circuits (ASICs), application-specific standard products (ASSPs), systems-on-a-chip (SoCs), payload-programmable logic devices (CPLDs), computer hardware, firmware, software, and / or combinations thereof. These various embodiments may include implementations in one or more computer programs that can be executed and / or interpreted on a programmable system including at least one programmable processor, which may be a dedicated or general-purpose programmable processor, capable of receiving data and instructions from a storage system, at least one input device, and at least one output device, and transmitting data and instructions to the storage system, the at least one input device, and the at least one output device.

[0159] The program code used to implement the methods of this application may be written in any combination of one or more programming languages. This program code may be provided to a processor or controller of a general-purpose computer, special-purpose computer, or other programmable data processing device, such that when executed by the processor or controller, the functions / operations specified in the flowcharts and / or block diagrams are implemented. The program code may be executed entirely on a machine, partially on a machine, as a standalone software package partially on a machine and partially on a remote machine, or entirely on a remote machine or server.

[0160] In the context of this application, a machine-readable medium can be a tangible medium that may contain or store a program for use by or in conjunction with an instruction execution system, apparatus, or device. A machine-readable medium can be a machine-readable signal medium or a machine-readable storage medium. Machine-readable media can be, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatus, or devices, or any suitable combination of the foregoing. More specific examples of machine-readable storage media include electrical connections based on one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fibers, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the foregoing.

[0161] To provide interaction with a user, the systems and techniques described herein can be implemented on a computer having: a display device for displaying information to the user (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor); and a keyboard and pointing device (e.g., a mouse or trackball) through which the user provides input to the computer. Other types of devices can also be used to provide interaction with the user; for example, feedback provided to the user can be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user can be received in any form (including sound input, voice input, or tactile input).

[0162] The systems and technologies described herein can be implemented in computing systems that include backend components (e.g., as a data server), or computing systems that include middleware components (e.g., an application server), or computing systems that include frontend components (e.g., a user computer with a graphical user interface or web browser through which a user can interact with embodiments of the systems and technologies described herein), or any combination of such backend, middleware, or frontend components. The components of the system can be interconnected via digital data communication of any form or medium (e.g., a communication network). Examples of communication networks include local area networks (LANs), wide area networks (WANs), and the Internet.

[0163] Computer systems can include clients and servers. Clients and servers are generally located far apart and typically interact via communication networks. Client-server relationships are created by computer programs running on the respective computers and having a client-server relationship with each other. Servers can be cloud servers, servers in distributed systems, or servers incorporating blockchain technology.

[0164] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A pose estimation method, characterized in that, include: Based on the inertial measurement unit (IMU) data of the object under test at acquisition time t and the time before it, the relative pose of the object under test at acquisition time t is obtained. Based on the relative pose and the pose optimization at time p, the pose prior at time t is obtained; where t and p are both positive numbers, and time p is the previous time before time t. Based on the prior pose at acquisition time t and the observations at acquisition time t, the pose estimate of the object under test at acquisition time t is obtained. Based on the pose estimate of the object under test at time t, the object under test is located. Wherein, the pose optimization amount at the p-th acquisition time is obtained by satisfying the requirement that pose estimates at q acquisition times are calculated, based on the target optimization amount of at least one of the pose estimates at M acquisition times, and the relative pose amount between the at least one pose estimate and the pose estimate at the p-th acquisition time; where q is a positive integer greater than or equal to 2, M is a positive integer greater than or equal to 1, and the M acquisition times are earlier than the p-th acquisition time; the target optimization amount is obtained by optimizing the at least one pose estimate using the factor graph optimization library GTSAM method based on smoothing and graph construction.

2. The method according to claim 1, characterized in that, The localization of the object under test based on the pose estimate of the object at acquisition time t includes: The map is updated based on the target optimization quantity of multiple pose estimates at the t-th acquisition time, wherein the map is established for the environment in which the object under test is located at the multiple previous times. Based on the pose estimate at acquisition time t and the updated map, the object to be measured is located.

3. The method according to claim 1 or 2, characterized in that, The methods for obtaining the observations at time t include: Obtain the target point cloud data at time t; For any point in the target point cloud data, determine multiple desired points corresponding to the arbitrary point from the map; The map is established at a time earlier than the t-th acquisition time for the environment of the object under test. The expected point is a point in the map that satisfies a first condition and a second condition with respect to any point. The first condition includes that the distance between each expected point and any point is less than or equal to a first threshold. The second condition includes that multiple expected points can form a plane. Based on each point in the target point cloud data and the multiple expected points corresponding to each point, the observations at the t-th acquisition time are obtained.

4. The method according to claim 3, characterized in that, The acquisition of the target point cloud data at time t includes: Obtain the initial point cloud data at time t obtained by radar scanning; The initial point cloud data at acquisition time t is preprocessed and the point cloud distortion is corrected to obtain the target point cloud data at acquisition time t.

5. The method according to claim 4, characterized in that, The step of performing point cloud preprocessing and point cloud distortion processing on the initial point cloud data at acquisition time t to obtain the target point cloud data at acquisition time t includes: The initial point cloud data at time t is preprocessed to obtain the first point cloud data. Based on the pose prior at time t, the second point cloud data is selected from the first point cloud data. Point cloud distortion processing is performed on the second point cloud data to obtain the target point cloud data at acquisition time t.

6. The method according to claim 1 or 2, characterized in that, The process of obtaining the prior pose at acquisition time t based on the relative pose and the pose optimization at acquisition time p includes: The result of adding the relative pose quantity and the pose optimization quantity at acquisition time p is used as the pose prior quantity at acquisition time t.

7. The method according to claim 1 or 2, characterized in that, The pose prior and observations at acquisition time t are used to obtain the pose estimate of the object at acquisition time t, including: The prior pose and the observation at acquisition time t are input into the error iterative Kalman filter system to obtain the pose estimate of the object under test at acquisition time t.

8. A pose estimation device, comprising: The first acquisition unit is used to obtain the relative pose of the object under test at acquisition time t based on the inertial measurement unit (IMU) data of the object under test at acquisition time t and the data of the object at previous times. The second acquisition unit is used to obtain the prior pose value at acquisition time t based on the relative pose value and the pose optimization value at acquisition time p; where t and p are both positive numbers, and acquisition time p is the acquisition time before acquisition time t. The third acquisition unit is used to obtain the pose estimate of the object under test at the acquisition time t based on the pose prior at the acquisition time t and the observation at the acquisition time t. The localization unit is used to locate the object under test based on the pose estimate of the object under test at acquisition time t. Wherein, the pose optimization amount at the p-th acquisition time is obtained by satisfying the requirement that pose estimates at q acquisition times are calculated, based on the target optimization amount of at least one of the pose estimates at M acquisition times, and the relative pose amount between the at least one pose estimate and the pose estimate at the p-th acquisition time; where q is a positive integer greater than or equal to 2, M is a positive integer greater than or equal to 1, and the M acquisition times are earlier than the p-th acquisition time; the target optimization amount is obtained by optimizing the at least one pose estimate using the factor graph optimization library GTSAM method based on smoothing and graph construction.

9. A driving device, characterized in that, Includes the pose estimation device as described in claim 8.

10. An electronic device, characterized in that, The electronic device includes: At least one processor; and A memory communicatively connected to the at least one processor; wherein, The memory stores instructions that can be executed by the at least one processor to enable the at least one processor to perform the method of any one of claims 1-7.

11. A non-transitory computer-readable storage medium storing computer instructions, characterized in that, The computer instructions are used to cause the computer to perform the method according to any one of claims 1-7.

Citation Information

Patent Citations

  • Inertia measurement unit pose data optimization method and device, and electronic equipment

    CN111928847A

  • Information processing device, information processing method and program

    JP2023024042A