A map building method, apparatus, computer device, and storage medium

Abstract

This invention relates to the field of intelligent algorithm technology, and discloses a map construction method, apparatus, computer device, and storage medium. The method includes: acquiring IMU sensor data at various timestamps and laser pose data for each acquired image frame; performing pose prediction calculations on the IMU sensor data at each timestamp to obtain the predicted pose for the corresponding timestamp; matching the timestamps of the IMU sensor data with the image frames of the laser pose data to establish correspondences between IMU sensor data and laser pose data, between IMU sensor data and image frames, and between image frames and predicted poses; setting corresponding inter-frame constraints for each current image frame to optimize the laser pose data of the current image frame to obtain the optimized pose of the current frame; and constructing a global map based on the optimized poses corresponding to each image frame. This invention improves the accuracy of map construction.

Description

Technical Field

[0001] This invention relates to the field of map building technology, and specifically to a map building method, apparatus, computer equipment, and storage medium. Background Technology

[0002] In SLAM (Simultaneous Localization and Mapping) technology, LiDAR and IMU (Inertial Measurement Unit) are widely used for environmental perception and localization of mobile robots. LiDAR obtains distance information about the environment by emitting a laser beam and measuring its return time, while IMU is used to measure the vehicle's acceleration and angular velocity.

[0003] Traditional methods typically use a single sensor for localization, resulting in significant positioning errors and poor real-time performance. Existing technologies utilize loosely coupled localization systems for mobile robot localization. In these systems, the LiDAR and IMU operate independently, with low coupling between sensors. For example, in 2016, Google open-sourced Cartographer, a SLAM algorithm framework that achieves low computational resource consumption in multi-sensor configurations. However, its front-end motion estimation is based on the assumption of a uniform velocity model and does not consider the inherent constraints between observations from multiple sensors. Therefore, Cartographer's multi-sensor fusion is loosely coupled, primarily relying on LiDAR localization, and IMU and odometry data are not incorporated into the true objective function.

[0004] The above method only reflects the fusion of lidar and IMU at the result level, without considering the inherent constraints between the two observations, thus resulting in poor accuracy of the constructed global map. Summary of the Invention

[0005] In view of this, the present invention provides a map building method, apparatus, computer equipment, and storage medium to solve the problem of poor accuracy in global map building.

[0006] In a first aspect, the present invention provides a map construction method, the method comprising:

[0007] Acquire IMU sensor data at each timestamp and laser pose data for each image frame acquired;

[0008] Perform pose prediction calculations on the IMU sensor data for each timestamp to obtain the predicted pose for the corresponding timestamp;

[0009] The timestamps of the IMU sensor data are matched with the image frames of the laser pose data to establish the correspondence between the IMU sensor data and the laser pose data, the correspondence between the IMU sensor data and the image frames, and the correspondence between the image frames and the predicted pose.

[0010] For each current image frame, corresponding inter-frame constraints are set to optimize the laser pose data of the current image frame and obtain the optimized pose of the current frame. The inter-frame constraints are set based on the IMU sensor data and predicted pose of the previous image frame of the current image frame, as well as the IMU sensor data, laser pose data and predicted pose of the current image frame.

[0011] A global map is constructed based on the optimized poses corresponding to each image frame.

[0012] The above scheme tightly couples IMU sensor data and laser pose data by matching IMU sensor data with laser pose data and setting inter-frame constraints based on the IMU sensor data and laser pose data. This makes data fusion of IMU sensor data and laser pose data easier and can solve the problem of degradation scenarios. Pose prediction calculation is performed on the IMU sensor data for each timestamp, which reduces the impact of noise and drift, reduces cumulative error, improves positioning accuracy, and thus improves the accuracy of map construction.

[0013] In one optional implementation, pose prediction calculation is performed on the IMU sensor data for each timestamp to obtain the predicted pose for the corresponding timestamp, including:

[0014] Obtain the IMU pre-integration model; the IMU pre-integration model is a pre-established model used to predict the pose change between two timestamps;

[0015] For each timestamp, the IMU sensor data of the current timestamp is input into the IMU pre-integration model to obtain the predicted pose change from the previous timestamp to the current timestamp;

[0016] The predicted pose of the current timestamp is calculated based on the predicted pose of the previous timestamp and the change in predicted pose from the previous timestamp to the current timestamp.

[0017] The above scheme further improves the accuracy of map construction by setting a high-precision IMU pre-integration model to perform pose prediction calculations.

[0018] In one optional implementation, while inputting the IMU sensor data of the current timestamp into the IMU pre-integration model to obtain the predicted pose change from the previous timestamp to the current timestamp, the predicted velocity change from the previous timestamp to the current timestamp is also obtained.

[0019] The step of calculating the predicted pose at the current timestamp based on the predicted pose at the previous timestamp and the change in predicted pose from the previous timestamp to the current timestamp includes:

[0020] Multiply the predicted pose of the previous timestamp by the change in the predicted pose to obtain the predicted pose of the current timestamp.

[0021] The method further includes:

[0022] Multiply the predicted velocity of the previous timestamp by the change in the predicted velocity to obtain the predicted velocity of the current timestamp.

[0023] The above scheme introduces the predicted velocity change and predicted velocity, which increases the number of parameters in the map building process and improves the accuracy of map building.

[0024] In one optional implementation, for each current image frame, corresponding inter-frame constraints are set to optimize the laser pose data of the current image frame and obtain the optimized pose of the current frame, including:

[0025] IMU constraints are set based on the predicted pose and velocity of the previous image frame, the predicted pose and velocity of the current image frame, and the changes in predicted pose and velocity between the previous and current image frames.

[0026] IMU zero-bias constraints are set based on the zero-bias of the previous image frame, the estimated zero-bias of the current image frame, the measured zero-bias of the current image frame, and the covariance matrix of the current image frame. The zero-bias of the previous image frame is the zero-bias after optimization of the previous image frame stored in the IMU pre-integration model. The estimated zero-bias of the current image frame is the zero-bias estimated based on the IMU sensor data of the current image frame. The covariance matrix of the current image frame is calculated by the IMU pre-integration model based on the current IMU sensor data.

[0027] Set pose constraints based on the predicted pose corresponding to the current image frame, the laser pose data corresponding to the current image frame, and the covariance matrix corresponding to the current image frame.

[0028] Based on the IMU constraints, IMU zero bias constraints, and pose constraints, the laser pose data corresponding to the current image frame is optimized to obtain the optimized pose corresponding to the current image frame.

[0029] The above scheme further defines the inter-frame constraints and refines the scheme.

[0030] In an optional implementation, the method further includes:

[0031] Based on the IMU constraints, IMU zero bias constraints and pose constraints, the estimated zero bias corresponding to the current image frame is optimized to obtain the zero bias corresponding to the current image frame.

[0032] The IMU pre-integration model is updated using the zero bias corresponding to the current image frame.

[0033] The above scheme updates the zero bias during optimization and updates the IMU pre-integration model with the updated zero bias, so that the IMU pre-integration model can be adjusted in a timely manner, thereby improving the accuracy of map construction.

[0034] In an optional implementation, after matching the timestamp of the IMU sensor data with the image frame of the laser pose data, the method further includes:

[0035] For each image frame, the pose change between the previous image frame and the current image frame is calculated based on the predicted pose corresponding to the current image frame and the optimized pose corresponding to the previous image frame.

[0036] Based on the pose change between the previous image frame and the current image frame, point cloud distortion correction is performed on the laser pose data corresponding to the current image frame.

[0037] The above solution improves the accuracy of laser pose data by point cloud distortion correction, thereby improving the accuracy of map construction.

[0038] In one optional implementation, the step of performing point cloud distortion correction on the laser pose data corresponding to the current image frame based on the pose change between the previous image frame and the current image frame includes:

[0039] Traverse the laser pose data of the current image frame to obtain the interval time between each laser scan data point in the laser pose data of the current image frame and the last laser scan data point in the laser pose data of the current image frame.

[0040] Based on the pose change between the previous image frame and the current image frame and the interval time, the pose change of each laser scanning data point in the laser pose data corresponding to the current image frame relative to the optimized pose corresponding to the previous image frame is linearly interpolated.

