Imu-centric localization and global map optimization method, device and equipment

Abstract

This invention provides an IMU-centered method, apparatus, and device for localization and global map optimization. The method includes: acquiring IMU data within a time period corresponding to a first frame of LiDAR data and a second frame of LiDAR data; determining the initial state estimate of the electronic device in the target IMU coordinate system corresponding to the second frame of LiDAR data, where the target IMU coordinate system is the IMU coordinate system used when acquiring the first frame of LiDAR data, based on the initial state estimate; determining the equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system based on the initial state estimate, and determining the measurement residual constraint in the target IMU coordinate system based on the first feature point corresponding to the second frame of LiDAR data; determining the current pose of the electronic device based on the equivalent prior distribution and the measurement residual constraint, and constructing a target global map. This method can obtain a highly accurate current pose and target global map.

Description

Technical Field

[0001] This invention relates to the field of map construction technology, and in particular to a method, apparatus and device for IMU-centered positioning and global map optimization. Background Technology

[0002] With the booming development of the autonomous driving field, LiDAR (Light Detection and Ranging) sensors and inertial measurement units (IMUs) have become widely used. Robots can use these LiDAR sensors and IMUs to construct a global map of the target in the robot's current environment.

[0003] Existing global map construction methods involve the robot acquiring LiDAR data from the LiDAR sensor and IMU data from the IMU, then processing this data using Kalman gain calculation to construct a global map of the target. However, during this process, poor initialization of system state estimation can easily lead to a decrease in the performance of the Extended Kalman Filter (EKF), resulting in reduced robot positioning accuracy and ultimately, an inaccurate global map of the target acquired by the robot. Summary of the Invention

[0004] This invention provides a localization and global map optimization method, apparatus, and device centered on an IMU, to address the shortcomings of existing global map construction methods where the robot's positioning accuracy is reduced, resulting in an inaccurate target global map. By utilizing the equivalent prior distribution and measurement residual constraints corresponding to the final state estimation error in the target IMU coordinate system, a highly accurate target global map corresponding to the current environment of the electronic device can be effectively constructed. Furthermore, the electronic device can also accurately obtain its current pose in the current environment.

[0005] This invention provides an IMU-centric localization and global map optimization method, comprising:

[0006] Acquire the inertial measurement unit (IMU) data within the time period corresponding to the first frame of lidar data and the second frame of lidar data, wherein the first frame of lidar data and the second frame of lidar data are adjacent frames of data;

[0007] Based on the IMU data, the initial state estimate of the electronic device corresponding to the second frame of lidar data in the target IMU coordinate system is determined. The target IMU coordinate system is the IMU coordinate system when the first frame of lidar data was acquired.

[0008] Based on the initial state estimate, the equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system is determined, and the measurement residual constraint in the target IMU coordinate system is determined based on the first feature point corresponding to the second frame of lidar data.

[0009] Based on the equivalent prior distribution and the measurement residual constraint, the current pose of the electronic device is determined, and a global map of the target is constructed.

[0010] According to the present invention, a positioning and global map optimization method centered on an IMU includes determining the initial state estimate of an electronic device corresponding to a second frame of lidar data in a target IMU coordinate system based on the IMU data. The method comprises: determining a first kinematic model, a current system state, and a current state error corresponding to the current system state in the target IMU coordinate system based on the IMU data; determining a second kinematic model corresponding to the current state error based on the first kinematic model, the current system state, and the current state error; and determining the initial state estimate of the second frame of lidar data based on the second kinematic model.

[0011] According to the present invention, a positioning and global map optimization method centered on an IMU includes determining the equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system based on the initial state estimate. This includes: determining the actual system state and the estimated state error corresponding to the estimated system state in the target IMU coordinate system based on the initial state estimate; and determining the equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system based on the actual system state and the estimated state error.

[0012] According to the present invention, a positioning and global map optimization method centered on an IMU includes determining the equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system based on the actual system state and the estimated state error. The method comprises: determining the maximum a posteriori probability estimate corresponding to the actual system state based on the actual system state and the prior estimate; iteratively processing the maximum a posteriori probability estimate to obtain the final state estimation error; and determining the equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system based on the estimated state error.

[0013] According to the present invention, a positioning and global map optimization method centered on an IMU is provided, wherein the method determines the measurement residual constraints in the target IMU coordinate system based on the first feature point corresponding to the second frame of lidar data, including: transforming the first feature point corresponding to the second frame of lidar data to the target IMU coordinate system to obtain a second feature point; establishing a first residual function corresponding to the second feature point to an edge and a second residual function corresponding to the second feature point to a surface; and determining the measurement residual constraints based on the first residual function and the second residual function.

[0014] According to the present invention, a positioning and global map optimization method centered on an IMU is provided, wherein determining measurement residual constraints based on a first residual function and a second residual function includes: linearizing the first residual function to obtain a first target residual function; linearizing the second residual function to obtain a second target residual function; and determining measurement residual constraints based on the first target residual function and the second target residual function.

[0015] According to the present invention, a localization and global map optimization method centered on an IMU is provided, which determines the current pose of an electronic device based on the equivalent prior distribution and the measurement residual constraint, and constructs a target global map, including: determining the target system state update group corresponding to the second frame of lidar data based on the equivalent prior distribution and the measurement residual constraint; and using the target system state update group to determine the current pose of the electronic device and construct a target global map.

[0016] The present invention also provides a positioning and global map optimization device, comprising:

[0017] The acquisition module is used to acquire inertial measurement unit (IMU) data within the time period corresponding to the first frame of lidar data and the second frame of lidar data, wherein the first frame of lidar data and the second frame of lidar data are adjacent frames.

