Pose determination methods and related devices, electronic equipment and storage media

Abstract

This application discloses a pose determination method and related devices, electronic devices, and storage media. The pose determination method includes: constructing a Kalman filter equation based on the state variables of an inertial measurement unit (IMU), and correcting the IMU's state variables based on error state variables to obtain a target state variable of the IMU; determining whether to update the pose state of an odometry based on the fusion result between the target state variable and point cloud data from a point cloud collector; adjusting the first observation in the Kalman filter equation to obtain a new Kalman filter equation in response to the determination to update the odometry's pose state; predicting a new error state variable based on the new Kalman filter equation, and re-executing the step of correcting the IMU's state variables based on the error state variable and subsequent steps until it is determined that no pose state update is needed, and using the latest pose state as the target pose of the odometry. This scheme can improve the accuracy of the pose state.

Description

Technical Field

[0001] This application relates to the field of data processing technology, and in particular to a pose determination method and related apparatus, electronic equipment and storage medium. Background Technology

[0002] In recent years, laser-inertial odometry (LIO) has developed rapidly, and can be specifically divided into graph optimization methods and Kalman filter-based methods.

[0003] Currently, real-time acceleration, angular velocity, and position information are typically acquired from sensors such as IMUs (Inertial Measurement Units) and RTKs (Real-time Kinematic) to construct a Kalman filter model, predicting the positional relationship at the next moment. The predicted values ​​are then corrected through precise point cloud matching, and the Kalman gain equation is updated. The real-time matching result is then superimposed onto the pose from the previous moment. However, as time accumulates, the trajectory of the pose superposition becomes increasingly longer. The random error in IMU measurements increases over time, and the accuracy of RTK positioning results decreases due to factors such as occlusion, causing point cloud matching to fail or fail. Consequently, the system cannot update the Kalman gain equation, ultimately leading to inaccurate pose predictions. Therefore, improving the accuracy of pose state updates has become an urgent problem to be solved. Summary of the Invention

[0004] The main technical problem addressed by this application is to provide a pose determination method and related devices, electronic devices, and storage media that can improve the accuracy of pose state.

[0005] To address the aforementioned technical problems, the first aspect of this application provides a pose determination method, comprising: constructing a Kalman filter equation based on the state variables of an inertial measurement unit (IMU), and correcting the state variables of the IMU based on error state variables to obtain a target state variable of the IMU; wherein the error state variable is predicted based on the Kalman filter equation; and determining whether to update the pose state of the odometry based on the fusion result between the target state variable and the point cloud data from the point cloud collector; in response to determining that the pose state of the odometry needs to be updated, adjusting the first observation in the Kalman filter equation to obtain a new Kalman filter equation; wherein the first observation represents the observation corresponding to the IMU in the Kalman filter equation; based on this, predicting a new error state variable based on the new Kalman filter equation, and re-executing the step of correcting the state variables of the IMU based on the error state variable and subsequent steps until it is determined that the pose state does not need to be updated, and using the latest pose state as the target pose of the odometry.

[0006] To address the aforementioned technical problems, a second aspect of this application provides a pose determination device, comprising a construction module, an update module, an adjustment module, and a prediction module. The construction module constructs a Kalman filter equation based on the state variables of an inertial measurement unit (IMU), and corrects the IMU's state variables based on error state variables to obtain a target state variable for the IMU. The error state variable is predicted based on the Kalman filter equation. The update module determines whether to update the odometry's pose state based on the fusion result between the target state variable and point cloud data from a point cloud collector. The adjustment module adjusts the first observation in the Kalman filter equation in response to the determination to update the odometry's pose state to obtain a new Kalman filter equation. The first observation represents the observation corresponding to the IMU in the Kalman filter equation. The prediction module predicts a new error state variable based on the new Kalman filter equation and re-executes the steps of correcting the IMU's state variables based on the error state variable and subsequent steps until it is determined that no pose state update is needed. The latest pose state is then used as the target pose of the odometry.

[0007] To address the aforementioned technical problems, a third aspect of this application provides an electronic device, including a memory and a processor coupled to each other. The memory stores program instructions, and the processor executes the program instructions to implement the pose determination method described in the first aspect.

[0008] To address the aforementioned technical problems, a fourth aspect of this application provides a computer-readable storage medium storing program instructions executable by a processor, the program instructions being used to implement the pose determination method described in the first aspect.

[0009] The above scheme constructs a Kalman filter equation based on the state variables of the inertial measurement unit (IMU), and corrects the IMU's state variables based on the error state variables to obtain the target state variables of the IMU. The error state variables are predicted based on the Kalman filter equation. Based on the fusion result between the target state variables and the point cloud data from the point cloud collector, it is determined whether to update the odometry pose state. In response to the determination to update the odometry pose state, the first observation in the Kalman filter equation is adjusted to obtain a new Kalman filter equation. The first observation represents the observation corresponding to the IMU in the Kalman filter equation. Based on this, a new error state variable is predicted based on the new Kalman filter equation, and the steps of correcting the IMU's state variables based on the error state variable and subsequent steps are repeated until it is determined that no pose state update is needed. The latest pose state is used as the odometer. The target pose of the odometry is determined through two methods. First, by correcting the state variables of the inertial measurement unit (IMU) based on the error state variables, the accuracy of the IMU's target state variables is improved. Second, by fusing the target state variables with the point cloud data from the point cloud collector, it is determined whether to update the odometry's pose state. In response to this determination, the first observation in the Kalman filter equation is adjusted to obtain a new Kalman filter equation. This process can be continuously updated to obtain new Kalman filter equations. Based on these new equations, new error state variables are predicted, and the steps of correcting the IMU's state variables based on the error state variables and subsequent steps are repeated until it is determined that no pose state update is needed. The latest pose state is used as the target pose of the odometry, which helps reduce errors through repeated iterations. Therefore, the accuracy of the pose state can be improved.