[0041] Based on the pose change of each laser scanning data point relative to the optimized pose of the previous image frame, the laser pose of each laser scanning data point in the laser coordinate system is transformed to the global map coordinate system in order to perform point cloud distortion correction.

[0042] The above scheme further defines the point cloud distortion removal process and refines the scheme.

[0043] In a second aspect, the present invention provides a map building apparatus, the apparatus comprising:

[0044] The data acquisition module is used to acquire IMU sensor data at each time stamp and laser pose data for each image frame acquired.

[0045] The pose prediction module is used to perform pose prediction calculations on the IMU sensor data at each timestamp to obtain the predicted pose for the corresponding timestamp.

[0046] The matching module is used to match the timestamp of the IMU sensor data with the image frame of the laser pose data to establish the correspondence between the IMU sensor data and the laser pose data, the correspondence between the IMU sensor data and the image frame, and the correspondence between the image frame and the predicted pose.

[0047] The inter-frame constraint module is used to set corresponding inter-frame constraint conditions for each current image frame in order to optimize the laser pose data of the current image frame and obtain the optimized pose of the current frame. The inter-frame constraint conditions are set according to the IMU sensor data and predicted pose of the previous image frame of the current image frame, as well as the IMU sensor data, laser pose data and predicted pose of the current image frame.

[0048] The map building module is used to construct a global map based on the optimized poses corresponding to each image frame.

[0049] Thirdly, the present invention provides a computer device, including: a memory and a processor, the memory and the processor being communicatively connected to each other, the memory storing computer instructions, and the processor executing the computer instructions to perform the map construction method of the first aspect or any corresponding embodiment described above.

[0050] Fourthly, the present invention provides a computer-readable storage medium storing computer instructions for causing a computer to perform the map construction method of the first aspect or any corresponding embodiment thereof. Attached Figure Description

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

[0052] Figure 1 This is a schematic flowchart of a map construction method according to an embodiment of the present invention;

[0053] Figure 2 This is a flowchart illustrating another map construction method according to an embodiment of the present invention;

[0054] Figure 3 A schematic diagram of a map construction method according to an embodiment of this application is shown;

[0055] Figure 4 A flowchart illustrating the static initialization process described in this application embodiment is shown;

[0056] Figure 5 A flowchart illustrating the establishment of the pre-integral model involved in an embodiment of this application is shown;

[0057] Figure 6 A flowchart illustrating point cloud distortion correction according to an embodiment of this application is shown;

[0058] Figure 7 A schematic diagram of inter-frame constraints related to an embodiment of this application is shown;

[0059] Figure 8 A flowchart illustrating the joint optimization involved in an embodiment of this application is shown;

[0060] Figure 9 The following diagrams show a comparison of the mapping effects involved in the embodiments of this application;

[0061] Figure 10 A schematic diagram of local subgraph nodes in related technologies is shown;

[0062] Figure 11 A schematic diagram of a partial subgraph node involved in an embodiment of this application is shown;

[0063] Figure 12 This is a structural block diagram of a map building apparatus according to an embodiment of the present invention;

[0064] Figure 13 This is a schematic diagram of the hardware structure of a computer device according to an embodiment of the present invention. Detailed Implementation

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

[0066] In SLAM technology, LiDAR and IMU are widely used for environmental perception and localization of mobile robots. LiDAR obtains distance information about the environment by emitting a laser beam and measuring its return time, while IMU is used to measure the vehicle's acceleration and angular velocity. Traditional methods typically use a single sensor for localization, resulting in large localization errors and poor real-time performance. Existing technologies use loosely coupled localization systems for mobile robot localization, in which LiDAR and IMU operate independently, with low coupling between the sensors. For example, in 2016, Google open-sourced Cartographer, a SLAM algorithm framework that achieves low computational resource consumption in multi-sensor configurations. However, its front-end motion estimation is based on the assumption of a uniform velocity model and does not consider the inherent constraints between observations from multiple sensors. Therefore, Cartographer's multi-sensor fusion is loosely coupled, primarily relying on LiDAR positioning. IMU and odometry data are not incorporated into the true objective function. This loose coupling only affects the results of radar and IMU fusion, failing to consider the inherent constraints between the two observations and thus unable to address some degradation scenarios. Other problems include: a) Data inconsistency: Due to the different frequencies and properties of LiDAR and IMU data, their data may be inconsistent in time and space, leading to difficulties in data fusion; b) Noise and drift: Both LiDAR and IMU are affected by noise and sensor drift, which can lead to inaccuracies in localization and map building; c) Difficulty in motion estimation: LiDAR has limitations in motion estimation within the environment, while IMU accuracy may accumulate errors over long-term use. All these problems contribute to poor accuracy in map building in related technologies.

[0067] Therefore, this invention provides a map construction method that optimizes the map construction by establishing an IMU pre-integration model, fully considering the inter-frame constraints of IMU and radar observation data, in order to solve the problem of poor map construction accuracy in the prior art.

[0068] According to an embodiment of the present invention, a map construction method embodiment is provided. It should be noted that the steps shown in the flowchart in the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions. Furthermore, although a logical order is shown in the flowchart, in some cases, the steps shown or described may be executed in a different order than that shown here.

[0069] This embodiment provides a map building method that can be used on the aforementioned mobile terminals, such as tablet computers. Figure 1 This is a flowchart of a map construction method according to an embodiment of the present invention, such as... Figure 1 As shown, the process includes the following steps:

[0070] Step S101: Obtain IMU sensor data at each time stamp and laser pose data for each acquired image frame.

[0071] IMU (Inertial Measurement Unit) sensor data refers to data measured by IMU sensors, including acceleration and angular velocity. IMU sensors can be mounted on target objects used for map building, such as vehicles or robots used for map creation. The IMU sensors measure the target object's data at various time stamps. Laser pose data is obtained by acquiring distance information from the surrounding environment using a lidar system to form a point cloud. This point cloud is then preprocessed and scanned using a laser odometry system. The lidar system can be mounted on the target object for data acquisition. The lidar measures the target object's laser pose data in various image frames; pose refers to position and orientation.

[0072] Step S102: Perform pose prediction calculation on the IMU sensor data for each timestamp to obtain the predicted pose for the corresponding timestamp.

[0073] The predicted pose at a given timestamp can be calculated by performing pose prediction calculations based on IMU sensor data from a certain number of timestamps prior to that timestamp. For example, predicting the pose for the next timestamp based on IMU sensor data from the current timestamp. This pose prediction calculation can be performed using a pre-trained neural network model for pose prediction or a validated mathematical model for pose prediction. This process is continuous; whenever new IMU sensor data is acquired, the predicted pose for the next timestamp is calculated.

[0074] Step S103: Match the timestamp of the IMU sensor data with the image frame of the laser pose data to establish the correspondence between the IMU sensor data and the laser pose data, the correspondence between the IMU sensor data and the image frame, and the correspondence between the image frame and the predicted pose.

[0075] Because the acquisition frequencies of IMU sensor data and laser pose data are different, that is, the intervals between two adjacent timestamps and two adjacent image frames are different, for example, the interval between two timestamps is shorter, in order to establish the correspondence between IMU sensor data and laser pose data, it is necessary to first match the timestamps and image frames corresponding to the same moment, process the two states of the target object (IMU sensor data and laser pose data) at the same moment, and then obtain the correspondence between IMU sensor data and image frames, and the correspondence between image frames and predicted pose.

[0076] Step S104: For each current image frame, set corresponding inter-frame constraints to optimize the laser pose data of the current image frame and obtain the optimized pose of the current frame.

[0077] The inter-frame constraint is set based on the IMU sensor data and predicted pose of the previous image frame of the current image frame, as well as the IMU sensor data, laser pose data, and predicted pose of the current image frame.

[0078] Ideally, for the same image frame, the predicted pose of the image frame based on IMU data and the pose in the corresponding laser pose data of the image frame should be the same. Therefore, IMU sensor data and laser pose data are combined, and inter-frame constraints are set for each image frame based on the IMU sensor data and predicted pose of the previous image frame, as well as the IMU sensor data, laser pose data and predicted pose of the current image frame, to increase the accuracy of the laser pose data corresponding to the current image frame.