[0018] The processing module is used to determine, based on the IMU data, the initial state estimate of the electronic device corresponding to the second frame of LiDAR data in the target IMU coordinate system, where the target IMU coordinate system is the IMU coordinate system used when acquiring the first frame of LiDAR data; based on the initial state estimate, determine the equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system, and based on the first feature point corresponding to the second frame of LiDAR data, determine the measurement residual constraint in the target IMU coordinate system; based on the equivalent prior distribution and the measurement residual constraint, determine the current pose of the electronic device, and construct a global map of the target.

[0019] The present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the IMU-centered positioning and global map optimization method as described above.

[0020] The present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the IMU-centered localization and global map optimization method as described above.

[0021] The present invention also provides a computer program product, including a computer program that, when executed by a processor, implements the IMU-centered positioning and global map optimization method as described above.

[0022] The present invention provides an IMU-centered positioning and global map optimization method, apparatus, and device. This involves acquiring IMU data within a time period corresponding to a first frame of LiDAR data and a second frame of LiDAR data, where the first and second frames are adjacent frames. Based on the IMU data, an initial state estimate of the electronic device corresponding to the second frame of LiDAR data in a target IMU coordinate system is determined. The target IMU coordinate system is the IMU coordinate system used when acquiring the first frame of LiDAR data. Based on the initial state estimate, an equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system is determined. Furthermore, based on a first feature point corresponding to the second frame of LiDAR data, a measurement residual constraint in the target IMU coordinate system is determined. Finally, based on the equivalent prior distribution and the measurement residual constraint, the current pose of the electronic device is determined, and a target global map is constructed. This method addresses the shortcomings of existing global map construction methods, where the robot's reduced positioning accuracy leads to inaccurate target global maps. By utilizing the equivalent prior distribution and measurement residual constraints corresponding to the final state estimation error in the target IMU coordinate system, a highly accurate target global map corresponding to the current environment of the electronic device can be effectively constructed. Furthermore, the electronic device can accurately obtain its current pose in the current environment. Attached Figure Description

[0023] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0024] Figure 1This is a flowchart illustrating the IMU-centered positioning and global map optimization method provided by the present invention;

[0025] Figure 2 This is a schematic diagram of a scenario for the IMU-centered positioning and global map optimization method provided by the present invention;

[0026] Figure 3 This is a schematic diagram of the positioning and global map optimization device provided by the present invention;

[0027] Figure 4 This is a schematic diagram of the structure of the electronic device provided by the present invention. Detailed Implementation

[0028] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0029] It should be noted that the electronic device involved in the embodiments of the present invention may be a robot. The robot refers to a machine that can automatically perform a series of complex actions under computer programming and can replace human workers in various complex environments.

[0030] Optionally, the electronic device may be equipped with a lidar sensor for acquiring lidar data.

[0031] The lidar data is 3D point cloud data.

[0032] Optionally, the electronic device may be equipped with an inertial measurement unit (IMU) for acquiring IMU data.

[0033] Optionally, the IMU may include an accelerometer and a gyroscope.

[0034] Among them, the accelerometer is used to collect acceleration data;

[0035] Gyroscopes are used to collect angular velocity data.

[0036] For example, since the data acquisition frequency of the LiDAR sensor is 10 Hz and the data acquisition frequency of the IMU is 100 Hz-400 Hz, the electronic device can acquire at least one IMU data between the k-th frame of LiDAR data and the (k+1)-th frame of LiDAR data, where k ≥ 1, and the k-th frame of LiDAR data and the (k+1)-th frame of LiDAR data are consecutive adjacent frames.

[0037] It should be noted that when the lidar sensor receives the k-th scan frame, that is, when the electronic device acquires the k-th frame of lidar data, the lidar coordinate system corresponding to the lidar sensor can be l. k This indicates that the k-th frame of lidar data is in the lidar coordinate system l. k The data below, at this time, the IMU coordinate system corresponding to the IMU can be b k This indicates that the IMU data can be in the IMU coordinate system b. k The following data.

[0038] When an electronic device acquires the (k+1)th frame of lidar data, the lidar coordinate system corresponding to the lidar sensor can be used as l k+1 This indicates that the (k+1)th frame of lidar data is in the lidar coordinate system l. k+1 The data below, at this time, the IMU coordinate system corresponding to the IMU can be b k+1 This indicates that the IMU data can be in the IMU coordinate system b. k+1 The following data.

[0039] In addition, the system status corresponding to the electronic device is available. This indicates that it refers to the coordinate system b from the IMU. k+1 To IMU coordinate system b k The relative transformation between these two consecutive lidar scan frames.

[0040] It should be noted that the execution subject involved in the embodiments of the present invention can be a positioning and global map optimization device or an electronic device. The embodiments of the present invention will be further described below using an electronic device as an example.

[0041] like Figure 1 The diagram shown is a flowchart illustrating the IMU-centered localization and global map optimization method provided by this invention, which may include:

[0042] 101. Obtain the inertial measurement unit (IMU) data within the time period corresponding to the first and second frames of lidar data.

[0043] The first frame of lidar data and the second frame of lidar data are adjacent frames. The first frame of lidar data can be represented by the k-th frame of lidar data, and the second frame of lidar data can be represented by the (k+1)-th frame of lidar data.

[0044] Since the data acquisition frequency of the IMU is greater than that of the LiDAR sensor, the electronic device can acquire at least one IMU data while using the LiDAR sensor to acquire the k-th frame and the (k+1)-th frame of LiDAR data.

