CN-115930936-B - IMU-centered positioning and global map optimization method, device and equipment

CN115930936BCN 115930936 BCN115930936 BCN 115930936BCN-115930936-B

Abstract

The invention provides a method, a device and equipment for positioning and global map optimization by taking an IMU as a center, wherein the method comprises the steps of acquiring inertial measurement unit IMU data in a time period corresponding to first frame laser radar data and second frame laser radar data; according to the initial state estimation value, determining equivalent prior distribution corresponding to final state estimation errors under the target IMU coordinate system, determining measurement residual error constraint under the target IMU coordinate system according to first characteristic points corresponding to the second laser radar data, determining current pose corresponding to the electronic equipment according to the equivalent prior distribution and the measurement residual error constraint, and constructing a target global map. The method can obtain the current pose and the target global map with higher accuracy.

Inventors

SUN SHIYING
SHI PENGCHENG
ZHAO XIAOGUANG
ZHANG YUJIA
TAN MIN

Assignees

中国科学院自动化研究所

Dates

Publication Date: 20260505
Application Date: 20221025

Claims (8)

1. An IMU-centric positioning and global map optimization method, comprising: Acquiring Inertial Measurement Unit (IMU) data in a time period corresponding to first frame laser radar data and second frame laser radar data, wherein the first frame laser radar data and the second frame laser radar data are adjacent frame data; According to the IMU data, determining an initial state estimation value corresponding to the second frame of laser radar data of the electronic equipment under a target IMU coordinate system, wherein the target IMU coordinate system is the IMU coordinate system when the first frame of laser radar data is acquired; determining equivalent prior distribution corresponding to final state estimation errors under the target IMU coordinate system according to the initial state estimation value, and determining measurement residual error constraint under the target IMU coordinate system according to first characteristic points corresponding to the second frame of laser radar data; Determining the current pose corresponding to the electronic equipment according to the equivalent prior distribution and the measurement residual constraint, and constructing a target global map; the determining the current pose corresponding to the electronic device according to the equivalent prior distribution and the measurement residual constraint comprises the following steps: Determining a target system state update group corresponding to the second frame of laser radar data according to the equivalent prior distribution and the measurement residual error constraint; Under the condition that the iteration convergence of the target system state update group is determined, obtaining new state estimation and new covariance; determining a current pose corresponding to the electronic equipment based on the new state estimation and the new covariance; the equivalent prior distribution is represented by the following expression: Wherein, the Representing the final state estimation error(s), Representing the inverse of jacobian J k+1 , Representing the inverse of the jacobian matrix J k+1 , Representing estimated system state The corresponding covariance is obtained by the method, Represents the maximum a posteriori probability estimate corresponding to the true system state x k+1 , Representing the a priori estimates, Representing a subtraction defined on the population of plums; The target system state update group includes: Representing a new state estimation error; Representing new state estimation, namely the current pose corresponding to the electronic equipment; Representing the new covariance; Representing a set of jacobian matrices; representing residual error with respect to maximum posterior probability estimate Is a transpose of the first jacobian matrix of (c), Representation of Residual error with respect to maximum posterior probability estimate Is transposed of the second jacobian matrix of (a); k= (H T R -1 H+P -1 ) -1 H T R -1 , representing a kalman gain matrix, Representing about the second characteristic point Is F ei denotes the residual with respect to the second feature point F pi represents the residual with respect to the second feature point A second jacobian matrix of (a); Representing a set of measurement residual constraints; representing a transpose of the first matrix of target residual functions, Representing a transpose of the second target residual function matrix; I represents an identity matrix;
2. The method of claim 1, wherein determining, from the IMU data, an initial state estimate for the electronic device in the target IMU coordinate system for the second frame of lidar data comprises: According to the IMU data, determining a first kinematic model, a current system state and a current state error corresponding to the current system state, which correspond to the electronic equipment under a target IMU coordinate system; Determining a second kinematic model corresponding to the current state error according to the first kinematic model, the current system state and the current state error; and determining an initial state estimated value corresponding to the second frame of laser radar data according to the second kinematic model.
3. The method according to claim 1, wherein determining an equivalent prior distribution corresponding to a final state estimation error in the target IMU coordinate system according to the initial state estimation value includes: Determining a real system state corresponding to the target IMU coordinate system of the electronic equipment and an estimated state error corresponding to the estimated system state according to the initial state estimated value; determining equivalent prior distribution corresponding to final state estimation errors under the target IMU coordinate system according to the real system state and the estimation state errors; The determining the equivalent prior distribution corresponding to the final state estimation error in the target IMU coordinate system according to the real system state and the estimation state error comprises the following steps: Determining a maximum posterior probability estimated value corresponding to the real system state according to the real system state and the prior estimated value; performing iterative processing on the maximum posterior probability estimation value to obtain a final state estimation error; And determining equivalent prior distribution corresponding to the final state estimation error under the target IMU coordinate system according to the estimation state error.
4. The method according to claim 1, wherein determining the measurement residual constraint in the target IMU coordinate system according to the first feature point corresponding to the second frame of lidar data includes: Converting the first characteristic points corresponding to the second frame of laser radar data into the target IMU coordinate system to obtain second characteristic points; Establishing a first residual function corresponding to the second characteristic point-to-edge and a second residual function corresponding to the second characteristic-to-surface; and determining measurement residual constraints according to the first residual function and the second residual function.
5. The method of claim 4, wherein determining a measurement residual constraint from the first residual function and the second residual function comprises: 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 according to the first target residual function and the second target residual function.
6. A positioning and global map optimization apparatus, comprising: the acquisition module is used for acquiring Inertial Measurement Unit (IMU) data in a time period corresponding to first frame laser radar data and second frame laser radar data, wherein the first frame laser radar data and the second frame laser radar data are adjacent frame data; The processing module is used for determining an initial state estimation value corresponding to the second frame of laser radar data of the electronic equipment under a target IMU coordinate system according to the IMU data, wherein the target IMU coordinate system is an IMU coordinate system when the first frame of laser radar data is acquired; determining equivalent prior distribution corresponding to final state estimation errors under the target IMU coordinate system according to the initial state estimation value, and determining measurement residual error constraint under the target IMU coordinate system according to first characteristic points corresponding to the second frame of laser radar data; The processing module is specifically configured to: Determining a target system state update group corresponding to the second frame of laser radar data according to the equivalent prior distribution and the measurement residual error constraint; Under the condition that the iteration convergence of the target system state update group is determined, obtaining new state estimation and new covariance; determining a current pose corresponding to the electronic equipment based on the new state estimation and the new covariance; the equivalent prior distribution is represented by the following expression: Wherein, the Representing the final state estimation error(s), Representing the inverse of jacobian J k+1 , Representing the inverse of the jacobian matrix J k+1 , Representing estimated system state The corresponding covariance is obtained by the method, Represents the maximum a posteriori probability estimate corresponding to the true system state x k+1 , Representing the a priori estimates, Representing a subtraction defined on the population of plums; The target system state update group includes: Representing a new state estimation error; Representing new state estimation, namely the current pose corresponding to the electronic equipment; Representing the new covariance; Representing a set of jacobian matrices; representing residual error with respect to maximum posterior probability estimate Is a transpose of the first jacobian matrix of (c), Representation of Residual error with respect to maximum posterior probability estimate Is transposed of the second jacobian matrix of (a); k= (H T R -1 H+P -1 ) -1 H T R -1 , representing a kalman gain matrix, Representing about the second characteristic point Is F ei denotes the residual with respect to the second feature point F pi represents the residual with respect to the second feature point A second jacobian matrix of (a); Representing a set of measurement residual constraints; representing a transpose of the first matrix of target residual functions, Representing a transpose of the second target residual function matrix; I represents an identity matrix;
7. An electronic device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, wherein the processor implements the IMU-centric localization and global map optimization method of any one of claims 1 to 5 when the program is executed by the processor.
8. A non-transitory computer readable storage medium having stored thereon a computer program, which when executed by a processor implements the IMU-centric localization and global map optimization method of any one of claims 1 to 5.