[0079] Step S105: Construct a global map based on the optimized poses corresponding to each image frame.

[0080] After optimizing the laser pose data of each image frame, a global map can be constructed based on the optimized poses corresponding to each image frame. Optionally, global map construction can be performed using SLAM (simultaneous localization and mapping).

[0081] It should be noted that global map construction is divided into pose determination and map construction. In this application, steps S101 to S104 are for pose determination, and step S105 is for map construction. Map construction is divided into local mapping and global mapping. Local SLAM, also known as the front end, matches laser pose data with IMU sensor data and uses it as one of the inter-frame constraints in the optimization of step S104. Based on the optimization results, local sub-maps are constructed, and the results are input to the back end. Global SLAM, also known as the back end, mainly performs global optimization and global mapping. Each time the front end writes a frame of point cloud (laser pose data) to the local sub-map, it sends this frame of point cloud and the corresponding target object (such as a car or robot) pose to the back end for constraint calculation. After the back end accumulates a certain number of frame inputs (e.g., 90 frames) of point cloud and completes the constraint calculation, it begins global optimization, finally outputting a high-precision map and the optimized robot pose.

[0082] It should be noted that SLAM is divided into mapping mode and localization mode, and this solution mainly describes the mapping mode. When applying SLAM to a target object, the mapping mode is first activated to build a map of the entire scene. This mode only needs to be activated once. After the map is built, the constructed global map is used as the localization map for subsequent target object localization. In other words, after building the global map using this solution, the global map will subsequently be used as the localization map. The target object will load the previously built localization map and activate the localization mode. The main purpose of the localization mode is to obtain the real-time pose (localization data) of the target object. At this time, the front-end's main role is matching, and the back-end's main role is optimization. Although a global map is also built, the back-end will delete the newly created global map after optimization. In the localization mode, the pose of the target object is also optimized through steps S101 to S104. This solution can also improve the localization accuracy in the localization mode.

[0083] The map construction method provided in this embodiment matches IMU sensor data with laser pose data and sets inter-frame constraints based on the IMU sensor data and laser pose data, thus tightly coupling the IMU sensor data and laser pose data. This makes data fusion of IMU sensor data and laser pose data easier and can solve the problem of degradation scenarios. Pose prediction calculation is performed on the IMU sensor data for each timestamp, which reduces the impact of noise and drift, reduces cumulative error, improves positioning accuracy, and thus improves the accuracy of map construction.

[0084] This embodiment provides a map building method that can be used on the aforementioned mobile terminals, such as mobile phones and tablets. Figure 2 This is a flowchart of a map construction method according to an embodiment of the present invention, such as... Figure 2As shown, the process includes the following steps:

[0085] Step S201: Obtain IMU sensor data at each time stamp and laser pose data for each acquired image frame.

[0086] For example, Figure 3 A schematic diagram of a map construction method according to an embodiment of this application is shown. Figure 3 As shown, map construction is divided into several parts. Steps S201 to S205 of this application are performed in the motion estimation unit of the tightly coupled multi-sensor system. Since this application mainly focuses on the design of the motion estimation unit of the tightly coupled multi-sensor system, the construction of the global map based on the optimized pose is briefly described in step 206. The sensor unit collects laser pose data and IMU sensor data. Local SLAM, also known as the front end, performs brute-force search matching through CSM (correlative scan matching) and fine matching through Ceres (an open-source C++ library). This serves as a laser pose constraint and works with the motion estimation unit of the tightly coupled multi-sensor system to perform local optimization (joint optimization). Based on the joint optimization results, a local sub-map is constructed, and the local sub-map construction results are input to the back end. Global SLAM, also known as the backend, mainly performs global optimization and global mapping. Each time the frontend writes a frame of point cloud (laser pose data) to the local sub-map, it sends this frame of point cloud and the pose of the corresponding target object (such as a car or robot) to the backend for constraint calculation. After the backend accumulates a certain number of frame inputs (e.g., 90 frames) of point cloud and completes the constraint calculation, it starts global optimization and finally outputs a high-precision map and the optimized robot pose.

[0087] It should be noted that SLAM is divided into mapping mode and localization mode, and this solution mainly describes the mapping mode. When applying SLAM to a target object, the mapping mode is first activated to build a map of the entire scene. This mode only needs to be activated once. After the map is built, the constructed global map is used as the localization map for subsequent target object localization. In other words, after building the global map using this solution, the global map will subsequently be used as the localization map. The target object will load the previously built localization map and activate the localization mode. The main purpose of the localization mode is to obtain the real-time pose (localization data) of the target object. At this time, the front-end's main role is matching, and the back-end's main role is optimization. Although a global map is also built, the back-end will delete the newly created global map after optimization. In the localization mode, the pose of the target object is also optimized through steps S201 to S205. This solution can also improve the localization accuracy in the localization mode.

[0088] For details of step S201, please refer to [link / reference]. Figure 1Step S101 of the illustrated embodiment will not be described again here.

[0089] Step S202: Perform pose prediction calculation on the IMU sensor data for each timestamp to obtain the predicted pose for the corresponding timestamp.

[0090] Specifically, step S202 includes:

[0091] Step S2021: Obtain the IMU pre-integration model.

[0092] The IMU pre-integration model is a pre-built model used to predict the amount of pose change between two timestamps.

[0093] Optionally, static initialization can be performed by estimating the initial pose of the target object (such as a car or robot) using IMU sensor data at the initial power-on moment. The target object is the object equipped with IMU sensors and LiDAR to collect relevant data. In practical applications, the default pose of the target object upon power-on is a unit quaternion, meaning that it is assumed that the IMU sensors in the target object are approximately horizontal at the initial moment. However, in reality, the target object may have a certain pose upon power-on, such as when the target object has not adjusted its pose after operation, when the target object is placed in a handheld backpack during dataset recording, or when the robot is not placed horizontally in special circumstances. Therefore, static initialization is required.

[0094] Figure 4 A flowchart of the static initialization process according to an embodiment of this application is shown. For example, if static initialization has not been performed previously and the front end has accumulated num frames of point cloud data (i.e., laser pose data), static initialization begins. Static initialization is divided into a preprocessing stage and a formal initialization stage. In the preprocessing stage, the average linear acceleration and angular velocity of the IMU sensor data input to the front end during the accumulation of num frames of point cloud data are calculated to estimate the initial rotation vector (the rotation vector is a vector whose direction is the rotation axis and whose magnitude is the rotation angle) and the IMU initial zero bias. In the formal processing stage, the obtained initial rotation vector and the average value of the accumulated IMU sensor data are used to initialize the pose and velocity of the IMU sensor. The estimated IMU initial zero bias is used to initialize the IMU pre-integrator, thus completing the initialization of the IMU pre-integrator and the estimation of the initial pose and velocity.

[0095] Step S2022: For each timestamp, input the IMU sensor data of the current timestamp into the IMU pre-integration model to obtain the predicted pose change from the previous timestamp to the current timestamp.

[0096] Step S2023: Calculate the predicted pose of the current timestamp based on the predicted pose of the previous timestamp and the change in predicted pose from the previous timestamp to the current timestamp.

[0097] Optionally, the predicted pose of the previous timestamp can be multiplied by the change in the predicted pose to obtain the predicted pose of the current timestamp.

[0098] In step S2024, the IMU sensor data at the current timestamp is input into the IMU pre-integration model to obtain the predicted pose change from the previous timestamp to the current timestamp, and at the same time, the predicted velocity change from the previous timestamp to the current timestamp is also obtained.

[0099] Optionally, the predicted velocity at the previous timestamp can be multiplied by the change in the predicted velocity to obtain the predicted velocity at the current timestamp.

[0100] Optionally, the process of establishing the IMU pre-integration model is as follows:

[0101] The integral process of PVQ (P is position, V is velocity, and Q is attitude) over continuous time is as follows:

[0102]