[0045] 102. Based on the IMU data, determine the initial state estimate of the electronic device corresponding to the second frame of lidar data in the target IMU coordinate system.

[0046] Wherein, the target IMU coordinate system is the IMU coordinate system used when acquiring the first frame of lidar data, which can be represented by b. k express;

[0047] Initial state estimates are available express.

[0048] After acquiring IMU data, the electronic device can perform forward propagation processing on the IMU data to determine the electronic device's position in the target IMU coordinate system b. k The initial state estimate corresponding to the (k+1)th frame of lidar data

[0049] In some embodiments, the electronic device determines the initial state estimate corresponding to the second frame of lidar data in the target IMU coordinate system based on the IMU data. This may include: the electronic device determining a first kinematic model, the current system state, and the current state error corresponding to the current system state in the target IMU coordinate system based on the IMU data; the electronic device determining a second kinematic model corresponding to the current state error based on the first kinematic model, the current system state, and the current state error; and the electronic device determining the initial state estimate corresponding to the second frame of lidar data based on the second kinematic model.

[0050] After acquiring IMU data, the electronic device can process the IMU data to obtain the electronic device's position in the target IMU coordinate system b. k The corresponding first kinematic model and the current system state x t and the current system status x t Corresponding current state error The current state error Defined in the current system state x t Corresponding estimated state On the tangent space; then, the electronic device, based on the first kinematic model and the current system state x t and current state error Determine the current state error The corresponding second kinematic model is used to determine the initial state estimate corresponding to the (k+1)th frame of lidar data.

[0051] Optionally, the electronic device determines, based on IMU data, the first kinematic model corresponding to the electronic device in the target IMU coordinate system, the current system state, and the current state error corresponding to the current system state. This may include: the electronic device determining the first kinematic model corresponding to the electronic device in the target IMU coordinate system according to a first formula; the electronic device determining the current system state according to a second formula; and the electronic device determining the current state error corresponding to the current system state according to a third formula.

[0052] The first formula is:

[0053] Indicates the IMU coordinate system b at time t. k+1 Relative to IMU coordinate system b k Location, Indicates position The corresponding derivative; This represents the velocity of the IMU at time t. Indicates speed The corresponding derivative; Indicates the IMU coordinate system b at time t. k+1 Relative to IMU coordinate system b k The rotation; This indicates the acceleration collected by the accelerometer; This indicates the bias corresponding to the accelerometer; n a This represents the Gaussian white noise corresponding to the data collected by the accelerometer; This indicates that the target IMU coordinate system is b. k gravitational acceleration, Represents gravitational acceleration The corresponding derivative; Indicates rotation The corresponding derivative; Indicates the antisymmetric operator; This represents the angular velocity collected by the gyroscope; This indicates the bias corresponding to the gyroscope; n ω This represents the Gaussian white noise corresponding to the data collected by the gyroscope; This indicates the bias corresponding to the gyroscope. The corresponding derivative; n bω Indicates bias The corresponding Gaussian white noise; This indicates the bias corresponding to the accelerometer. The corresponding derivative; n ba Indicates bias The corresponding Gaussian white noise. In other words, IMU data can include: angular velocity. and acceleration The first kinematic model may include: position corresponding derivative speed corresponding derivative gravitational acceleration corresponding derivative Rotation corresponding derivative bias The corresponding derivative and bias The corresponding derivative wait.

[0054] The second formula is

[0055] x t Indicates the current system status; Indicates including rotation speed and location These are the combined parameters of these three state variables. In other words, the current system state x t It can include: rotation speed Location bias bias and gravitational acceleration wait.

[0056] The third formula is

[0057]

[0058] Indicates the current system state x t The corresponding current state error, current state error It is an 18-dimensional random vector, and the current state error is... The covariance is Represent real numbers; Indicates the current system state x t The corresponding estimated state; This represents subtraction defined on a Lie group, used to compute the current system state x. t With estimated state The difference between them; Indicates rotation With estimated rotation The corresponding angular error, Indicates estimated rotation The corresponding transpose matrix; Indicates speed Corresponding estimated speed The corresponding speed error; Indicates position Corresponding estimated position The corresponding positional error; Indicates bias The corresponding estimated bias; Indicates bias The corresponding estimated bias; Represents gravitational acceleration The corresponding estimated gravitational acceleration; Indicates estimated bias The corresponding bias error; Indicates estimated bias The corresponding bias error; Indicates the estimated gravitational acceleration The corresponding gravitational acceleration error; Represents combined parameters Corresponding estimated parameters The corresponding inverse matrix.

[0059] Then, the electronic device combines the first, second, and third formulas to accurately calculate the current state error. The corresponding second kinematic model can be represented by the fourth formula.

[0060] The fourth formula is

[0061] Represents the second kinematic model; F t G represents the error state transition matrix at time t; t This represents the Jacobian matrix corresponding to the noise at time t; Represents a white noise vector. n represents Gaussian white noise ω The corresponding transpose matrix, n represents Gaussian white noise a The corresponding transpose matrix, n represents Gaussian white noise bω The corresponding transpose matrix, n represents Gaussian white noise ba The corresponding transpose matrix.

[0062] Optional, error state transition matrix

[0063]

[0064] Jacobian matrix corresponding to noise

[0065] in, Indicates rotation The corresponding estimated rotation; Indicates speed The corresponding estimated speed; Indicates position The corresponding estimated location.

[0066] Optionally, the electronic device determines the initial state estimate corresponding to the second frame of lidar data based on the second kinematic model, which may include: the electronic device acquiring the IMU data b in the current frame. k Compared with the previous frame of IMU data b k-1 The relative pose between them; the electronic device determines the initial state estimate corresponding to the second frame of lidar data based on the second kinematic model and the relative pose.