Description

IMU-centered positioning and global map optimization method, device and equipment Technical Field The present invention relates to the field of map construction technologies, and in particular, to a method, an apparatus, and a device for positioning and global map optimization with IMU as a center. Background With the vigorous development of the automatic driving field, a laser radar (Light Detection AND RANGING, light Detection) sensor and an inertial measurement unit (Inertial Measurement Unit, IMU) have been popularized in a large scale, and a robot can utilize the Light radar sensor and the IMU to construct a global map of a target corresponding to the current environment of the robot. The existing global map construction method is that after the robot acquires laser radar data acquired by the LiDAR sensor and IMU data acquired by the IMU, the data can be processed by a Kalman gain calculation method to construct a target global map, however, in the whole process, the performance of an Extended Kalman filtering state estimator (Extended KALMAN FILTER, EKF) is easy to be reduced due to poor initialization of system state estimation, so that the positioning accuracy of the robot is reduced, and the problem that the target global map finally acquired by the robot is inaccurate can occur. Disclosure of Invention The invention provides a positioning and global map optimizing method, device and equipment taking an IMU as a center, which are used for solving the defect that in the existing global map constructing method, the positioning accuracy of a robot is reduced, so that a target global map finally obtained by the robot is not accurate enough, realizing the equivalent prior distribution and measurement residual constraint corresponding to a final state estimation error under a target IMU coordinate system, effectively constructing a target global map with higher accuracy corresponding to the current environment of an electronic device, and in addition, accurately obtaining the current pose of the electronic device in the current environment of the electronic device. The invention provides a positioning and global map optimization method taking an IMU as a center, which comprises the following steps: Acquiring Inertial Measurement Unit (IMU) data in a time period corresponding to first frame laser radar data and second frame laser radar data, wherein the first frame laser radar data and the second frame laser radar data are adjacent frame data; According to the IMU data, determining an initial state estimation value corresponding to the second frame of laser radar data of the electronic equipment under a target IMU coordinate system, wherein the target IMU coordinate system is the IMU coordinate system when the first frame of laser radar data is acquired; According to the initial state estimation value, determining equivalent prior distribution corresponding to final state estimation errors under the target IMU coordinate system, and according to first characteristic points corresponding to the second frame of laser radar data, determining measurement residual error constraint under the target IMU coordinate system; And determining the current pose corresponding to the electronic equipment according to the equivalent prior distribution and the measurement residual constraint, and constructing a target global map. The method for positioning and global map optimization by taking the IMU as the center comprises the steps of determining an initial state estimation value corresponding to second frame of laser radar data of electronic equipment in a target IMU coordinate system according to IMU data, determining a first kinematic model corresponding to the electronic equipment in the target IMU coordinate system, a current system state and a current state error corresponding to the current system state according to the IMU data, determining a second kinematic model corresponding to the current state error according to the first kinematic model, the current system state and the current state error, and determining an initial state estimation value corresponding to the second frame of laser radar data according to the second kinematic model. The method for positioning and global map optimization by taking the IMU as the center comprises the steps of determining equivalent prior distribution corresponding to final state estimation errors under a target IMU coordinate system according to initial state estimation values, determining real system states corresponding to the target IMU coordinate system of electronic equipment and estimation state errors corresponding to the estimation system states according to the initial state estimation values, and determining equivalent prior distribution corresponding to final state estimation errors under the target IMU coordinate system according to the real system states and the estimation state errors. The method for optimizing the positioning and global map by taking the IMU