[0103] Among them, formulas (1) to (3) represent the integral processes of IMU position, velocity and attitude over continuous time. Represents t k+1 The location of the IMU at any given time. Represents t k The position at that moment; They represent t respectively k+1 and t k The speed of time; They represent t respectively k+1 t k And the attitude at time t during the integration process; This represents the rotation of the IMU coordinate system relative to the world coordinate system at time t; n a n w g w These represent the IMU's acceleration, angular velocity, zero bias, acceleration noise, angular velocity noise, and the acceleration component in the world coordinate system, respectively; Ω represents the antisymmetric matrix.

[0104] Multiply both sides of the equation by the world coordinate system to b. k Transformation of frame IMU coordinate system as well as To ensure that inter-frame constraints are independent of the pose between two frames and depend only on the observations, the following formula is obtained:

[0105]

[0106] in:

[0107]

[0108] In the formula Represents continuous time t k to t k+1 Two frames (b) k b k+1 The predicted changes in position, velocity, and attitude between IMU data. (Using b) k Using the reference image frame, it can be seen from equation (4) that in b k to b k+1 Between frames, the three predicted changes are only related to the IMU's bias state and are independent of other states.

[0109] Furthermore, in real-world scenarios, the data obtained is discrete IMU sensor data. Therefore, it is necessary to transform the continuous problem into a discrete problem and use the median integral to approximate the acceleration during the time interval from i to i+1, as shown in the following formula:

[0110]

[0111] The pre-integration formula derived from the median integral assumes that the acceleration and angular velocity of the IMU sensor remain constant. However, in reality, the acceleration, angular velocity, and attitude of the IMU sensor change constantly. Furthermore, the IMU sensor acquires data at a very high frequency, such as 100Hz, and the interval between two data points is extremely short. If we assume that the acceleration and angular velocity remain constant over a short period, then the above formula... The three values ​​in the pre-integration are replaced by the average of the acceleration, angular velocity, and attitude of the two consecutive frames. Therefore, the pre-integration of the IMU in discrete time is continuously updated by means of median integration. For each new IMU sensor data received from the front-end IMU sensor data queue, median integration is performed to update the predicted attitude for the next time step.

[0112] Since the bias will be updated in subsequent step 205, the most recent predicted changes (predicted pose changes and predicted velocity changes) calculated by the IMU pre-integration model will be recalculated using the updated bias for the next pose prediction calculation and joint optimization. However, recalculation is computationally expensive. Therefore, the first-order Jacobi approximation can be used to calculate the predicted changes after bias optimization, as shown in the following formula:

[0113] f(X+ΔX)=f(X)+JΔX (6)

[0114] Where X represents the zero bias, ΔX represents the change in the zero bias, and J represents the Jacobian matrix. Substituting equation (6) into equation (5) yields the modeling process for the zero bias in the IMU pre-integration model, as shown below:

[0115]

[0116] Since multiple measurement devices (IMU sensors, LiDAR, etc.) are used in the map construction process, the covariance matrix can be used for information fusion and weight allocation during the fusion of data acquired from multiple measurement devices. In the calculation of the IMU pre-integration model, the covariance is used to estimate the uncertainty of the actual measurement values ​​and the impact of error propagation. The covariance can be used to describe the confidence level of the predicted change; the smaller the covariance, the greater the confidence level.

[0117] The error Kalman filter method is used when solving for the covariance matrix (confidence level) and the Jacobian matrix. The covariance matrix is ​​initialized as a 15×15 matrix with values ​​of 0, and the Jacobian matrix is ​​initialized as a 15×15 identity matrix. The covariance matrix and Jacobian matrix are updated as the predicted changes are calculated, as follows:

[0118] Based on the error Kalman filter principle, the propagation of the IMU error state in discrete time can be calculated, as shown in the following formula:

[0119]

[0120] Where, δα k+1 ,δθ k+1 ,δβ k+1 ,δb ak+1 ,δb wk+1 They represent t respectively k+1 The errors of five state variables at any given time: position, velocity, attitude, acceleration bias, and angular velocity bias; δα k ,δθ k ,δβ k ,δb ak ,δb wk They represent t respectively k Errors in five state variables: position, velocity, attitude, acceleration bias, and angular velocity bias; n ak n wk , They represent t respectively k Gaussian white noise with constant acceleration, angular velocity, zero acceleration bias, and zero angular velocity bias, n ak+1 n wk+1 They represent t respectively k+1 Gaussian white noise for instantaneous acceleration and angular velocity. 01～24 Indicates t k+1Error at time t k The coefficient of the time error is represented as a 15×15 F matrix, v 00～23 Indicates t k+1 Time error with respect to t k The coefficients of the time-order noise are represented as a 15×18 G matrix. R k R k Indicates t k and t k+1 The amount of rotation of the IMU coordinate system relative to the world coordinate system at any given time.

[0121] Update the Jacobian matrix J and the covariance matrix P using the following formulas:

[0122] J k+1 =FJ k (9)

[0123] P k+1 =FP k F T +Gn k G T (10)

[0124] Among them, J k J is the Jacobian matrix at time k. k+1 It is the Jacobian matrix at time k+1, P k P is the covariance matrix at time k. k+1 It is the covariance matrix at time k+1, matrix F T It is the transpose of F, matrix G T It is the transpose of G.

[0125] In practical applications, the above update calculation process is solved using a median integral function. Each time new IMU sensor data is acquired, it is input into the median integral function along with the difference between the corresponding timestamps of the two previous IMU sensor data to perform update calculations of the predicted change, Jacobian matrix, and covariance matrix.

[0126] It should be noted that the purpose of pose prediction calculation is to provide a more accurate prior pose for subsequent steps (point cloud distortion correction, factor map construction). For example, the optimized pose corresponding to the previous image frame is TR (i.e., the position T and orientation R of the target object in the local map coordinate system at time t1 of the previous image frame). The predicted position change P and predicted orientation change Q calculated by the IMU pre-integration model can represent the change in pose of the target object (which can also be understood as the IMU) in the local map coordinate system from t1 to the current time t2. Multiplying T and R by P and Q respectively on the right yields the pose of the target object in the local map coordinate system at the current time t2. Using this method, the prior pose of the target object in the local coordinate system at each time corresponding to the IMU sensor data can be predicted. Optionally, the predicted prior pose can be stored in a container vector.

[0127] For example, Figure 5 A flowchart illustrating the establishment of the pre-integral model involved in an embodiment of this application is shown.

[0128] Step S203: For each image frame, calculate the pose change between the previous image frame and the current image frame based on the predicted pose corresponding to the current image frame and the optimized pose corresponding to the previous image frame; and perform point cloud distortion correction on the laser pose data corresponding to the current image frame based on the pose change between the previous image frame and the current image frame.

[0129] Specifically, step S203 includes:

[0130] Step S2031: Traverse the laser pose data of the current image frame to obtain the interval time between each laser scan data point in the laser pose data of the current image frame and the last laser scan data point in the laser pose data of the current image frame.

[0131] Laser pose data does not undergo distortion when the target object is stationary. However, the target object is constantly moving during map construction, while LiDAR scanning is performed periodically, and each image frame's point cloud contains multiple laser scan data points (e.g., each image frame's point cloud contains 1800 laser scan data points, and the radar scan cycle is 0.02 seconds). The time corresponding to each laser scan data point is also different, meaning that each laser scan data point corresponds to a different laser coordinate system. In subsequent steps, it is necessary for all laser scan data points in the point cloud of an image frame to correspond to the same global map coordinate system. Therefore, point cloud distortion correction is required, transforming the coordinates of each laser scan data point in the vehicle coordinate system to the global map coordinate system.

[0132] In related technologies, under the assumption that the target object moves at a constant speed, the pose of the laser scanning data points at the corresponding moment is calculated using pose data acquired by other sensors (such as IMU, wheel odometry) or laser odometry, and then converted to the global map coordinate system for point cloud distortion correction. This method can achieve the desired effect under constant or slow speed conditions, but it cannot handle high-speed motion or scene degradation environments well. The point cloud distortion correction in this solution is based on the optimized pose corresponding to the previous image frame and the predicted pose corresponding to the current image frame. Both the optimized pose and the predicted pose are processed and relatively accurate pose data, so the accuracy of point cloud distortion correction is higher and it is not affected by high-speed motion or scene degradation.