[0010] It should be understood that the above general description and the following detailed description are exemplary and explanatory only, and are not intended to limit this application. Attached Figure Description

[0011] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with this application and, together with the specification, serve to explain the technical solutions of this application.

[0012] Figure 1 This is a flowchart illustrating an embodiment of the pose determination method of this application;

[0013] Figure 2 This is a flowchart illustrating another embodiment of the pose determination method of this application;

[0014] Figure 3 This is a schematic diagram of the frame of an embodiment of the pose determination device of this application;

[0015] Figure 4This is a schematic diagram of the framework of an embodiment of the electronic device of this application;

[0016] Figure 5 This is a schematic diagram of a framework of an embodiment of the computer-readable storage medium of this application. Detailed Implementation

[0017] The embodiments of this application will now be described in detail with reference to the accompanying drawings.

[0018] In the following description, specific details such as particular system architectures, interfaces, and technologies are presented for illustrative purposes rather than for limiting purposes, in order to provide a thorough understanding of this application.

[0019] In this document, the term "and / or" is merely a description of the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A alone, A and B simultaneously, or B alone. Additionally, the character " / " generally indicates that the preceding and following related objects are in an "or" relationship. Furthermore, "many" in this document means two or more. Moreover, the term "at least one" in this document means any combination of at least two of any one or more of a plurality of objects. For example, including at least one of A, B, and C can mean including any one or more elements selected from the set consisting of A, B, and C. "Several" means at least one. The terms "first," "second," etc., in the specification, claims, and accompanying drawings are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence.

[0020] Please see Figure 1 , Figure 1 This is a flowchart illustrating an embodiment of the pose determination method of this application.

[0021] Specifically, this may include the following steps:

[0022] Step S11: Based on the state variables of the inertial measurement unit, construct the Kalman filter equation, and based on the error state variables, correct the state variables of the inertial measurement unit to obtain the target state variables of the inertial measurement unit.

[0023] In this embodiment of the disclosure, the error state quantity is predicted based on the Kalman filter equation. The Kalman filter uses the linear system state equation to predict the system state through observation data and obtain the error state quantity.

[0024] In one implementation scenario, state variables can be acquired through an inertial measurement unit (IMU). For example, the vehicle's acceleration v = (v_accelerated / decelerated) can be detected by the IMU. x ,v y ,v z ) and angular velocity ω=(ω x ,ωy ,ω z Information such as acceleration and angular velocity can also be obtained through a gyroscope. The method for acquiring state variables can be determined based on the actual situation and is not specifically limited here.

[0025] In one implementation scenario, Kalman filter equations are constructed based on the state variables of the inertial measurement unit (IMU). Specifically, the kinematic equations and the error Kalman filter equations are defined and initialized based on the state variables of the IMU. For example, the noise from the IMU readings is used as the error state variable for the Kalman filter, resulting in the continuous kinematic equations, which can be expressed as follows:

[0026]

[0027]

[0028]

[0029]

[0030]

[0031]

[0032] Among them, the state variables Let represent the derivative with respect to time, (·) ∧ Let b be the antisymmetric matrix representation of the vector. a and b g This represents the random noise of the acceleration and inertial measurement units. Furthermore, since the readings of the state quantities are all discrete, the discrete representation can be obtained from the continuous kinematic equations as follows:

[0033] δp(t+Δt)=δp+δvΔt

[0034]

[0035]

[0036] δb g (t+Δt)=δb g +η g

[0037] δb a (t+Δt)=δb a +η a

[0038] δg(t+Δt)=δg

[0039] After obtaining the Kalman filter equation, the error state variables are predicted based on the Kalman filter equation. For example, where i is the measurement index of the inertial measurement unit frame and x is the state variable of the inertial measurement unit, the error state equation can be expressed as:

[0040]

[0041] Where Q is the covariance matrix with respect to the error state noise w.

[0042] Furthermore, based on the error state quantity, the state quantity of the inertial measurement unit (IMU) is corrected to obtain the target state quantity of the IMU. As one possible implementation, the error state quantity can be predicted using the Kalman filter equation and used as a fixed error state quantity. The state quantity of the IMU is then corrected using this fixed error state quantity. Unlike the previously disclosed implementation, the state quantity of the IMU can be pre-integrated to obtain a predicted state quantity. The target state quantity of the IMU is then obtained based on the difference between the predicted state quantity and the error state quantity. For example, the Jacobian matrix F can be calculated first, and can be expressed as:

[0043]

[0044] Based on this, the state variables of the inertial measurement unit are pre-integrated to obtain the predicted state variables. These predicted state variables can represent the unit's own pose state, for example, using 6-dimensional data (x, y, z, pitch angle, roll angle, yaw angle). For example, the expression can be:

[0045] δx p =Fδx

[0046] P p =FPF T +Q

[0047] Where the superscript p represents the predicted state variable, P p Let represent the predicted covariance matrix, and Q represent the system noise matrix. Based on the difference between the predicted state variables and the error state variables, the target state variables of the inertial measurement unit (IMU) are obtained. It is understood that the predicted state variables may contain errors; therefore, the error state variables are removed to maximize the accuracy of the target state variables of the IMU. The above method obtains the predicted state variables by pre-integrating the state variables of the IMU, and then obtains the target state variables based on the difference between the predicted and error state variables. Finally, the error state variables are removed from the predicted state variables to correct the state variables of the IMU, thereby maximizing the accuracy of the target state variables.

[0048] Step S12: Based on the fusion result between the target state variable and the point cloud data from the point cloud collector, determine whether to update the pose state of the odometry.

[0049] In one implementation scenario, point cloud data can be collected using a point cloud collector, which can be, but is not limited to, LiDAR, binocular cameras, etc.

[0050] In one implementation scenario, to determine whether to update the odometry pose state, the target state variable can be fused with point cloud data to obtain fused data. The resulting multi-frame fused data is then stitched together to obtain a first stitched map. Further, the first stitched map is matched with the latest point cloud data to obtain a matching result. Based on whether the matching result converges, it is determined whether to update the odometry pose state.

[0051] In another implementation scenario, unlike the aforementioned implementation, motion compensation can be performed on the point cloud data based on the target state variables to obtain target point cloud data. Motion compensation using the target state variables can remove motion distortion caused by the relative motion of the point cloud data following the device. Since distortion affects the accuracy of the measured point cloud data—specifically, when the device speed is high, distortion can introduce significant errors, thus affecting the precision of the point cloud data—motion compensation using the target state variables can eliminate the point cloud distortion problem caused by the offset of the point cloud collector relative to itself during the acquisition process. After obtaining the target point cloud data, a first stitched map is obtained based on it. As one possible implementation, the target point cloud data can be stitched together to obtain the first stitched map. After generating new target point cloud data, the new target point cloud data is updated to the first stitched map, and so on, continuously updating the first stitched map as new target point cloud data is generated. Unlike the aforementioned implementation, this method determines whether the sliding window is full. If the sliding window is full, the historical target point cloud data at the bottom of the sliding window is discarded, and the second stitched map is updated. The second stitched map is obtained by stitching together the historical target point cloud data within the sliding window before the update, thus obtaining the first stitched map. It is understood that historical target point cloud data exists within the sliding window, and this historical target point cloud data is obtained before the sliding window is determined. Furthermore, the historical target point cloud data within the sliding window is not generated simultaneously; therefore, the historical target point cloud data at the bottom of the sliding window is the first to enter the sliding window, and can be continuously updated along with the target point cloud data. If the sliding window is not full, the second stitched map is updated to obtain the first stitched map. Specifically, when the sliding window is not full, target point cloud data is added to the second stitched map, thereby updating the second stitched map and obtaining the first stitched map. The above method, by determining whether the sliding window is filled, determines how to update the second mosaic map and thus obtain the first mosaic map, which helps to improve the accuracy of the first mosaic map and thus improve its real-time performance.

[0052] Further, the first stitched map and the latest point cloud data are matched to determine whether to update the odometry pose state. As one possible implementation, matching the first stitched map and the latest point cloud data involves matching the historical target point cloud data in the first stitched map with the latest point cloud data. The ratio of successfully matched data in the target point cloud data is obtained and compared with a preset threshold. If the ratio is greater than the preset threshold, the odometry pose state is updated; otherwise, the fusion result based on the target state quantity and the point cloud data from the point cloud collector is re-executed to determine whether to update the odometry pose state and subsequent steps. This differs from the aforementioned implementation, where the first stitched map and the latest point cloud data are matched to obtain a matching result. Understandably, the first stitched map is obtained by stitching together target point cloud data. The point cloud data is updated based on the acquisition frequency of the point cloud collector. The first stitched map is then matched with the latest point cloud data to obtain a matching result. This result can be the percentage of successful matches or the first successful match value; the specific matching result can be determined based on the actual situation and is not limited here. The odometry pose state is then updated based on whether the matching result converges. That is, if the matching result has converged, the odometry pose state is updated; if the matching result has not converged, the fusion result based on the target state variable and the point cloud data from the point cloud collector is re-executed to determine whether to update the odometry pose state and subsequent steps. Furthermore, by matching the first stitched map with the latest point cloud data to obtain a matching result, and then determining whether to update the odometry pose state based on whether the matching result converges, it helps to improve the odometry pose state. The above method obtains target point cloud data by performing motion compensation on point cloud data based on target state variables. This helps to fuse target state variables and point cloud data. The first stitched map is obtained through the target point cloud data, which helps to improve the real-time performance of the first stitched map. The first stitched map is then matched with the latest point cloud data to determine whether to update the odometry pose state, thereby improving the accuracy of the odometry pose state.