[0067] Among them, relative pose can be used express.

[0068] After acquiring the second kinematic model, the electronic device can solve the differential equation corresponding to the fourth formula to obtain the current state error. The corresponding transfer equation; then, the electronic device uses this transfer equation to determine the relative pose. The current state error is calculated by comparing it with the IMU data obtained in step 101. Obtain the initial state estimate corresponding to the arrival of the (k+1)th frame of lidar data.

[0069] Furthermore, the electronic device can also obtain the covariance corresponding to the arrival of the (k+1)th frame of lidar data.

[0070] 103. Based on the initial state estimate, determine the equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system, and based on the first feature point corresponding to the second frame of lidar data, determine the measurement residual constraint in the target IMU coordinate system.

[0071] Among them, feature points are three-dimensional point cloud data, which can be divided into line features and surface features;

[0072] Any first feature point in the (k+1)th frame of lidar data can be used express.

[0073] Electronic devices acquire initial state estimates and the first feature point Then, based on the initial state estimate The target IMU coordinate system b can be accurately determined. k Final state estimation error The corresponding equivalent prior distribution; based on the first feature point It can accurately determine the measurement residual constraints.

[0074] It should be noted that there are no time restrictions on the timing of electronic devices acquiring equivalent prior distributions and measurement residual constraints.

[0075] In some embodiments, the electronic device determines the equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system based on the initial state estimate. This may include: the electronic device determining the actual system state and the estimated state error corresponding to the estimated system state in the target IMU coordinate system based on the initial state estimate; and the electronic device determining the equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system based on the actual system state and the estimated state error.

[0076] Electronic devices acquire initial state estimates and the first feature point Subsequently, in the world-centric invariant extended Kalman filter (EKF) state estimator, the solution needed is to obtain the state estimator from the IMU coordinate system b. k+1 To IMU coordinate system b k The relative transformation, that is, the electronic device based on the initial state estimate. The target IMU coordinate system b of the electronic device can be determined. k The corresponding real system state and estimate system state Corresponding estimated state error Then, the electronic device will adjust according to the actual system state. and estimated state error Accurately determine the target IMU coordinate system b k Final state estimation error The corresponding equivalent prior distribution.

[0077] Among them, the invariant EKF is a highly efficient recursive filter (autoregressive filter).

[0078] Optionally, the electronic device determines the true system state corresponding to the electronic device in the target IMU coordinate system and the estimated state error corresponding to the estimated system state based on the initial state estimate. This may include: the electronic device determining the true system state corresponding to the electronic device in the target IMU coordinate system according to the fifth formula; and the electronic device determining the estimated state error corresponding to the estimated system state according to the sixth formula.

[0079] Among them, the fifth formula is

[0080] Represents the actual system state; Indicates including rotation speed and location These are the combined parameters of these three state variables. In other words, the actual system state. It can include: rotation speed Location bias bias and gravitational acceleration wait.

[0081] The sixth formula is

[0082] Indicates the estimated system state The corresponding estimated state error; This represents the initial value of the frame-map matching obtained by the IMU after forward propagation of the final state estimate of the previous frame; This represents subtraction defined on a Lie group, used to compute the state of the real system. With initial value The difference between them.

[0083] Electronic devices can accurately determine the true system state based on the fifth formula. According to the sixth formula, the electronic device can accurately determine the estimated state error.

[0084] In some embodiments, the electronic device determines the equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system based on the actual system state and the estimated state error. This may include: the electronic device determining the maximum a posteriori probability estimate corresponding to the actual system state based on the actual system state and the prior estimate; the electronic device iteratively processing the maximum a posteriori probability estimate to obtain the final state estimation error; and the electronic device determining the equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system based on the estimated state error.

[0085] To facilitate subsequent calculations, x k+1 Replace the real system state Will Instead of estimating state error Based on this, before fusing the (k+1)th frame of LiDAR data, the electronic device operates in the actual system state x. k+1 An a prior estimate was added. Thus achieving the true system state x k+1 The purpose is to perform accurate forward propagation; then, the electronic device adjusts the propagation based on the actual system state x.k+1 and prior estimates The true system state x can be accurately determined. k+1 The corresponding maximum posterior probability estimate Thus, the final state estimation error can be accurately obtained. and final state estimation error The corresponding equivalent prior distribution.

[0086] Optionally, the electronic device determines the maximum a posteriori probability estimate corresponding to the real system state based on the real system state and the prior estimate. This may include: the electronic device determining the Gaussian distribution corresponding to the real system state according to the seventh formula; and the electronic device determining the maximum a posteriori probability estimate corresponding to the real system state based on the Gaussian distribution and the second frame of lidar data.

[0087] Among them, the seventh formula is

[0088] Represents the actual system state x k+1 The corresponding prior estimate; Indicates the estimated system state error The corresponding Gaussian distribution; Indicates the estimated system state The corresponding covariance.

[0089] After accurately determining the Gaussian distribution using the seventh formula, the electronic device can obtain the true system state x based on this Gaussian distribution and the (k+1)th frame of lidar data. k+1 The corresponding posterior probability; then, the electronic device maximizes this posterior probability to accurately obtain the true system state x. k+1 The corresponding maximum a posteriori probability estimate, which can be used express.

[0090] Optionally, the electronic device iteratively processes the maximum a posteriori probability estimate to obtain the final state estimation error, which may include: the electronic device determining the final state estimation error corresponding to the true system state according to the eighth formula.

[0091] Among them, the eighth formula is