[0133] Optionally, when the laser pose data of an image frame is acquired, the point cloud and laser pose data are synchronized in time, and it is determined whether the point cloud of the current image frame is the first frame image data. If it is, it is used as the initial pose for the aforementioned static initialization; otherwise, point cloud distortion is performed normally.

[0134] Optionally, if the point cloud of the current image frame is not the first frame image data, the IMU sensor data before the corresponding time of the current image frame is deleted when performing point cloud distortion correction, so that the map building process after static initialization is based on the data acquired at the current time and thereafter.

[0135] Step S2032: Based on the pose change between the previous image frame and the current image frame and the time interval, linearly interpolate the pose change of each laser scanning data point in the laser pose data corresponding to the current image frame relative to the optimized pose corresponding to the previous image frame.

[0136] Step S2033: Based on the pose change of each laser scanning data point relative to the optimized pose of the previous image frame, the laser pose of each laser scanning data point in the laser coordinate system is transformed to the global map coordinate system in order to perform point cloud distortion removal.

[0137] For example, Figure 6 A flowchart illustrating point cloud distortion correction according to an embodiment of this application is shown. Figure 6As shown, when the laser pose data of an image frame is acquired, point cloud time synchronization begins. After synchronization, it is determined whether the point cloud of the current image frame is the data from the first frame used for front-end optimization and mapping. If so, the pose of the target object in the local coordinate system (local_slam) corresponding to all laser scanning data points in the point cloud data of this image frame is used as the initial pose of the IMU pre-integration model during static initialization. If not, the predicted pose corresponding to the current image frame predicted by the IMU pre-integration model is retrieved. This pose is accumulated from the target object's power-on state to the current moment, which is the robot's pose P1 in the local coordinate system (local_slam) under this point cloud. The result of the latest joint optimization (i.e., the result of step 205 corresponding to the previous image frame) is the pose of the target object in the local coordinate system (local_slam) corresponding to the point cloud of the previous image frame. This pose is also the target object's pose P2 in the local coordinate system (local_slam) at the time corresponding to the first data point of the current image frame point cloud. Multiply P2 by P1 inversely to obtain the robot's pose change in the local coordinate system (local_slam) during the previous point cloud scanning cycle. Iterate through the time-synchronized point cloud, extracting the relative time of each data point in this frame relative to the last point in the frame. Since the radar scanning cycle is fixed, the weight of each laser scanning data point's corresponding time relative to the entire scanning cycle can be calculated based on its relative time. Then, based on the pose change within the scanning cycle and the time weight of each laser scanning data point, linear interpolate the laser pose change of each laser scanning data point relative to the previous image frame. Based on this change and the laser pose of the previous image frame, obtain the robot's pose P in the local map coordinate system (local_fream) at the corresponding time of each laser scanning data point in this frame. Simultaneously, delete IMU sensor data before the timestamp of this point cloud frame to ensure that joint optimization starts from the current frame. The point cloud is traversed after time synchronization. The pose P3 of each laser scanning data point in the target object coordinate system `tracking_fream` is obtained. The pose P is right-multiplied by P3 to transform the pose of each laser scanning data point in the target object coordinate system `tracking_fream` to the same local coordinate system `local_slam`, thus completing the point cloud distortion removal. Both the local map coordinate system `local_fream` and the global map coordinate system `global_fream` are map coordinate systems with the same origin. The difference lies in the fact that the coordinates in the global map coordinate system have been optimized by the backend.The target object coordinate system, tracking_fream (for example, when the target object is a car, the target object coordinate system is the vehicle coordinate system), is the coordinate system in which the final optimized pose is located. If the radar coordinate system corresponding to the IMU sensor data is different from the target object coordinate system, the radar coordinate system needs to be transformed to the target object coordinate system first. In this scheme, the radar is installed at the center of the target object, so no transformation is required.

[0138] Step S204: Match the timestamp of the IMU sensor data with the image frame of the laser pose data to establish the correspondence between the IMU sensor data and the laser pose data, the correspondence between the IMU sensor data and the image frame, and the correspondence between the image frame and the predicted pose.

[0139] Please see details Figure 1 Step S103 of the illustrated embodiment will not be described again here.

[0140] Step S205: For each current image frame, set corresponding inter-frame constraints to optimize the laser pose data of the current image frame and obtain the optimized pose of the current frame.

[0141] The inter-frame constraint is set based on the IMU sensor data and predicted pose of the previous image frame of the current image frame, as well as the IMU sensor data, laser pose data, and predicted pose of the current image frame.

[0142] Specifically, step S205 includes:

[0143] Step S2051: Set IMU constraints based on the predicted pose and predicted velocity corresponding to the previous image frame, the predicted pose and predicted velocity corresponding to the current image frame, and the changes in predicted pose and predicted velocity between the previous image frame and the current image frame.

[0144] Optionally, inter-frame constraints can be set using a factor graph. The factor graph internally employs a probability-based Bayesian tree, so that each time a constraint is added to the factor graph, the probabilistic relationship between the current node and other nodes is adjusted based on the Bayesian tree's connectivity. The factor graph includes factor nodes and variable nodes. Variable nodes are the nodes to be optimized, and they are optimized by connecting to the factor nodes. In this scheme, factor nodes represent the quantities required to set inter-frame constraints, and variable nodes represent the laser pose data of the current image frame. Optimization of the variable nodes yields the optimized pose of the current frame.

[0145] For example, Figure 7 A schematic diagram illustrating inter-frame constraints related to embodiments of this application is shown. For example... Figure 7As shown, taking a laser pose data frequency of 10Hz and an IMU sensor data frequency of 100Hz as an example, scan1 and scan2 represent two consecutive frames of laser pose data, with 10 IMU sensor data points between adjacent frames. Given the two consecutive frames of laser pose data, the change in laser pose data between the two frames can be calculated. The IMU-predicted change also calculates the change in the target object's pose between these two frames. In an ideal scenario, the two should be completely equal, forming an inter-frame constraint and constructing a nonlinear optimization problem. The purpose of the inter-frame constraint is to ensure that the change between the optimized laser pose data and the previous frame's laser pose data does not deviate from the constraint formed by the IMU-predicted change.

[0146] Optionally, a local factor graph can be constructed using the GTSAM nonlinear optimization library for joint optimization. The GTSAM nonlinear optimization library can find the optimal solution for nonlinear optimization problems with inter-frame constraints.

[0147] The predicted pose and velocity of the previous image frame, the predicted pose and velocity of the current image frame, and the changes in predicted pose and velocity between the previous and current image frames are added to the factor node as IMU constraints.

[0148] Step S2052: Set IMU zero bias constraints based on the zero bias corresponding to the previous image frame, the estimated zero bias corresponding to the current image frame, the measured zero bias corresponding to the current image frame, and the covariance matrix corresponding to the current image frame.

[0149] The zero bias corresponding to the previous image frame is the zero bias after optimization of the previous image frame stored in the IMU pre-integration model; the estimated zero bias corresponding to the current image frame is the zero bias estimated based on the IMU sensor data of the current image frame; the covariance matrix corresponding to the current image frame is calculated by the IMU pre-integration model based on the current IMU sensor data.

[0150] The zero bias corresponding to the previous image frame, the estimated zero bias corresponding to the current image frame, the measured zero bias corresponding to the current image frame, and the covariance matrix corresponding to the current image frame are added to the factor node as IMU zero bias constraints.

[0151] Step S2053: Set pose constraints based on the predicted pose corresponding to the current image frame, the laser pose data corresponding to the current image frame, and the covariance matrix corresponding to the current image frame.

[0152] The predicted pose of the current image frame, the laser pose data of the current image frame, and the covariance matrix of the current image frame are added to the factor node as pose constraints.

[0153] Step S2054: Based on the IMU constraint, IMU zero bias constraint and pose constraint, optimize the laser pose data corresponding to the current image frame to obtain the optimized pose corresponding to the current image frame.