[0053] Step S13: In response to determining the pose state of the updated odometer, adjust the first observation in the Kalman filter equation to obtain a new Kalman filter equation.

[0054] In this embodiment of the disclosure, the first observation represents the observation corresponding to the inertial measurement unit in the Kalman filter equation. It is understood that the first observation can be the pose offset obtained by matching point cloud data. For example, the pose offset between the first stitched map and the latest point cloud data can be obtained. The pose offset can be represented by 6-dimensional data (x, y, z, pitch angle, roll angle, yaw angle), that is, it can be the pose offset between the latest point cloud data and the corresponding historical target point cloud data in the first stitched map.

[0055] In one implementation scenario, given the determined pose state of the updated odometry, the first observation in the Kalman filter equation is adjusted to obtain a new Kalman filter equation. Specifically, given the determined pose state of the updated odometry, the first observation in the Kalman filter equation can be adjusted based on the matching result between the first stitched map and the latest point cloud data, i.e., the fitness of the matching result. The first observation can be the covariance matrix of the observation noise; the higher the matching result fitness, the smaller the covariance matrix of the observation noise, and vice versa. For example, it can be represented as follows:

[0056]

[0057] Where z represents the observed data, n represents the observed noise, and N represents the covariance matrix of the observed noise.

[0058] In one implementation scenario, positioning data from a positioning device can be acquired. This positioning data can include the device's latitude and longitude information and attitude angle information. Furthermore, the positioning device can be, but is not limited to, an RTK measuring instrument, a GPS measuring instrument, etc., without specific limitations. Based on the positioning data, the second observation in the latest Kalman filter equation is adjusted. The second observation can be the covariance matrix of the observation noise, and it characterizes the observation corresponding to the positioning device in the Kalman filter equation. For example, the positioning device can be an RTK measuring instrument. The system can determine the state bit output by the RTK measuring instrument. If the state bit is a fixed solution, the covariance matrix of the observation noise is decreased; if the state bit is a floating-point solution or a single-point solution, the covariance matrix of the observation noise is increased. It is understandable that the status bit output by the RTK measuring instrument represents a fixed solution, indicating that the correct position coordinates have been calculated with an accuracy in the centimeter range. The status bit output by the RTK measuring instrument represents a floating-point solution, indicating that a fixed solution has not yet been calculated, with an accuracy in the centimeter to meter range. The status bit output by the RTK measuring instrument represents a single-point solution, indicating that the RTK receiver has not received a differential signal, and only the satellite positioning result is used as the output, with an accuracy in the meter range. The above method, by adjusting the second observation in the latest Kalman filter equation based on positioning data, helps to improve the accuracy of the second observation in the latest Kalman filter equation.

[0059] Step S14: Based on the new Kalman filter equation, predict the new error state quantity, and re-execute the step of correcting the state quantity of the inertial measurement unit based on the error state quantity and subsequent steps until it is determined that there is no need to update the pose state. Use the latest pose state as the target pose of the odometry.

[0060] In one implementation scenario, it is detected whether the observations in the new Kalman filter equation have been adjusted. In response to the adjusted observations in the new Kalman filter equation, a new error state quantity is predicted based on the new Kalman filter equation. Specifically, it can be detected whether the first or second observation in the new Kalman filter equation has been adjusted. If at least one of the first and second observations has been adjusted, a new error state quantity is predicted based on the new Kalman filter equation. For example, the Jacobian matrix of the observation equation compared to the error state can be obtained first, and its expression can be represented as follows:

[0061]

[0062] Furthermore, the Kalman gain matrix and the new error state variables are calculated, and their expressions can be represented as follows:

[0063] K = P p H T HP p H T +N) -1

[0064] δx=K[zh(x)]

[0065] P=(I-KH)P p

[0066] Where K is the Kalman gain, P is the covariance matrix of the adjusted observation noise, and δx is the new error state quantity. In response to the fact that the observations in the new Kalman filter equation have not been adjusted, the fusion result based on the target state quantity and the point cloud data from the point cloud collector is re-executed to determine whether to update the odometry pose state and subsequent steps. Specifically, the fusion result based on the target state quantity and the point cloud data from the point cloud collector can be re-executed to determine whether to update the odometry pose state until a new error state quantity is predicted based on the new Kalman filter equation. Then, the state quantity of the inertial measurement unit (IMU) is corrected using the new error state quantity to obtain the target state quantity of the IMU, and this process is repeated continuously to update the odometry pose state. This method, by detecting whether the observations in the new Kalman filter equation have been adjusted, and thus determining whether a new error state quantity has been predicted, helps to improve the accuracy of the new error state quantity.

[0067] In one implementation scenario, based on the new Kalman filter equation, a new error state quantity is predicted. The steps of correcting the state quantity of the inertial measurement unit based on the error state quantity and subsequent steps are repeated until it is determined that no pose state update is needed. The latest pose state is then used as the target pose of the odometry.