[0092] Represents the maximum posterior probability estimate The corresponding final state estimation error; This represents subtraction defined on a Lie group, used to compute the state x of the real system. k+1 With the maximum a posteriori probability estimate The difference between them.

[0093] According to the eighth formula, electronic devices can accurately determine the maximum a posteriori probability estimated state. Corresponding final state estimation error

[0094] Optionally, the electronic device determines the equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system based on the estimated state error. This can include: the electronic device projecting the final state estimation error onto the tangent space corresponding to the maximum a posteriori probability estimate to obtain the ninth formula; and the electronic device obtaining the equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system based on the ninth formula.

[0095] Among them, the ninth formula is

[0096] This represents subtraction defined on a Lie group, used to compute the state x of the real system. k+1 with prior estimates The difference between them; This represents addition defined on a Lie group, used to compute the maximum a posteriori probability estimate. Error with final state estimation The sum of; J k+1 This represents the final state estimation error. The Jacobian matrix obtained by differentiation;

[0097] Equivalent prior distributions are available express, Represents the Jacobian matrix J k+1 The corresponding inverse matrix, Represents the Jacobian matrix J k+1 The corresponding inverse matrix.

[0098] Electronic devices based on state estimation error Projection onto the maximum a posteriori probability estimate On the corresponding tangent space, the ninth formula can be accurately obtained, and thus the final state estimation error can be accurately obtained. The corresponding equivalent prior distribution.

[0099] In some embodiments, the electronic device determines the measurement residual constraint in the target IMU coordinate system based on the first feature point corresponding to the second frame of lidar data. This may include: the electronic device transforming the first feature point corresponding to the second frame of lidar data into the target IMU coordinate system to obtain the second feature point; the electronic device establishing a first residual function from the second feature point to the edge and a second residual function from the second feature point to the surface; and the electronic device determining the measurement residual constraint based on the first residual function and the second residual function.

[0100] To ensure the consistency of each first feature point, the electronic device first transforms the first feature points corresponding to the second frame of LiDAR data into the target IMU coordinate system to obtain the second feature points. Then, based on these second feature points, the electronic device constructs point-to-edge or point-to-surface metric functions to accurately determine the target IMU coordinate system b. k Measurement residual constraints are applied.

[0101] Optionally, the electronic device may transform the first feature point corresponding to the second frame of lidar data into the target IMU coordinate system to obtain the second feature point. This may include: the electronic device determining the second feature point according to the tenth formula.

[0102] Among them, the tenth formula is

[0103] Represents the radar coordinate system l k The second feature point below; Represents the radar coordinate system l k Relative to the target IMU coordinate system b k The rotation; Indicates IMU coordinate system b k+1 Relative to the target IMU coordinate system b k Rotation estimation; Represents the radar coordinate system l k+1 The second feature point below; Represents the radar coordinate system l k Relative to IMU coordinate system b k Location; Represents the radar coordinate system l k Relative to the target IMU coordinate system b k Location estimation.

[0104] Based on this, the first residual function is

[0105] The second residual function is

[0106] in, Indicates intermediate parameters;

[0107] Second feature point In the case of an edge vertex, obtain the two nearest neighbors, namely: and Then, based on and Determine the second feature point The corresponding edge; at the second feature point In the case of a planar point, obtain the three nearest neighbor points, namely: and Then, based on and Determine the second feature point The corresponding plane.

[0108] According to the tenth formula, the electronic equipment can accurately determine the target IMU coordinate system b. k The second feature point below.

[0109] Optional, electronic devices are available This represents the second feature point.

[0110] in, Indicates the second feature point The corresponding truth value; n i Indicates the measurement of the second feature point The corresponding Gaussian white noise.

[0111] In some embodiments, the electronic device determines measurement residual constraints based on a first residual function and a second residual function, which may include: the electronic device linearizing the first residual function to obtain a first target residual function; the electronic device linearizing the second residual function to obtain a second target residual function; and the electronic device determining measurement residual constraints based on the first target residual function and the second target residual function.

[0112] Wherein, the first objective residual function is

[0113]

[0114] The second objective residual function is:

[0115]

[0116] This represents the residual estimate with respect to the maximum a posteriori probability. The first Jacobian matrix; F ei Represents the residual with respect to the second characteristic point The first Jacobian matrix; express This represents the residual estimate with respect to the maximum a posteriori probability. The second Jacobian matrix; F pi Represents the residual with respect to the second characteristic point The second Jacobian matrix;

[0117] The first and second objective residual functions obtained from the electronic device are more easily combined with equivalent prior distributions to effectively obtain measurement residual constraints, and thus obtain the estimation error of the final state. The maximum a posteriori probability estimation problem is then solved by the electronic device using the iterative Kalman filtering method.

[0118] 104. Based on the equivalent prior distribution and measurement residual constraints, determine the current pose of the electronic device and construct a global map of the target.

[0119] The current pose refers to the current position information and current rotation information of the electronic device.

[0120] The target global map is a 3D point cloud map.

[0121] In some embodiments, the electronic device determines its current pose based on the equivalent prior distribution and measurement residual constraints, and constructs a global map of the target. This may include: the electronic device determining the target system state update group corresponding to the second frame of lidar data based on the equivalent prior distribution and measurement residual constraints; and the electronic device using the target system state update group to determine its current pose and construct a global map of the target.

[0122] Among them, the target system status update group is

[0123]

[0124] This represents the new state estimation error;

[0125] This represents the new state estimate, i.e., the current pose of the electronic device;

[0126] Indicate the new covariance;

[0127] Represents a group of Jacobian matrices;