[0154] Add the laser data corresponding to the current image frame to the variable node, and optimize the variable node through each factor node.

[0155] Optionally, based on the IMU constraints, IMU zero-bias constraints, and pose constraints, the estimated zero-bias corresponding to the current image frame is optimized to obtain the zero-bias corresponding to the current image frame. The IMU pre-integration model is then updated using the zero-bias corresponding to the current image frame. The estimated zero-bias corresponding to the current image frame is calculated based on the IMU sensor data acquired in the current image frame. During joint optimization, the estimated zero-bias corresponding to the current image frame is also optimized to obtain the zero-bias corresponding to the current image frame. This zero-bias can be used to update the IMU pre-integration model, enabling pose prediction calculations in the next image frame to be based on this zero-bias. In other words, the zero-bias update is a continuous process. The zero-bias is updated each time during joint optimization, and the updated zero-bias is then used to update the IMU pre-integration model. This ensures that the IMU pre-integration model uses the zero-bias just updated in the previous frame when performing pose prediction calculations, helping to eliminate accumulated errors and improve the accuracy of subsequent map construction.

[0156] Furthermore, after updating the IMU pre-integration model, the predicted pose of the current image frame is updated using the Jacobian matrix in the updated IMU pre-integration model, providing more accurate data for pose prediction calculation in the next image frame. See step S202 for details, which will not be elaborated here.

[0157] Optionally, during the first joint optimization, the GTSAM nonlinear optimization library is initialized using the initial laser pose during static initialization, and the IMU sensor data before the corresponding time of the current image frame is deleted to ensure that the joint optimization is not affected by the previous IMU sensor data.

[0158] For example, when initializing the GTSAM nonlinear optimization library, the ISAM2 (IBM SecurityAccess Manager version 9.0.2) optimizer is first reset, new variable nodes and factor nodes are initialized, and then the IMU data before the corresponding time of the current image frame is deleted from the IMU sensor data queue. Next, a factor graph is constructed, and the ISAM2 optimizer in GTSAM is called to optimize and solve the factor graph. After optimization, the values ​​of the factor nodes and variable nodes saved in the factor graph are set to 0 (cleared). The purpose of this is: 1. To ensure that the next IMU pre-integration starts from the corresponding time of the current image frame, the predicted pose change when entering joint optimization next time is exactly the pose change of the IMU sensor data between the current image frame and the next image frame, unaffected by previous data; 2. To obtain the optimized zero bias, the prior condition for the next pre-integration is to perform the pre-integration calculation based on the optimized zero bias. Finally, the optimized zero bias is obtained and passed to the IMU pre-integration model for reset. During the reset, the optimized zero bias is assigned to the zero bias in the pre-integrator. After the reset, the next IMU pre-integration will be calculated based on this zero bias.

[0159] Optionally, since the front-end is always a process of local optimization and local mapping, the scale of joint optimization will increase with the increase of constraints. Because increasing the optimization scale increases computational cost, and the front-end only needs to find the local optimal estimate without optimizing previous state variables, a sliding window optimization scheme is used to control the scale of joint optimization. Furthermore, since the sliding window optimization scheme needs to handle the marginalization problem well, every certain number of image frames, such as every 100 frames, an marginalization processing flow is entered, and the ISAM2 optimizer is reset to ensure optimization efficiency. Specifically, when the laser pose data of the 100th frame is acquired, edge processing is initiated; the pose, velocity, bias, and covariance matrix corresponding to the previous image frame are retrieved from the optimizer; the optimizer is reset, resetting the entire optimization problem; the latest pose, velocity, bias, and covariance matrix are added to the factor nodes in the factor graph, and the variables to be optimized (pose, velocity, and bias) are added to the variable nodes in the factor graph; the constructed factor graph is placed into the optimizer for optimization, and after optimization, the factor nodes and variable nodes are cleared to zero for use in the joint optimization of the next image frame; the counter is reset to 1 and counting starts again.

[0160] For example, Figure 8 A flowchart illustrating the joint optimization involved in an embodiment of this application is shown.

[0161] Step S206: Construct a global map based on the optimized poses corresponding to each image frame.

[0162] After obtaining the optimized poses corresponding to each image frame, the front end can construct a local sub-map based on the optimized poses of each image frame and input it to the back end. After accumulating a certain number of frames, the back end performs global optimization to construct a global map.

[0163] The map construction method provided in this embodiment matches IMU sensor data with laser pose data and sets inter-frame constraints based on the IMU sensor data and laser pose data, thus tightly coupling the IMU sensor data and laser pose data. This makes data fusion of IMU sensor data and laser pose data easier and can solve the problem of degradation scenarios. Pose prediction calculation is performed on the IMU sensor data for each timestamp, which reduces the impact of noise and drift, reduces cumulative error, improves positioning accuracy, and thus improves the accuracy of map construction.

[0164] Furthermore, by updating the zero bias in the joint optimization and updating the IMU pre-integration model based on the updated zero bias, the IMU pre-integration model can be adjusted in a timely manner, thus improving prediction accuracy.

[0165] Furthermore, combining the predicted pose with point cloud distortion correction reduces ghosting in the mapping and improves the accuracy of loop closure detection in the backend.

[0166] Furthermore, by utilizing IMU sensor data, the initial posture of the robot was estimated, thus improving the robustness of the system.

[0167] Furthermore, updating the predicted pose after zero-bias optimization using the Jacobian matrix reduces computational load and time, thus improving the real-time performance of map construction.

[0168] As one or more specific application embodiments of the present invention, the advantages of the solution of this application compared with the solutions in related technologies are described below in conjunction with specific application scenarios.

[0169] This embodiment uses a robot equipped with an IMU and a LiDAR to collect a dataset. First, the collected dataset is statically initialized. The LiDAR sensor's acquisition frequency is 10Hz, and the IMU sensor's acquisition frequency is 100Hz. The static initialization results are as follows:

[0170] local_trajectory_builder_2d.cc:237]P:0,0,0

[0171] local_trajectory_builder_2d.ce:238]V:0,0,0

[0172] local_trajectory_builder_2d.cc:239]Q:0.999964,-3.79094e-05,0.00844484,0

[0173] local_trajectory_builder_2d.cc:240]Ba:-0.00161761,-7.26155e-06,0.0957649

[0174] [local_trajectory_builder_2d.cc:241]Bg:-9.42143e-06,2.92026e-05,1.31041e-05

[0175] [local_trajectory_builder_2d.cc:564]name.

[0176] Since the dataset is recorded by hand-held backpack, it has a certain attitude and zero bias at the initial moment. However, the solutions in related technologies assume that the initial attitude is on the horizontal plane, which will cause subsequent cumulative errors, delays, etc., and has poor robustness.

[0177] Figure 9 The accompanying diagrams show a comparison of the mapping effects described in the embodiments of this application. For example... Figure 9 As shown, the left side shows the mapping effect of the proposed solution, and the right side shows the mapping effect of the solution in the related technology. It can be seen that the proposed solution can provide an accurate initial value when constructing a local subgraph, thus solving the mapping problem in scene degradation.

[0178] Next, the robustness of robot localization during movement is used to measure localization performance. The high-precision map used for localization consists of 11 local submaps. The 9th submap, which has the worst localization performance, is selected to evaluate the overall localization performance of the solution. The evaluation is based on two main aspects: first, the loop closure detection score between the 9th submap and other nodes; and second, the pose difference of the 9th submap in the global coordinate system between map trajectory 0 and localization trajectory 1 after global optimization in the backend. The accuracy of loop closure constraints found by the proposed solution in localization mode is higher than that of the cartographer in related technologies. Figure 10 This diagram illustrates a local subgraph node in the related art. Figure 11 A schematic diagram of a local subgraph node involved in an embodiment of this application is shown. Figure 10 and Figure 11 In the map trajectory 0 and positioning trajectory 1, the node coordinates of the 9th local sub-map are (0,9) and (1,9) respectively. Ideally, these two node coordinates should coincide in the global coordinate system. Figure 10 The coordinates of these two nodes are clearly not aligned, mainly due to the accuracy of loop closure detection and accumulated mileage errors at the front end. Figure 11 As can be seen from the data, the proposed solution can improve the overall positioning performance under the same data conditions.