[0068] The above scheme constructs a Kalman filter equation based on the state variables of the inertial measurement unit (IMU), and corrects the IMU's state variables based on the error state variables to obtain the target state variables of the IMU. The error state variables are predicted based on the Kalman filter equation. Based on the fusion result between the target state variables and the point cloud data from the point cloud collector, it is determined whether to update the odometry pose state. In response to the determination to update the odometry pose state, the first observation in the Kalman filter equation is adjusted to obtain a new Kalman filter equation. The first observation represents the observation corresponding to the IMU in the Kalman filter equation. Based on this, a new error state variable is predicted based on the new Kalman filter equation, and the steps of correcting the IMU's state variables based on the error state variable and subsequent steps are repeated until it is determined that no pose state update is needed. The latest pose state is used as the odometer. The target pose of the odometry is determined through two methods. First, by correcting the state variables of the inertial measurement unit (IMU) based on the error state variables, the accuracy of the IMU's target state variables is improved. Second, by fusing the target state variables with the point cloud data from the point cloud collector, it is determined whether to update the odometry's pose state. In response to this determination, the first observation in the Kalman filter equation is adjusted to obtain a new Kalman filter equation. This process can be continuously updated to obtain new Kalman filter equations. Based on these new equations, new error state variables are predicted, and the steps of correcting the IMU's state variables based on the error state variables and subsequent steps are repeated until it is determined that no pose state update is needed. The latest pose state is used as the target pose of the odometry, which helps reduce errors through repeated iterations. Therefore, the accuracy of the pose state can be improved.

[0069] Please see Figure 2 , Figure 2This is a flowchart illustrating another embodiment of the pose determination method of this application. IMU state variables can be acquired via an IMU, which may include information such as acceleration and angular velocity. Pre-integration is performed on the IMU state variables to obtain predicted state variables. Based on the difference between the predicted state variables and the error state variables, the target state variables of the inertial measurement unit are obtained. Point cloud data is acquired via a lidar. Motion compensation is performed on the point cloud data based on the target state variables to obtain target point cloud data. Dynamic point clouds are then removed from the target point cloud data. The point cloud data with the dynamic point clouds removed is then stitched together to obtain a first stitched map. Specifically, it is determined whether the sliding window is filled. If the sliding window is filled, the historical target point cloud data at the bottom of the sliding window is discarded, and the second stitched map is updated to obtain the first stitched map. The second stitched map is obtained by stitching together the historical target point cloud data within the sliding window before the update. If the sliding window is not filled, the second stitched map is updated to obtain the first stitched map. The first stitched map is matched with the latest point cloud data to obtain a matching result. The matching result is then checked for convergence. If convergence fails, the fusion result based on the target state variable and the point cloud data from the point cloud collector is re-executed to determine whether the odometry pose state should be updated and a new matching result obtained. The convergence of this new matching result is then checked until convergence is achieved. If convergence is achieved, the odometry pose state is updated, and the first observation in the Kalman filter equation is adjusted. Furthermore, positioning data is acquired via RTK, which can be latitude and longitude coordinates. The second observation in the Kalman filter equation is adjusted using these latitude and longitude coordinates. The second observation can be the covariance matrix of the observation noise. Specifically, the latitude and longitude coordinates are assessed. If the latitude and longitude coordinates are a fixed solution, the covariance matrix of the observation noise is decreased; if the latitude and longitude coordinates are a floating-point solution or a single-point solution, the covariance matrix of the observation noise is increased. It is understood that adjusting either the first or second observation in the Kalman filter equation yields a new Kalman filter equation. Furthermore, it can be detected whether the first or second observation in the new Kalman filter equation has been adjusted. If at least one of the first or second observation has been adjusted, a new error state quantity is predicted based on the new Kalman filter equation, and the steps of correcting the state quantity of the inertial measurement unit based on the error state quantity and subsequent steps are re-executed until it is determined that there is no need to update the pose state. The latest pose state is then used as the target pose of the odometry.

[0070] The above scheme constructs a Kalman filter equation based on the state variables of the inertial measurement unit (IMU), and corrects the IMU's state variables based on the error state variables to obtain the target state variables of the IMU. The error state variables are predicted based on the Kalman filter equation. Based on the fusion result between the target state variables and the point cloud data from the point cloud collector, it is determined whether to update the odometry pose state. In response to the determination to update the odometry pose state, the first observation in the Kalman filter equation is adjusted to obtain a new Kalman filter equation. The first observation represents the observation corresponding to the IMU in the Kalman filter equation. Based on this, a new error state variable is predicted based on the new Kalman filter equation, and the steps of correcting the IMU's state variables based on the error state variable and subsequent steps are repeated until it is determined that no pose state update is needed. The latest pose state is used as the odometer. The target pose of the odometry is determined through two methods. First, by correcting the state variables of the inertial measurement unit (IMU) based on the error state variables, the accuracy of the IMU's target state variables is improved. Second, by fusing the target state variables with the point cloud data from the point cloud collector, it is determined whether to update the odometry's pose state. In response to this determination, the first observation in the Kalman filter equation is adjusted to obtain a new Kalman filter equation. This process can be continuously updated to obtain new Kalman filter equations. Based on these new equations, new error state variables are predicted, and the steps of correcting the IMU's state variables based on the error state variables and subsequent steps are repeated until it is determined that no pose state update is needed. The latest pose state is used as the target pose of the odometry, which helps reduce errors through repeated iterations. Therefore, the accuracy of the pose state can be improved.