[0128] K = (H T R -1 H+P -1 ) -1 H T R -1 , representing the Kalman gain matrix,

[0129] Indicates the second feature point covariance;

[0130] Indicates the measurement residual constraint group;

[0131] I represents the identity matrix;

[0132] Given that the target system state update group has converged iteratively, the electronic device can obtain...

[0133] To the new state estimate and new covariance Then, the electronic device is based on

[0134] The new state estimate and the new covariance Determine the current pose of the electronic device and construct a global map of the target.

[0135] Optionally, during the forward propagation processing of IMU data, the electronic device also needs to determine the next system state based on the reset formula. Reset.

[0136] Among them, the reset formula

[0137]

[0138] At the same time, relative pose and The corresponding covariance is set to zero, and the covariances of the other variables are subjected to corresponding linear transformations according to the reset formula.

[0139] This represents the initial velocity value for the next state. Indicates IMU coordinate system b k To IMU coordinate system b k+1 rotation, Indicates IMU coordinate system b k+1 In IMU coordinate system b k The speed of the downward movement; Indicates the initial value of the position of the next state; Indicates bias The corresponding next bias; Indicates bias The corresponding next bias; Indicates IMU coordinate system b k+1 The acceleration due to gravity.

[0140] like Figure 2 The image shown is a schematic diagram of a scenario illustrating the IMU-centric positioning and global map optimization method provided by this invention. Figure 2 The electronic equipment includes a feature extraction module, a lidar-inertial odometry module, and a lidar mapping module.

[0141] The electronic device inputs LiDAR data obtained from scanning using a LiDAR sensor into a feature extraction module. Then, it determines the corresponding line and surface features of the LiDAR data. Next, in the LiDAR-Inertial Mapping module, the electronic device uses frame-to-frame matching to obtain the feature correlation between frames, including the line and surface features and the forward-propagation processed IMU data. Then, in the LiDAR-Inertial Odometry module, after determining the convergence of the feature correlation using an invariant extended Kalman filter state estimator, the electronic device outputs an initial odometry, which represents the pose transformation relationship of the electronic device (e.g., a robot) between two consecutive frames of IMU measurement data. This initial odometry can also be called pure odometry. Finally, the electronic device inputs this pure odometry as an initial estimate into the LiDAR mapping module. The electronic device can then use the LiDAR mapping module to correct the pure odometry using frame-to-map matching, outputting a real-time map-refined odometry and a new real-time state estimate. This allows us to obtain the current pose of the electronic device. At the same time, in the lidar mapping module, a highly accurate global map of the target can be constructed.

[0142] The frame-to-frame matching method refers to the data matching between the k-th frame of LiDAR data and the (k+1)-th frame of LiDAR data to obtain the initial state estimate.

[0143] Optionally, after step 104, the method may further include: the electronic device outputting a global map of the target.

[0144] This allows users to obtain a timely view of the target's global map.

[0145] In this embodiment of the invention, inertial measurement unit (IMU) data within the time period corresponding to the first and second frames of LiDAR data are acquired. Based on the IMU data, the initial state estimate of the electronic device corresponding to the second frame of LiDAR data in the target IMU coordinate system is determined. Based on the initial state estimate, the equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system is determined, and the measurement residual constraint in the target IMU coordinate system is determined based on the first feature point corresponding to the second frame of LiDAR data. Based on the equivalent prior distribution and the measurement residual constraint, the current pose of the electronic device is determined, and a target global map is constructed. This method addresses the deficiency in existing global map construction methods where the robot's final target global map is inaccurate due to reduced robot positioning accuracy. By utilizing the equivalent prior distribution and measurement residual constraint corresponding to the final state estimation error in the target IMU coordinate system, a highly accurate target global map corresponding to the current environment of the electronic device can be effectively constructed. Furthermore, the electronic device can accurately obtain its current pose in the current environment, thus achieving a highly accurate positioning result.

[0146] The positioning and global map optimization device provided by the present invention is described below. The positioning and global map optimization device described below can be referred to in correspondence with the world-centered positioning and global map optimization method described above.

[0147] like Figure 3 The diagram shown is a structural schematic of the positioning and global map optimization device provided by the present invention, which may include:

[0148] The acquisition module 301 is used to acquire the inertial measurement unit (IMU) data within the time period corresponding to the first frame of lidar data and the second frame of lidar data, wherein the first frame of lidar data and the second frame of lidar data are adjacent frame data.

[0149] The processing module 302 is configured to determine, based on the IMU data, the initial state estimate of the electronic device corresponding to the second frame of LiDAR data in the target IMU coordinate system, wherein the target IMU coordinate system is the IMU coordinate system used when acquiring the first frame of LiDAR data; based on the initial state estimate, determine the equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system, and based on the first feature point corresponding to the second frame of LiDAR data, determine the measurement residual constraint in the target IMU coordinate system; based on the equivalent prior distribution and the measurement residual constraint, determine the current pose of the electronic device, and construct a global map of the target.

[0150] Optionally, the processing module 302 is specifically used to determine, based on the IMU data, the first kinematic model of the electronic device in the target IMU coordinate system, the current system state, and the current state error corresponding to the current system state; based on the first kinematic model, the current system state, and the current state error, determine the second kinematic model corresponding to the current state error; and based on the second kinematic model, determine the initial state estimate corresponding to the second frame of lidar data.

[0151] Optionally, the processing module 302 is specifically used to determine the actual system state of the electronic device in the target IMU coordinate system and the estimated state error corresponding to the estimated system state based on the initial state estimate; and to determine the equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system based on the actual system state and the estimated state error.