[0179] This embodiment also provides a map building apparatus for implementing the above embodiments and preferred embodiments; details already described will not be repeated. As used below, the term "module" can refer to a combination of software and / or hardware that performs a predetermined function. Although the apparatus described in the following embodiments is preferably implemented in software, hardware implementation, or a combination of software and hardware, is also possible and contemplated.

[0180] This embodiment provides a map building device, such as... Figure 12 As shown, it includes:

[0181] The data acquisition module 1201 is used to acquire IMU sensor data at each time stamp and laser pose data of each acquired image frame;

[0182] The pose prediction module 1202 is used to perform pose prediction calculations on the IMU sensor data for each timestamp to obtain the predicted pose for the corresponding timestamp.

[0183] The matching module 1203 is used to match the timestamp of the IMU sensor data with the image frame of the laser pose data to establish the correspondence between the IMU sensor data and the laser pose data, the correspondence between the IMU sensor data and the image frame, and the correspondence between the image frame and the predicted pose.

[0184] The inter-frame constraint module 1204 is used to set corresponding inter-frame constraint conditions for each current image frame in order to optimize the laser pose data of the current image frame and obtain the optimized pose of the current frame. The inter-frame constraint conditions are set according to the IMU sensor data and predicted pose of the previous image frame of the current image frame, as well as the IMU sensor data, laser pose data and predicted pose of the current image frame.

[0185] The map building module 1205 is used to build a global map based on the optimized poses corresponding to each image frame.

[0186] In one optional implementation, pose prediction calculation is performed on the IMU sensor data for each timestamp to obtain the predicted pose for the corresponding timestamp, including:

[0187] Obtain the IMU pre-integration model; this IMU pre-integration model is a pre-established model used to predict the pose change between two timestamps.

[0188] For each timestamp, the IMU sensor data of the current timestamp is input into the IMU pre-integration model to obtain the predicted pose change from the previous timestamp to the current timestamp;

[0189] The predicted pose of the current timestamp is calculated based on the predicted pose of the previous timestamp and the change in predicted pose from the previous timestamp to the current timestamp.

[0190] In one optional implementation, while inputting the IMU sensor data of the current timestamp into the IMU pre-integration model to obtain the predicted pose change from the previous timestamp to the current timestamp, the predicted velocity change from the previous timestamp to the current timestamp is also obtained.

[0191] The predicted pose at the current timestamp is calculated based on the predicted pose at the previous timestamp and the change in predicted pose from the previous timestamp to the current timestamp, including:

[0192] Multiply the predicted pose of the previous timestamp by the change in the predicted pose to obtain the predicted pose of the current timestamp.

[0193] The velocity prediction module is used to multiply the predicted velocity of the previous timestamp by the change in the predicted velocity to obtain the predicted velocity of the current timestamp.

[0194] In one optional implementation, for each current image frame, corresponding inter-frame constraints are set to optimize the laser pose data of the current image frame and obtain the optimized pose of the current frame, including:

[0195] IMU constraints are set based on the predicted pose and velocity of the previous image frame, the predicted pose and velocity of the current image frame, and the changes in predicted pose and velocity between the previous and current image frames.

[0196] Based on the zero bias corresponding to the previous image frame, the estimated zero bias corresponding to the current image frame, the measured zero bias corresponding to the current image frame, and the covariance matrix corresponding to the current image frame, IMU zero bias constraints are set; the zero bias corresponding to the previous image frame is the zero bias after optimization of the previous image frame stored in the IMU pre-integration model; the estimated zero bias corresponding to the current image frame is the zero bias estimated based on the IMU sensor data of the current image frame; the covariance matrix corresponding to the current image frame is calculated by the IMU pre-integration model based on the current IMU sensor data.

[0197] Set pose constraints based on the predicted pose corresponding to the current image frame, the laser pose data corresponding to the current image frame, and the covariance matrix corresponding to the current image frame.

[0198] Based on the IMU constraint, the IMU zero bias constraint, and the pose constraint, the laser pose data corresponding to the current image frame is optimized to obtain the optimized pose corresponding to the current image frame.

[0199] In one optional implementation, based on the IMU constraint, the IMU zero bias constraint and the pose constraint, the estimated zero bias corresponding to the current image frame is optimized to obtain the zero bias corresponding to the current image frame.

[0200] The IMU pre-integration model is updated using the zero bias corresponding to the current image frame.

[0201] In one optional implementation, after matching the timestamp of the IMU sensor data with the image frame of the laser pose data, for each image frame, the pose change between the previous image frame and the current image frame is calculated based on the predicted pose corresponding to the current image frame and the optimized pose corresponding to the previous image frame.

[0202] Based on the pose change between the previous image frame and the current image frame, point cloud distortion correction is performed on the laser pose data corresponding to the current image frame.

[0203] In one optional implementation, the point cloud distortion correction of the corresponding laser pose data of the current image frame based on the pose change between the previous image frame and the current image frame includes:

[0204] Traverse the laser pose data of the current image frame to obtain the interval time between each laser scan data point in the laser pose data of the current image frame and the last laser scan data point in the laser pose data of the current image frame.

[0205] Based on the pose change between the previous image frame and the current image frame and the time interval, the pose change of each laser scanning data point in the laser pose data corresponding to the current image frame relative to the optimized pose corresponding to the previous image frame is linearly interpolated.

[0206] Based on the pose change of each laser scanning data point relative to the optimized pose of the previous image frame, the laser pose of each laser scanning data point in the laser coordinate system is transformed to the global map coordinate system in order to perform point cloud distortion correction.

[0207] Further functional descriptions of the above modules and units are the same as those in the corresponding embodiments described above, and will not be repeated here.

[0208] In this embodiment, the map building device is presented in the form of a functional unit. Here, a unit refers to an ASIC (Application Specific Integrated Circuit) circuit, a processor and memory that execute one or more software or fixed programs, and / or other devices that can provide the above functions.

[0209] This invention also provides a computer device having the above-described features. Figure 12 The map building device shown.

[0210] Please see Figure 13 , Figure 13 This is a schematic diagram of the structure of a computer device provided in an optional embodiment of the present invention, such as... Figure 13 As shown, the computer device includes one or more processors 10, memory 20, and interfaces for connecting the components, including high-speed interfaces and low-speed interfaces. The components communicate with each other via different buses and can be mounted on a common motherboard or otherwise installed as needed. The processors can process instructions executed within the computer device, including instructions stored in or on memory to display graphical information of a GUI on external input / output devices (such as display devices coupled to the interfaces). In some alternative implementations, multiple processors and / or multiple buses can be used with multiple memories and multiple memory modules, if desired. Similarly, multiple computer devices can be connected, each providing some of the necessary operations (e.g., as a server array, a group of blade servers, or a multiprocessor system). Figure 13 Take a processor 10 as an example.

[0211] Processor 10 may be a central processing unit, a network processor, or a combination thereof. Processor 10 may further include a hardware chip. The hardware chip may be an application-specific integrated circuit (ASIC), a programmable logic device (PLD), or a combination thereof. The programmable logic device may be a complex programmable logic device (CAMP), a field-programmable gate array (FPGA), a general-purpose array logic (GDA), or any combination thereof.

[0212] The memory 20 stores instructions executable by at least one processor 10 to cause the at least one processor 10 to perform the method shown in the above embodiments.

[0213] The memory 20 may include a program storage area and a data storage area. The program storage area may store the operating system and applications required for at least one function; the data storage area may store data created based on the use of the computer device. Furthermore, the memory 20 may include high-speed random access memory and may also include non-transitory memory, such as at least one disk storage device, flash memory device, or other non-transitory solid-state storage device. In some alternative embodiments, the memory 20 may optionally include memory remotely located relative to the processor 10, and these remote memories may be connected to the computer device via a network. Examples of such networks include, but are not limited to, the Internet, intranets, local area networks, mobile communication networks, and combinations thereof.