[0071] Those skilled in the art will understand that, in the above-described method of the specific implementation, the order in which each step is written does not imply a strict execution order and does not constitute any limitation on the implementation process. The specific execution order of each step should be determined by its function and possible internal logic.

[0072] Please see Figure 3 , Figure 3This is a schematic diagram of the framework of an embodiment of the pose determination device of this application. The pose determination device 30 includes a construction module 31, an update module 32, an adjustment module 33, and a prediction module 34. The construction module 31 is used to construct a Kalman filter equation based on the state variables of the inertial measurement unit (IMU), and to correct the state variables of the IMU based on the error state variables to obtain the target state variables of the IMU; wherein the error state variables are predicted based on the Kalman filter equation. The update module 32 is used to determine whether to update the pose state of the odometer based on the fusion result between the target state variables and the point cloud data from the point cloud collector. The adjustment module 33 is used to adjust the first observation in the Kalman filter equation in response to the determination to update the pose state of the odometer, to obtain a new Kalman filter equation; wherein the first observation represents the observation corresponding to the IMU in the Kalman filter equation. The prediction module 34 is used to predict a new error state variable based on the new Kalman filter equation, and to re-execute the step of correcting the state variables of the IMU based on the error state variable and subsequent steps until it is determined that the pose state does not need to be updated, and to use the latest pose state as the target pose of the odometer.

[0073] The above scheme, on the one hand, corrects the state variables of the inertial measurement unit (IMU) based on the error state variables to obtain the target state variables of the IMU, which helps improve the accuracy of the target state variables of the IMU. On the other hand, it determines whether to update the odometry pose state by fusing the target state variables and the point cloud data from the point cloud collector. In response to the determination to update the odometry pose state, the first observation in the Kalman filter equation is adjusted to obtain a new Kalman filter equation. This can be continuously updated to obtain new Kalman filter equations. Based on the new Kalman filter equations, new error state variables are predicted, and the steps of correcting the state variables of the IMU based on the error state variables and subsequent steps are repeated until it is determined that the pose state does not need to be updated. The latest pose state is used as the target pose of the odometry, which helps to reduce errors through repeated iterations. Therefore, it can improve the accuracy of the pose state.

[0074] In some disclosed embodiments, the update module 32 includes a fusion submodule, a determination submodule, and a matching submodule. The fusion submodule performs motion compensation on the point cloud data based on the target state variable to obtain target point cloud data; the determination submodule obtains a first stitched map based on the target point cloud data; and the matching submodule matches the first stitched map with the latest point cloud data to determine whether to update the odometry pose state.

[0075] Therefore, by performing motion compensation on point cloud data based on target state variables to obtain target point cloud data, it is helpful to fuse target state variables and point cloud data. The first stitched map can be obtained through the target point cloud data, which helps to improve the real-time performance of the first stitched map. Furthermore, by matching the first stitched map with the latest point cloud data, it is determined whether to update the odometry pose state, thereby improving the accuracy of the odometry pose state.

[0076] In some disclosed embodiments, the determining submodule includes a judging unit, which is used to judge whether the sliding window is filled; in response to the sliding window being filled, the historical target point cloud data at the bottom of the sliding window is discarded, and the second stitched map is updated to obtain the first stitched map; wherein, the second stitched map is obtained by stitching based on the historical target point cloud data in the sliding window before the update; in response to the sliding window not being filled, the second stitched map is updated to obtain the first stitched map.

[0077] Therefore, by determining whether the sliding window is filled, and thus how to update the second mosaic map to obtain the first mosaic map, the accuracy of the first mosaic map can be improved, thereby improving the real-time performance of the first mosaic map.

[0078] In some disclosed embodiments, the matching submodule includes a matching unit and a determining unit. The matching unit is used to match the first stitched map with the latest point cloud data to obtain a matching result; the determining unit is used to determine whether to update the pose state of the odometer based on whether the matching result converges.

[0079] Therefore, by matching the first stitched map with the latest point cloud data to obtain the matching result, and then determining whether to update the odometry pose state based on whether the matching result converges, it helps to improve the odometry pose state.

[0080] In some disclosed embodiments, the pose determination device 30 includes an acquisition module and a data adjustment module. The acquisition module acquires positioning data from the positioning device; the data adjustment module adjusts the second observation in the latest Kalman filter equation based on the positioning data; wherein the second observation characterizes the observation corresponding to the positioning device in the Kalman filter equation.

[0081] Therefore, adjusting the second observation in the latest Kalman filter equation based on location data helps to improve the accuracy of the second observation in the latest Kalman filter equation.

[0082] In some disclosed embodiments, the prediction module 34 includes a detection submodule, which is used to detect whether the observables in the new Kalman filter equation have been adjusted; in response to the observables in the new Kalman filter equation being adjusted, a new error state quantity is predicted based on the new Kalman filter equation; in response to the observables in the new Kalman filter equation not being adjusted, the fusion result based on the target state quantity and the point cloud data from the point cloud collector is re-executed to determine whether to update the odometry pose state and subsequent steps.