[0152] Optionally, the processing module 302 is specifically used to determine the maximum a posteriori probability estimate corresponding to the real system state based on the real system state and the prior estimate; to perform iterative processing on the maximum a posteriori probability estimate to obtain the final state estimation error; and to determine the equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system based on the estimated state error.

[0153] Optionally, the processing module 302 is specifically used to transform the first feature point corresponding to the second frame of lidar data into the target IMU coordinate system to obtain the second feature point; establish a first residual function corresponding to the second feature point to the edge and a second residual function corresponding to the second feature point to the surface; and determine the measurement residual constraint based on the first residual function and the second residual function.

[0154] Optionally, the processing module 302 is specifically used to linearize the first residual function to obtain a first target residual function; to linearize the second residual function to obtain a second target residual function; and to determine the measurement residual constraint based on the first target residual function and the second target residual function.

[0155] Optionally, the processing module 302 is specifically used to determine the target system state update group corresponding to the second frame of lidar data based on the equivalent prior distribution and the measurement residual constraint; and to determine the current pose of the electronic device using the target system state update group, and to construct a global map of the target.

[0156] like Figure 4The diagram shown is a structural schematic of the electronic device provided by the present invention. The electronic device may include: a processor 410, a communication interface 420, a memory 430, and a communication bus 440, wherein the processor 410, the communication interface 420, and the memory 430 communicate with each other through the communication bus 440. The processor 410 can call logic instructions in the memory 430 to execute an IMU-centered localization and global map optimization method. This method includes: acquiring inertial measurement unit (IMU) data within a time period corresponding to a first frame of LiDAR data and a second frame of LiDAR data, wherein the first frame and the second frame are adjacent frames; determining, based on the IMU data, an initial state estimate of the electronic device corresponding to the second frame of LiDAR data in a target IMU coordinate system, where the target IMU coordinate system is the IMU coordinate system used when acquiring the first frame of LiDAR data; determining, based on the initial state estimate, an equivalent prior distribution corresponding to the final estimation error in the target IMU coordinate system, and determining a measurement residual constraint in the target IMU coordinate system based on a first feature point corresponding to the second frame of LiDAR data; and determining the current pose of the electronic device based on the equivalent prior distribution and the measurement residual constraint, and constructing a target global map.

[0157] Furthermore, the logical instructions in the aforementioned memory 430 can be implemented as software functional units and, when sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, essentially, or the part that contributes to the prior art, or a 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.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. 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.

[0158] On the other hand, the present invention also provides a computer program product, which includes a computer program that can be stored on a non-transitory computer-readable storage medium. When the computer program is executed by a processor, the computer can execute the IMU-centered localization and global map optimization method provided by the above methods. The method includes: acquiring inertial measurement unit (IMU) data within a time period corresponding to a first frame of lidar data and a second frame of lidar data, wherein the first frame of lidar data and the second frame of lidar data are adjacent frames; determining, based on the IMU data, an initial state estimate of the electronic device corresponding to the second frame of lidar data in a target IMU coordinate system, wherein the target IMU coordinate system is the IMU coordinate system when the first frame of lidar data was acquired; determining, based on the initial state estimate, an equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system, and determining a measurement residual constraint in the target IMU coordinate system based on a first feature point corresponding to the second frame of lidar data; determining the current pose of the electronic device based on the equivalent prior distribution and the measurement residual constraint, and constructing a target global map.

[0159] In another aspect, the present invention also provides a non-transitory computer-readable storage medium storing a computer program thereon. When executed by a processor, the computer program implements an IMU-centered localization and global map optimization method provided by the methods described above. The method includes: acquiring inertial measurement unit (IMU) data within a time period corresponding to a first frame of lidar data and a second frame of lidar data, wherein the first frame of lidar data and the second frame of lidar data are adjacent frames; determining an initial state estimate of the electronic device corresponding to the second frame of lidar data in a target IMU coordinate system based on the IMU data, wherein the target IMU coordinate system is the IMU coordinate system at the time of acquiring the first frame of lidar data; determining an equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system based on the initial state estimate, and determining a measurement residual constraint in the target IMU coordinate system based on a first feature point corresponding to the second frame of lidar data; determining the current pose of the electronic device based on the equivalent prior distribution and the measurement residual constraint, and constructing a target global map.

[0160] The device embodiments described above are merely illustrative. 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 modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without any creative effort.

[0161] Through the above description of the embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus necessary general-purpose hardware platforms, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solutions, in essence or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in the various embodiments or some parts of the embodiments.