[0214] The memory 20 may include volatile memory, such as random access memory; the memory may also include non-volatile memory, such as flash memory, hard disk or solid-state drive; the memory 20 may also include a combination of the above types of memory.

[0215] The computer device also includes an input device 30 and an output device 40. The processor 10, memory 20, input device 30, and output device 40 can be connected via a bus or other means. Figure 13 Taking the example of a connection between China and Israel via a bus.

[0216] Input device 30 can receive input numerical or character information, and generate key signal inputs related to user settings and function control of the computer device, such as a touchscreen, keypad, mouse, trackpad, touchpad, joystick, one or more mouse buttons, trackball, joystick, etc. Output device 40 may include display devices, auxiliary lighting devices (e.g., LEDs), and haptic feedback devices (e.g., vibration motors). The aforementioned display devices include, but are not limited to, liquid crystal displays, light-emitting diodes, displays, and plasma displays. In some alternative embodiments, the display device may be a touchscreen.

[0217] This invention also provides a computer-readable storage medium. The methods described above according to embodiments of the invention can be implemented in hardware or firmware, or implemented as computer code that can be recorded on a storage medium, or implemented as computer code downloaded via a network and originally stored on a remote storage medium or a non-transitory machine-readable storage medium and then stored on a local storage medium. Thus, the methods described herein can be processed by software stored on a storage medium using a general-purpose computer, a dedicated processor, or programmable or dedicated hardware. The storage medium can be a magnetic disk, optical disk, read-only memory, random access memory, flash memory, hard disk, or solid-state drive, etc.; further, the storage medium can also include combinations of the above types of memory. It is understood that computers, processors, microprocessor controllers, or programmable hardware include storage components capable of storing or receiving software or computer code, which, when accessed and executed by the computer, processor, or hardware, implements the methods shown in the above embodiments.

[0218] Although embodiments of the invention have been described in conjunction with the accompanying drawings, those skilled in the art can make various modifications and variations without departing from the spirit and scope of the invention, and such modifications and variations all fall within the scope defined by the appended claims.

Claims

1. A map construction method, characterized in that, The method includes: Acquire IMU sensor data at each timestamp and laser pose data for each image frame acquired; Perform pose prediction calculations on the IMU sensor data for each timestamp to obtain the predicted pose for the corresponding timestamp; The timestamps of the IMU sensor data are matched with the image frames of the laser pose data to establish the correspondence between the IMU sensor data and the laser pose data, the correspondence between the IMU sensor data and the image frames, and the correspondence between the image frames and the predicted pose. For each current image frame, corresponding inter-frame constraints are set to optimize the laser pose data of the current image frame and obtain the optimized pose of the current frame. The inter-frame constraints are set based on the IMU sensor data and predicted pose of the previous image frame of the current image frame, as well as the IMU sensor data, laser pose data and predicted pose of the current image frame. A global map is constructed based on the optimized poses corresponding to each image frame.

2. The method according to claim 1, characterized in that, Perform pose prediction calculations on the IMU sensor data for each timestamp to obtain the predicted pose for the corresponding timestamp, including: Obtain the IMU pre-integration model; the IMU pre-integration model is a pre-established model used to predict the pose change between two timestamps; For each timestamp, the IMU sensor data of the current timestamp is input into the IMU pre-integration model to obtain the predicted pose change from the previous timestamp to the current timestamp; The predicted pose of the current timestamp is calculated based on the predicted pose of the previous timestamp and the change in predicted pose from the previous timestamp to the current timestamp.

3. The method according to claim 2, characterized in that, When the IMU sensor data at the current time stamp is input into the IMU pre-integration model to obtain the predicted pose change from the previous time stamp to the current time stamp, the predicted velocity change from the previous time stamp to the current time stamp is also obtained. The step of calculating the predicted pose at the current time point based on the predicted pose at the previous time point and the change in predicted pose from the previous time point to the current time point includes: Multiply the predicted pose of the previous timestamp by the change in the predicted pose to obtain the predicted pose of the current timestamp. The method further includes: Multiply the predicted velocity of the previous timestamp by the change in the predicted velocity to obtain the predicted velocity of the current timestamp.

4. The method according to claim 3, characterized in that, For each current image frame, corresponding inter-frame constraints are set to optimize the laser pose data of the current image frame, resulting in the optimized pose of the current frame, including: IMU constraints are set based on the predicted pose and velocity of the previous image frame, the predicted pose and velocity of the current image frame, and the changes in predicted pose and velocity between the previous and current image frames. IMU zero-bias constraints are set based on the zero-bias of the previous image frame, the estimated zero-bias of the current image frame, the measured zero-bias of the current image frame, and the covariance matrix of the current image frame. The zero-bias of the previous image frame is the zero-bias after optimization of the previous image frame stored in the IMU pre-integration model. The estimated zero-bias of the current image frame is the zero-bias estimated based on the IMU sensor data of the current image frame. The covariance matrix of the current image frame is calculated by the IMU pre-integration model based on the current IMU sensor data. Set pose constraints based on the predicted pose corresponding to the current image frame, the laser pose data corresponding to the current image frame, and the covariance matrix corresponding to the current image frame. Based on the IMU constraints, IMU zero bias constraints, and pose constraints, the laser pose data corresponding to the current image frame is optimized to obtain the optimized pose corresponding to the current image frame.

5. The method according to claim 4, characterized in that, The method further includes: Based on the IMU constraints, IMU zero bias constraints and pose constraints, the estimated zero bias corresponding to the current image frame is optimized to obtain the zero bias corresponding to the current image frame. The IMU pre-integration model is updated using the zero bias corresponding to the current image frame.

6. The method according to claim 1, characterized in that, After matching the timestamp of the IMU sensor data with the image frame of the laser pose data, the method further includes: For each image frame, the pose change between the previous image frame and the current image frame is calculated based on the predicted pose corresponding to the current image frame and the optimized pose corresponding to the previous image frame. Based on the pose change between the previous image frame and the current image frame, point cloud distortion correction is performed on the laser pose data corresponding to the current image frame.

7. The method according to claim 6, characterized in that, The step of performing point cloud distortion correction on the laser pose data corresponding to the current image frame based on the pose change between the previous image frame and the current image frame includes: Traverse the laser pose data of the current image frame to obtain the interval time between each laser scan data point in the laser pose data of the current image frame and the last laser scan data point in the laser pose data of the current image frame. Based on the pose change between the previous image frame and the current image frame and the interval time, the pose change of each laser scanning data point in the laser pose data corresponding to the current image frame relative to the optimized pose corresponding to the previous image frame is linearly interpolated. Based on the pose change of each laser scanning data point relative to the optimized pose of the previous image frame, the laser pose of each laser scanning data point in the laser coordinate system is transformed to the global map coordinate system in order to perform point cloud distortion correction.

8. A map building device, characterized in that, The device includes: The data acquisition module is used to acquire IMU sensor data at each time stamp and laser pose data for each image frame acquired. The pose prediction module is used to perform pose prediction calculations on the IMU sensor data at each timestamp to obtain the predicted pose for the corresponding timestamp. The matching module is used to match the timestamp of the IMU sensor data with the image frame of the laser pose data to establish the correspondence between the IMU sensor data and the laser pose data, the correspondence between the IMU sensor data and the image frame, and the correspondence between the image frame and the predicted pose. The inter-frame constraint module is used to set corresponding inter-frame constraint conditions for each current image frame in order to optimize the laser pose data of the current image frame and obtain the optimized pose of the current frame. The inter-frame constraint conditions are set according to the IMU sensor data and predicted pose of the previous image frame of the current image frame, as well as the IMU sensor data, laser pose data and predicted pose of the current image frame. The map building module is used to construct a global map based on the optimized poses corresponding to each image frame.

9. A computer device, characterized in that, include: A memory and a processor are communicatively connected, the memory stores computer instructions, and the processor executes the map construction method according to any one of claims 1 to 7 by executing the computer instructions.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores computer instructions for causing a computer to perform the map construction method according to any one of claims 1 to 7.

Images