[0083] Therefore, by detecting whether the observables in the new Kalman filter equation are adjusted, and thus determining whether a new error state quantity is predicted, it is helpful to improve the accuracy of the new error state quantity.

[0084] In some disclosed embodiments, the construction module 31 includes a calculation submodule and a determination submodule. The calculation submodule is used to perform pre-integration calculation on the state variables of the inertial measurement unit to obtain the predicted state variables; the determination submodule is used to obtain the target state variables of the inertial measurement unit based on the difference between the predicted state variables and the error state variables.

[0085] Therefore, by pre-integrating the state variables of the inertial measurement unit, the predicted state variables are obtained. Based on the difference between the predicted state variables and the error state variables, the target state variables of the inertial measurement unit are obtained. Based on the predicted state variables, the error state variables are removed to correct the state variables of the inertial measurement unit, thereby improving the accuracy of the target state variables as much as possible.

[0086] Please see Figure 4 , Figure 4 This is a schematic diagram of a framework of an embodiment of the electronic device of this application. The electronic device 40 includes a memory 41 and a processor 42 coupled to each other. The memory 41 stores program instructions, and the processor 42 is used to execute the program instructions to implement the steps in any of the above-described pose determination method embodiments. Specifically, the electronic device 40 may include, but is not limited to, desktop computers, laptops, servers, mobile phones, tablet computers, etc., and is not limited thereto.

[0087] Specifically, processor 42 controls itself and memory 41 to implement the steps in any of the above pose determination method embodiments. Processor 42 can also be called a CPU (Central Processing Unit). Processor 42 may be an integrated circuit chip with signal processing capabilities. Processor 42 can also be a general-purpose processor, digital signal processor (DSP), application-specific integrated circuit (ASIC), field-programmable gate array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components. A general-purpose processor can be a microprocessor or any conventional processor. Furthermore, processor 42 can be implemented using integrated circuit chips.

[0088] In the above scheme, the electronic device 40 can be used to implement the steps in any of the above pose determination method embodiments. On the one hand, by correcting the state quantity of the inertial measurement unit based on the error state quantity, the target state quantity of the inertial measurement unit is obtained, which helps to improve the accuracy of the target state quantity of the inertial measurement unit. On the other hand, by determining whether to update the pose state of the odometer based on the fusion result between the target state quantity and the point cloud data of the point cloud collector, the first observation in the Kalman filter equation is adjusted in response to the determination to update the pose state of the odometer, and a new Kalman filter equation is obtained. This can be continuously updated to obtain a new Kalman filter equation. Based on this, a new error state quantity is predicted based on the new Kalman filter equation, and the steps of correcting the state quantity of the inertial measurement unit based on the error state quantity and subsequent steps are re-executed until it is determined that no pose state update is needed. The latest pose state is used as the target pose of the odometer, which helps to reduce errors through repeated iterations. Therefore, the accuracy of the pose state can be improved.

[0089] Please see Figure 5 , Figure 5 This is a schematic diagram of a framework of an embodiment of the computer-readable storage medium of this application. The computer-readable storage medium 50 stores program instructions 51 that can be executed by a processor. The program instructions 51 are used to implement the steps in any of the above-described pose determination method embodiments.

[0090] In the above scheme, the computer-readable storage medium 50 can be used to implement the steps in any of the above pose determination method embodiments. On the one hand, by correcting the state quantity of the inertial measurement unit based on the error state quantity, the target state quantity of the inertial measurement unit is obtained, which helps to improve the accuracy of the target state quantity of the inertial measurement unit. On the other hand, by determining whether to update the pose state of the odometer based on the fusion result between the target state quantity and the point cloud data of the point cloud collector, and in response to determining that the pose state of the odometer needs to be updated, the first observation in the Kalman filter equation is adjusted to obtain a new Kalman filter equation. This can be continuously updated to obtain a new Kalman filter equation. Based on this, a new error state quantity is predicted based on the new Kalman filter equation, and the steps of correcting the state quantity of the inertial measurement unit based on the error state quantity and subsequent steps are re-executed until it is determined that the pose state does not need to be updated. The latest pose state is used as the target pose of the odometer, which helps to reduce errors through repeated iterations. Therefore, the accuracy of the pose state can be improved.

[0091] In some embodiments, the functions or modules of the apparatus provided in this disclosure can be used to perform the methods described in the above method embodiments. The specific implementation can be referred to the description of the above method embodiments, and for the sake of brevity, it will not be repeated here.

[0092] The description of the various embodiments above tends to emphasize the differences between the various embodiments. The similarities or similarities between them can be referred to, and for the sake of brevity, they will not be repeated here.

[0093] In the several embodiments provided in this application, it should be understood that the disclosed methods and apparatus can be implemented in other ways. For example, the apparatus implementations described above are merely illustrative. For instance, the division of modules or units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between devices or units may be electrical, mechanical, or other forms.

[0094] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment, depending on actual needs.