[0162] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A localization and global map optimization method centered on IMU, characterized in that, include: Acquire the inertial measurement unit (IMU) data within the time period corresponding to the first frame of lidar data and the second frame of lidar data, where the first frame of lidar data and the second frame of lidar data are adjacent frames; Based on the IMU data, the initial state estimate of the electronic device corresponding to the second frame of lidar data in the target IMU coordinate system is determined, wherein the target IMU coordinate system is the IMU coordinate system when the first frame of lidar data was acquired. Based on the initial state estimate, the equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system is determined, and the measurement residual constraint in the target IMU coordinate system is determined based on the first feature point corresponding to the second frame of lidar data. Based on the equivalent prior distribution and the measurement residual constraints, the current pose of the electronic device is determined, and a global map of the target is constructed. The step of determining the current pose of the electronic device based on the equivalent prior distribution and the measurement residual constraints includes: Based on the equivalent prior distribution and the measurement residual constraint, determine the target system state update group corresponding to the second frame of lidar data; Given that the state update group of the target system has converged iteratively, a new state estimate and a new covariance are obtained. The current pose of the electronic device is determined based on the new state estimate and the new covariance. The equivalent prior distribution is represented by the following expression: in, This represents the final state estimation error. Represents the Jacobian matrix J k+1 The corresponding inverse matrix, Represents the Jacobian matrix J k+1 The corresponding inverse matrix, Indicates the estimated system state The corresponding covariance, Represents the actual system state x k+1 The corresponding maximum a posteriori probability estimate, Represents the prior estimate. This represents subtraction defined on a Lie group; The target system status update group includes: This represents the new state estimation error; This represents the new state estimate, i.e., the current pose of the electronic device; Indicate the new covariance; Represents a group of Jacobian matrices; This represents the residual estimate with respect to the maximum a posteriori probability. The transpose of the first Jacobian matrix express Residual estimate with respect to maximum posterior probability The transpose of the second Jacobian matrix; K = (H T R -1 H+P -1 ) -1 H T R -1 , representing the Kalman gain matrix, Indicates the second feature point covariance, F ei Represents the residual with respect to the second characteristic point The first Jacobian matrix, F pi Represents the residual with respect to the second characteristic point The second Jacobian matrix; Indicates the measurement residual constraint group; This represents the transpose of the first objective residual function matrix. This represents the transpose of the second objective residual function matrix; I represents the identity matrix; 2. The method according to claim 1, characterized in that, The step of determining the initial state estimate of the electronic device corresponding to the second frame of lidar data in the target IMU coordinate system based on the IMU data includes: Based on the IMU data, determine the first kinematic model of the electronic device in the target IMU coordinate system, the current system state, and the current state error corresponding to the current system state. Based on the first kinematic model, the current system state, and the current state error, determine the second kinematic model corresponding to the current state error; Based on the second kinematic model, the initial state estimate corresponding to the second frame of lidar data is determined.

3. The method according to claim 1, characterized in that, The step of determining the equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system based on the initial state estimate includes: Based on the initial state estimate, determine the actual system state corresponding to the target IMU coordinate system of the electronic device and the estimated state error corresponding to the estimated system state; Based on the actual system state and the estimated state error, determine the equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system; The step of determining the equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system based on the actual system state and the estimated state error includes: Based on the actual system state and the prior estimate, determine the maximum posterior probability estimate corresponding to the actual system state; The maximum a posteriori probability estimate is iteratively processed to obtain the final state estimation error; Based on the estimated state error, determine the equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system.

4. The method according to claim 1, characterized in that, The step of determining the measurement residual constraint in the target IMU coordinate system based on the first feature point corresponding to the second frame of lidar data includes: The first feature point corresponding to the second frame of lidar data is transformed into the target IMU coordinate system to obtain the second feature point; Establish the first residual function from the second feature point to the edge and the second residual function from the second feature to the surface; The measurement residual constraints are determined based on the first residual function and the second residual function.

5. The method according to claim 4, characterized in that, The step of determining the measurement residual constraint based on the first residual function and the second residual function includes: The first residual function is linearized to obtain the first target residual function; The second residual function is linearized to obtain the second objective residual function; The measurement residual constraints are determined based on the first target residual function and the second target residual function.

6. A positioning and global map optimization device, characterized in that, include: The acquisition module is used to acquire inertial measurement unit (IMU) data within the time period corresponding to the first frame of lidar data and the second frame of lidar data, wherein the first frame of lidar data and the second frame of lidar data are adjacent frames of data. The processing module is configured to: determine, based on the IMU data, the initial state estimate of the electronic device corresponding to the second frame of LiDAR data in the target IMU coordinate system, wherein the target IMU coordinate system is the IMU coordinate system used when acquiring the first frame of LiDAR data; determine, based on the initial state estimate, the equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system, and determine the measurement residual constraint in the target IMU coordinate system based on the first feature point corresponding to the second frame of LiDAR data; determine the current pose of the electronic device based on the equivalent prior distribution and the measurement residual constraint, and construct a global map of the target. The processing module is specifically used for: Based on the equivalent prior distribution and the measurement residual constraint, determine the target system state update group corresponding to the second frame of lidar data; Given that the state update group of the target system has converged iteratively, a new state estimate and a new covariance are obtained. The current pose of the electronic device is determined based on the new state estimate and the new covariance. The equivalent prior distribution is represented by the following expression: in, This represents the final state estimation error. Represents the Jacobian matrix J k+1 The corresponding inverse matrix, Represents the Jacobian matrix J k+1 The corresponding inverse matrix, Indicates the estimated system state The corresponding covariance, Represents the actual system state x k+1 The corresponding maximum a posteriori probability estimate, Represents the prior estimate. This represents subtraction defined on a Lie group; The target system status update group includes: This represents the new state estimation error; This represents the new state estimate, i.e., the current pose of the electronic device; Indicate the new covariance; Represents a group of Jacobian matrices; This represents the residual estimate with respect to the maximum a posteriori probability. The transpose of the first Jacobian matrix express Residual estimate with respect to maximum posterior probability The transpose of the second Jacobian matrix; K = (H T R -1 H+P -1 ) -1 H T R -1 , representing the Kalman gain matrix, Indicates the second feature point covariance, F ei Represents the residual with respect to the second characteristic point The first Jacobian matrix, F pi Represents the residual with respect to the second characteristic point The second Jacobian matrix; Indicates the measurement residual constraint group; This represents the transpose of the first objective residual function matrix. This represents the transpose of the second objective residual function matrix; I represents the identity matrix; 7. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the IMU-centered positioning and global map optimization method as described in any one of claims 1 to 5.

8. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the IMU-centered positioning and global map optimization method as described in any one of claims 1 to 5.

Images