[0095] Furthermore, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0096] If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) or processor to execute all or part of the steps of the methods of various embodiments of this application. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0097] If the technical solution of this application involves personal information, the product using this technical solution has clearly informed the user of the personal information processing rules and obtained the user's voluntary consent before processing the personal information. If the technical solution of this application involves sensitive personal information, the product using this technical solution has obtained the user's separate consent before processing the sensitive personal information, and also meets the requirement of "express consent". For example, at personal information collection devices such as cameras, clear and prominent signs are set up to inform users that they have entered the scope of personal information collection and that personal information will be collected. If an individual voluntarily enters the collection scope, it is deemed that they have agreed to the collection of their personal information; or on the personal information processing device, with clear signs / information informing users of the personal information processing rules, authorization is obtained from the user through pop-up information or by asking the user to upload their personal information; wherein, the personal information processing rules may include information such as the personal information processor, the purpose of personal information processing, the processing method, and the types of personal information processed.

Claims

1. A method of pose determination, the method comprising: The method comprises: based on the state quantity of the inertial measurement unit, a Kalman filter equation is constructed, and based on an error state quantity, the state quantity of the inertial measurement unit is corrected to obtain a target state quantity of the inertial measurement unit; wherein the error state quantity is predicted based on the Kalman filter equation; based on the fusion result between the target state quantity and the point cloud data of the point cloud collector, it is determined whether to update the pose state of the odometer; in response to determining to update the pose state of the odometer, adjusting the first observation quantity in the Kalman filter equation to obtain a new Kalman filter equation; wherein the first observation quantity represents the observation quantity corresponding to the inertial measurement unit in the Kalman filter equation; based on the new Kalman filter equation, a new error state quantity is predicted, and the step of correcting the state quantity of the inertial measurement unit based on the error state quantity and the subsequent steps are re-executed until it is determined that the pose state does not need to be updated, and the latest pose state is taken as the target pose of the odometer.

2. The method of claim 1, wherein, The method comprises: based on the target state quantity, the point cloud data is motion compensated to obtain target point cloud data; based on the target point cloud data, a first spliced map is obtained; the first spliced map and the latest point cloud data are matched to determine whether to update the odometer pose state.

3. The method of claim 2, wherein, The method comprises: determine whether the sliding window is filled; in response to the sliding window being filled, discard the historical target point cloud data at the bottom of the sliding window, and update a second spliced map to obtain the first spliced map; wherein the second spliced map is spliced based on the historical target point cloud data in the sliding window before updating; in response to the sliding window not being filled, update the second spliced map to obtain the first spliced map.

4. The method of claim 2, wherein, The method comprises: match the first spliced map and the latest point cloud data to obtain a matching result; based on whether the matching result converges, it is determined whether to update the pose state of the odometer.

5. The method of claim 1, wherein, Before the new error state quantity is predicted based on the new Kalman filter equation, the method further comprises: obtaining positioning data of a positioning device; based on the positioning data, adjusting the second observation quantity in the latest Kalman filter equation; wherein the second observation quantity represents the observation quantity corresponding to the positioning device in the Kalman filter equation.

6. The method according to claim 1 or 5, characterized in that, The method comprises: detecting whether the observation quantity in the new Kalman filter equation has been adjusted; in response to the observation quantity in the new Kalman filter equation having been adjusted, predicting a new error state quantity based on the new Kalman filter equation; In response to the observation in the new Kalman filtering equation not being adjusted, the fusion result between the target state quantity and the point cloud data of the point cloud collector is re-executed to determine whether to update the pose state of the odometer and subsequent steps.

7. The method of claim 1, wherein, The target state quantity of the inertial measurement unit is obtained based on the error state quantity, including: The state quantity of the inertial measurement unit is pre-integrated to obtain a predicted state quantity; The target state quantity of the inertial measurement unit is obtained based on the difference between the predicted state quantity and the error state quantity.

8. A pose determination apparatus, characterized in that Including: The Kalman filtering equation is constructed based on the state quantity of the inertial measurement unit, and the target state quantity of the inertial measurement unit is obtained based on the error state quantity, wherein the error state quantity is predicted based on the Kalman filtering equation; The update module is configured to determine whether to update the pose state of the odometer based on the fusion result between the target state quantity and the point cloud data of the point cloud collector; The adjustment module is configured to adjust the first observation in the Kalman filtering equation in response to determining to update the pose state of the odometer, to obtain a new Kalman filtering equation, wherein the first observation represents the observation corresponding to the inertial measurement unit in the Kalman filtering equation; The prediction module is configured to predict a new error state quantity based on the new Kalman filtering equation, and re-execute the step of correcting the state quantity of the inertial measurement unit based on the error state quantity and subsequent steps until it is determined that the pose state does not need to be updated, and the latest pose state is taken as the target pose of the odometer.

9. An electronic device, comprising: The memory and the processor are coupled to each other, the memory stores program instructions, and the processor executes the program instructions to implement the pose determination method of any one of claims 1-7.

10. A computer-readable storage medium, characterized in that, The memory stores program instructions executable by the processor, and the program instructions are used to implement the pose determination method of any one of claims 1-7.

Images