Mapping method and device based on fusion of laser radar and inertial measurement unit

Abstract

The invention relates to the technical field of map construction, and provides a mapping method and device based on fusion of a laser radar and an inertial measurement unit. According to the scheme, motion distortion compensation is carried out on original point cloud data through motion estimation based on inertial data, the compensated point cloud is matched with a local map, and the local map is obtained through a filter; carrying out tight coupling fusion on the inertial data and the point cloud matching result, and outputting the pose information of the sensor at the current moment; according to the pose information of the sensor, the compensated point cloud is added into the dynamically managed local map, and the updated local map is periodically published, so that a complete mapping function from data acquisition, processing and fusion to map construction is realized. And through motion distortion compensation, the inertial data and the point cloud matching result are tightly coupled and fused, and the updated local map is periodically published, so that high mapping precision can still be kept when the mapping area is large.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of map construction, and in particular to a mapping method and device based on fusion of a laser radar and an inertial measurement unit. BACKGROUND

[0002] Simultaneous Localization and Mapping (SLAM) technology is the core basis for robots, autonomous vehicles and augmented reality devices to realize autonomous navigation and interaction in unknown environments. This technology aims to perceive the surrounding environment in real time through the sensors carried, while estimating the pose of itself and constructing an environment map. According to the difference of the main sensors, SLAM technology is mainly divided into two categories: visual SLAM and laser SLAM. Laser SLAM has become the preferred solution in many applications with extremely high reliability requirements due to its high ranging accuracy and insensitivity to light changes.

[0003] In the field of laser SLAM, the tightly coupled fusion method based on laser radar and inertial measurement unit (IMU) has become the mainstream technology route to improve system accuracy and robustness. Among them, the open source algorithm framework Fast-LIO2 effectively fuses laser radar feature points and inertial data through a tightly coupled iterated Kalman filter (IEKF) structure, so that it can realize robust odometer estimation in complex scenes with fast motion, noise interference or environment feature degradation. However, the existing fusion of laser radar and IMU data is suitable for small robots, and the mapping accuracy is low when the mapping area is large. SUMMARY

[0004] The embodiment of the present application provides a mapping method and device based on fusion of a laser radar and an inertial measurement unit, which can maintain high mapping accuracy when the mapping area is large during the driving process of a robot or an autonomous vehicle.

[0005] The embodiment of the present application provides a mapping method and device based on fusion of a laser radar and an inertial measurement unit, which can maintain high mapping accuracy when the mapping area is large during the driving process of a robot or an autonomous vehicle. synchronously acquiring measurement data of a plurality of sensors, and pre-processing the measurement data; the plurality of sensors at least include a laser radar and an inertial measurement unit, and the measurement data of the plurality of sensors at least include original point cloud data of the laser radar and inertial data of the inertial measurement unit; based on the inertial data, performing motion distortion compensation on the original point cloud data through motion estimation to obtain compensated point cloud; The compensated point cloud is matched with a local map, and the inertial data and the point cloud matching result are tightly coupled and fused through a filter to output pose information of the sensor at the current time; According to the pose information of the sensor, the compensated point cloud is added to the dynamically managed local map, and an updated local map is periodically published.

[0006] In an implementation manner, the method further includes: in a case where the positioning instruction is received, obtaining identity information of the user; Based on the identity information of the user, a storage path of a pre-constructed global point cloud map is dynamically obtained, and the global point cloud map is loaded; The compensated point cloud obtained in real time is matched with the global point cloud map, and global pose information in the global point cloud map is output, the global pose information being used to represent a position of the device in the global point cloud map.

[0007] In an implementation manner, the pre-processing of the measurement data includes: The original point cloud data is parsed, and the parsed original point cloud data is converted into standardized point cloud data containing three-dimensional coordinates, intensity information, a scan line number and a timestamp offset.

[0008] In an implementation manner, after the step of compensating the original point cloud data for motion distortion based on the inertial data through motion estimation to obtain a compensated point cloud, the method further includes: Based on the scan line number, a number of point clouds corresponding to the scan line number is calculated, and a curvature feature of a feature point in the point cloud data is calculated, wherein the curvature feature is determined by calculating an Euclidean distance between the feature point and a previous adjacent point of the feature point on the same scan line; In a case where the number of point clouds is less than a preset number, the point cloud data corresponding to the scan line number is removed.

[0009] In an implementation manner, in the step of compensating the original point cloud data for motion distortion based on the inertial data through motion estimation, the method further includes: The timestamp offset T and the enhanced inertial data are used to accurately calculate a pose of the sensor at a sampling time of each feature point; According to the pose of the sensor at the sampling time of each feature point, a continuous motion trajectory of the sensor from a starting sampling time to an ending sampling time of a current frame of point cloud is calculated; Based on the continuous motion trajectory, an accurate sampling time of the feature point is determined, and a sensor pose transformation from the accurate sampling time to the ending sampling time is calculated; An inverse transformation of the calculated pose transformation is applied to the feature points, mapping the feature point coordinates into the sensor coordinate system at the end acquisition time.

[0010] In an implementation, in the step of calculating the continuous motion trajectory of the sensor from the start acquisition time to the end acquisition time of the current frame point cloud according to the pose of the sensor at the sampling time of each feature point, the step comprises: The angular velocity and acceleration data in the inertial data are integrated using a median integration method to reconstruct the continuous motion trajectory.

[0011] In an implementation, the local map is managed based on a dynamic local map management mechanism based on an incremental KD tree and a sliding window mechanism, and specifically comprises: The distance between the current position of the laser radar and the boundary of the current local map is monitored in real time. When the distance is less than a preset threshold, a map boundary migration is triggered, new observation point clouds are dynamically added and redundant point clouds beyond the field of view are removed to generate a new local map.

[0012] In an implementation, in the step of performing state estimation by tightly coupling and fusing the inertial data and the point cloud matching result through a filter to output the pose information at the current time, the step further comprises: The extrinsic parameters between the laser radar and the inertial measurement unit are estimated online. The timestamp offset between the laser radar and the inertial measurement unit is compensated and calibrated.

[0013] In an implementation, the period of publishing the local map data is configurable.

[0014] In an implementation, in the case of receiving a positioning instruction, the step further comprises: obtaining identity information of a current operating system, and dynamically generating and obtaining a storage path of the global point cloud map based on the identity information.

[0015] Embodiments of the present application also provide a mapping device based on fusion of a laser radar and an inertial measurement unit, comprising: an acquisition module, a compensation module, a local map matching module, and a local map updating module.

[0016] The acquisition module synchronously acquires measurement data of multiple sensors, and pre-processes the measurement data; the multiple sensors at least include a laser radar and an inertial measurement unit, and the measurement data of the multiple sensors at least include raw point cloud data of the laser radar and inertial data of the inertial measurement unit.

[0017] The compensation module compensates motion distortion of the raw point cloud data based on the inertial data through motion estimation to obtain compensated point clouds.

[0018] The local map matching module matches the compensated point cloud with a local map, and tightly couples and fuses the inertial data and the point cloud matching result through a filter to output the pose information of the sensor at the current time.

[0019] The local map updating module adds the compensated point cloud into the dynamically managed local map according to the pose information of the sensor, and periodically publishes the updated local map.

[0020] In an implementation manner, the mapping device based on fusion of a laser radar and an inertial measurement unit provided by the embodiment of the application further includes a map loading module and a pose output module.

[0021] The acquisition module is further configured to acquire identity information of the user in a case where the positioning instruction is received.

[0022] The map loading module is configured to dynamically acquire a storage path of a pre-constructed global point cloud map based on the identity information of the user, and load the global point cloud map.

[0023] The pose output module is configured to match the compensated point cloud acquired in real time with the global point cloud map, and output global pose information in the global point cloud map, the global pose information being used to represent a position of the device in the global point cloud map.

[0024] The technical solution provided by the embodiment of the application is based on inertial data, and performs motion distortion compensation on original point cloud data through motion estimation, matches the compensated point cloud with a local map, and tightly couples and fuses the inertial data and the point cloud matching result through a filter to output the pose information of the sensor at the current time; the compensated point cloud is added into the dynamically managed local map according to the pose information of the sensor, and the updated local map is periodically published, so that complete mapping functions from data acquisition, processing, fusion to map construction are realized. Through motion distortion compensation, tightly coupling and fusing the inertial data and the point cloud matching result, and periodically publishing the updated local map, high mapping precision can be maintained when the mapping area is large. BRIEF DESCRIPTION OF DRAWINGS

[0025] In order to more clearly illustrate the technical solutions of the present application, the drawings needed in the embodiments will be briefly introduced as follows. Obviously, for those skilled in the art, other drawings can also be obtained without creative labor based on these drawings.

[0026] Figure 1 Flowchart of a mapping method based on fusion of a laser radar and an inertial measurement unit provided by the embodiment of the application Figure 1 Figure 2 ​A flowchart of a mapping method based on fusion of laser radar and inertial measurement unit provided by an embodiment of the present application Figure 2 ; Figure 3 A flowchart of a mapping method based on fusion of laser radar and inertial measurement unit provided by an embodiment of the present application Figure 3 ; Figure 4 A structural diagram of a mapping device based on fusion of laser radar and inertial measurement unit provided by an embodiment of the present application. DETAILED DESCRIPTION

[0027] The embodiments will be described in detail below with reference to the drawings. The following description is, unless otherwise indicated, related to the accompanying drawings, wherein the same numbers refer to the same or similar components throughout several views. The embodiments described in the following description are not meant to represent all embodiments consistent with the present application. Rather, they are merely examples of systems and methods consistent with some aspects of the present application as detailed in the appended claims.

[0028] It should be noted that the brief description of terms in the present application is only for the convenience of understanding the following described embodiments, and is not intended to limit the embodiments of the present application. Unless otherwise specified, these terms should be understood according to their ordinary and general meanings.

[0029] The terms first, second, third, etc. in the specification and claims of the present application and the above-described drawings are used to distinguish similar or similar objects or entities, and do not necessarily mean a specific order or sequence, unless otherwise noted. It should be understood that the terms used in this way can be interchanged under appropriate circumstances.

[0030] The term includes and has and any variation thereof is intended to cover but not exclusive of, for example, a product or device including a series of components need not be limited to all components clearly listed, but can include other components not clearly listed or inherent to these products or devices.

[0031] The term module refers to any known or later developed hardware, software, firmware, artificial intelligence, fuzzy logic or combination of hardware or / and software code capable of performing functions related to the element.

[0032] The scheme provided in the application is applied to the fields of automatic driving, mobile robots and augmented reality, and needs to realize high-precision real-time positioning and map construction in complex indoor and outdoor environments, for example, places such as industrial parks, campuses, communities and the like containing indoor and outdoor environments. The existing laser SLAM scheme (such as Fast-LIO2) has deficiencies in sensor adaptation, data fusion efficiency, map management and functional integrity. In order to solve the above technical problems, the embodiment of the application provides a mapping method based on fusion of a laser radar and an inertial measurement unit, as shown in Figure 1 Steps S110 to S140.

[0033] In step S110, the measurement data of a plurality of sensors is synchronously acquired, and the measurement data is preprocessed.

[0034] The plurality of sensors at least includes a laser radar and an inertial measurement unit, and the measurement data of the plurality of sensors at least includes raw point cloud data of the laser radar and inertial data of the inertial measurement unit.

[0035] The simultaneous localization and mapping method is realized through a robot operating system (ROS) framework. First, the raw point cloud data corresponding to the topic of the laser radar and the inertial data corresponding to the topic of the inertial measurement unit are synchronously subscribed through a ROS node, so as to acquire the raw point cloud data and the inertial data, wherein the inertial data includes angular velocity and acceleration.

[0036] Then, the measurement data is preprocessed. For example, in the use process of a mobile device, different brands of multi-line laser radars may be used or replaced. In order to realize unified and standardized analysis of data of different brands of laser radars and improve universality, a laser radar data parser is set up in the preprocessing node. The parser deletes the identification definition of a specific radar model in the original code, and writes corresponding analysis drivers for different radars based on the laser radar data transmission mechanism.

[0037] After analysis, the basic information of each point is extracted and uniformly packaged into a custom XYZIRT format point cloud. Ring is the scan line number, and Time is the timestamp offset relative to the start time of the frame. Finally, the custom point cloud is converted into a ROS standard sensor_msgs::PointCloud2 message using the pcl::toROSMsg function for publishing. In this way, once development is realized, a plurality of radars are adapted, and the generality of the algorithm is improved.

[0038] In step S120, the raw point cloud data is compensated for motion distortion through motion estimation based on the inertial data, to obtain compensated point cloud.

[0039] Motion distortion compensation is to reconstruct the continuous motion trajectory of the sensor based on the inertial data, and add median integral method (using the average of adjacent IMU frame measurement values for integration) and forward propagation algorithm. Specifically, as shown in FIG. 10, it includes steps S210 to S240. Figure 2

[0040] In step S210, the time stamp offset T of each feature point and the enhanced inertial data are used to accurately calculate the pose of the sensor at the sampling time of each feature point.

[0041] The radar data is time-stamped based on the system time, for example, in the embodiment of the application, the time stamp of the inertial unit is used as the reference. In this way, the radar data is time-stamped based on the time of the inertial data.

[0042] That is, the data of the laser radar is time-stamped based on the time system of the inertial data (time stamp offset compensation). The time stamp offset T of each laser point and the enhanced inertial data are used to accurately interpolate and calculate the sensor pose at the sampling time of each laser point .

[0043] Then, the continuous motion trajectory of the sensor from the start time to the end time of the current frame is calculated . For the point collected at time , based on the continuous motion trajectory, the pose at the collection time and the target pose at the end time of the frame are obtained. The relative transformation is calculated: Finally, the inverse transformation of the transformation is applied to the point , and the non-distorted coordinates of the point in the coordinate system at time are obtained: .

[0044] That is, for each feature point, based on the time stamp offset T, the sensor pose transformation from the collection time to the end time of the frame is calculated , and the inverse transformation thereof is applied to compensate the point cloud coordinates to the coordinate system at time , and the compensated point cloud is obtained.

[0045] The time stamp offset T of each feature point (laser point) and the enhanced inertial data by the median integral algorithm and the forward propagation algorithm are used to accurately interpolate and calculate the sensor pose at the sampling time of each feature point ​​sensor pose .

[0046] where, for the inertial data (angular velocity ω, acceleration a) of adjacent IMU frames, the median method formula is as follows. The subscript head is the head value, the subscript tail is the tail value, and the subscript avr is the median value. Then, ; .

[0047] The forward propagation algorithm (EKF prediction) is a process of predicting the state at the current time from the state at the last time using the inertial data, which is a prediction step in the iterative extended Kalman filter (IEKF). The state is predicted by the inertial data, and the sensor pose and velocity are updated. is the gyro bias, is the accelerometer bias, is the gravity vector, is the rotation matrix of the sensor, is the velocity information of the sensor, is the position information of the sensor.

[0048] Then the rotation update is: .

[0049] The velocity update is: .

[0050] The position update is: .

[0051] Backward compensation (frame processing in the opposite direction of motion), according to the time difference between the point cloud timestamp and the IMU frame, the motion distortion is compensated by backward propagation.

[0052] Conversion of acquisition points to IMU coordinate system: .

[0053] Position of acquisition points in the world coordinate system: .

[0054] Backward compensation to the current frame: .

[0055] The subscript L is the abbreviation of , which refers to the LiDAR coordinate system, is the transformation matrix of the coordinate point from the LiDAR coordinate system to the IMU coordinate system, is the IMU rotation matrix at the point acquisition time, is the IMU world position at the point acquisition time, and are used to represent the IMU state at the end of the current frame.

[0056] Step S220, according to the pose of each feature point at the sampling time of the sensor, the continuous motion trajectory of the sensor from the start sampling time to the end sampling time of the current frame point cloud is calculated.

[0057] The system continuously caches the inertial data stream of the inertial measurement unit. For the precise time stamp The laser points collected in step one offset compensation Need to know The sensor pose at the time (reference to the IMU coordinate system) That is, the transformation from the world coordinate system to the IMU coordinate system). The angular velocity ω and acceleration a of the IMU are pre-integrated using the median integral method to reconstruct a high-frequency, continuous IMU motion trajectory state sequence.

[0058] It should be noted that the continuous motion trajectory of the sensor is not a pre-calculated complete function. The continuous motion trajectory is physically embodied in the system as an IMU state buffer sorted by time stamp. The pose at any time is obtained by interpolation on the buffer. The IMU state sequence is formed by a series of discrete pose nodes on the time axis , wherein And Covering the start time to the end time of the current frame point cloud.

[0059] Wherein, the continuous motion trajectory means that through these discrete nodes, the pose at any time to can be continuously and accurately restored by using interpolation algorithms (such as linear interpolation, spherical linear interpolation). ).

[0060] Step S230, based on the continuous motion trajectory, the precise sampling time of the feature point is determined, and the sensor pose transformation from the precise sampling time to the end sampling time is calculated.

[0061] For example, the first sensor pose at the precise sampling time and the second sensor pose at the end sampling time are obtained; by multiplying the inverse matrix of the first sensor pose with the second sensor pose, the pose transformation matrix from the precise sampling time to the end sampling time is obtained. Based on the continuous motion trajectory, the first sensor pose corresponding to the precise sampling time and the second sensor pose corresponding to the end sampling time are queried and obtained; the relative pose transformation of the sensor from the precise sampling time to the end sampling time is calculated by using the first sensor pose and the second sensor pose.

[0062] ​For example, for a point , its pose at the acquisition time and the pose at the target time (end of frame time are known . The relative pose transformation from the IMU coordinate system at time to the IMU coordinate system at time is calculated . The relative pose transformation is as follows: .

[0063] where is the inverse matrix of the pose at time , indicating the transformation from the world coordinate system back to the IMU coordinate system at time . The physical meaning of the relative pose transformation is: first transform the point from the IMU coordinate system at time to the world coordinate system, and then transform from the world coordinate system to the IMU coordinate system at time , which is equivalent to directly performing coordinate system transformation from to .

[0064] In step S240, the inverse transformation of the calculated pose transformation is applied to the feature points, and the feature point coordinates are mapped to the sensor coordinate system at the end of the acquisition time.

[0065] The fusion algorithm debugging provides key data support. Let the laser radar coordinate system point be , the IMU coordinate system point , the external parameter be the rotation matrix and the translation vector , and the formula be: where , and .

[0066] is the transformation in the IMU coordinate system. The laser radar needs to be used to convert the external parameter (rotation matrix , translation vector ) of the inertial measurement unit to the transformation in the laser radar coordinate system, let the laser radar coordinate system point be , the IMU coordinate system point , and the external parameter between the laser radar coordinate system (L) and the IMU coordinate system (imu) has been obtained through calibration. In this way, the motion of the radar point can be converted to the IMU coordinate system for unified processing. Wherein: .

[0067] The inverse transformation of is applied to the original point Located at The compensated point Wherein, .

[0068] The point From the old coordinate system at the time of acquisition, the new coordinate system is moved (compensated) in reverse. (The laser radar coordinate system at the moment) After performing this operation on all points in a frame, the entire frame of point cloud is equivalent to being collected by a stationary sensor at Instant, thereby completely eliminating the motion distortion within the frame.

[0069] Step S130, match the compensated point cloud with the local map, and tightly coupled fusion of inertial data and point cloud matching results through a filter, output the pose information of the sensor at the current time.

[0070] Wherein, the filter includes but is not limited to iterative extended Kalman filter (IEKF), extended Kalman filter (EKF), unscented Kalman filter (UKF) or particle filter. For example, the embodiment of the application takes the iterative extended Kalman filter as an example, matches the compensated point cloud with a local map based on incremental KD tree dynamic management, and through the iterative extended Kalman filter (IEKF), tightly coupled fusion of inertial measurement unit predicted pose priori and point cloud matching observation residual (i.e. point cloud plane constraint), accurate estimation and output of the pose information (six degrees of freedom position and attitude) of the sensor at the current time.

[0071] Wherein, the incremental KD tree is a data structure for efficient management of multi-dimensional space data (such as point cloud). Unlike the traditional KD tree, which needs to be globally reconstructed every time it is updated, the incremental KD tree supports local insertion and deletion operations, and only updates the affected branches, thereby significantly reducing the computational overhead. In the embodiment of the application, the incremental KD tree dynamically manages the point cloud data in the local map for storage and fast query.

[0072] Wherein, the tightly coupled state estimation method of fusing inertial measurement unit pre-integration and point cloud plane constraint converts the point-to-plane distance residual into the Jacobian matrix of state estimation, and realizes accurate correction of the pose. Point cloud coordinate conversion: convert the feature points from the self coordinate system to the world coordinate system. is a rotation matrix, and T is a translation vector, is a point in the self coordinate system, is a point in the world coordinate system, Indicates the plane normal vector.

[0073] .

[0074] Plane residual calculation: distance residual of laser point to plane: .

[0075] Jacobi matrix construction: derivative of residual to state variable, specifically: . Wherein is an anti-symmetric matrix.

[0076] On this basis, the pose estimation convergence judgment and dynamic adjustment method based on plane residual, the mean of laser point to plane residual and the number of effective feature points are used to evaluate the convergence of pose estimation; effective feature point number: effct_feat_num > 1; residual mean: res_mean_last = total_residual / effct_feat_num, the residual mean is calculated by the total residual and the number of effective feature points, and the convergence quality is evaluated.

[0077] Step S140, according to the pose information of the sensor, the compensated point cloud is added to the dynamically managed local map, and the updated local map is periodically published.

[0078] According to the pose of the sensor, the effective point cloud of the current frame is added to the local map. The map adopts a sliding window mechanism for dynamic management. When the distance between the sensor position and the map boundary is less than a threshold, the boundary migration is triggered, the old point cloud is removed and the new point cloud is added.

[0079] The technical scheme provided by the embodiment of the application is based on inertial data, motion distortion compensation is performed on the original point cloud data through motion estimation, the compensated point cloud is matched with the local map, and a filter is used to tightly coupled fusion of inertial data and point cloud matching results, and the pose information of the sensor at the current time is output; according to the pose information of the sensor, the compensated point cloud is added to the dynamically managed local map, and the updated local map is periodically published, realizing the complete mapping function from data acquisition, processing, fusion to map construction. And through motion distortion compensation, tightly coupled fusion of inertial data and point cloud matching results and periodic publication of the updated local map, when the mapping area is large, the mapping precision can still be maintained.

[0080] In an implementation manner, in a scene in which a prior map has been constructed, a mobile device needs to realize rapid repositioning, so that the scheme has the ability to load the prior map for accurate positioning. The mapping method based on fusion of laser radar and inertial measurement unit provided by the embodiment of the application further includes: in the case that a positioning instruction is received, identity information of a user is acquired; based on the identity information of the user, a storage path of a pre-constructed global point cloud map is dynamically acquired, and the global point cloud map is loaded; the compensated point cloud acquired in real time is matched with the global point cloud map, and global pose information in the global point cloud map is output, the global pose information being used to represent the position of the device in the global point cloud map.

[0081] Specifically, the running mode is switched to the localization mode through a ROS service call, receiving a specific positioning instruction, or through a parameter server. The map management module calls an operating system API (such as calling the getuid() function to obtain current user identity information, for example, a user ID or a username, under a Linux system), and then obtains the complete storage path of the global point cloud map based on the user identity information, and loads the global point cloud map. It should be noted that during the global map construction process, the corresponding global point cloud map is saved according to the user identity information, and the complete storage path is also saved. For example, under a standardized map storage directory structure, the generated path can be: string map_path = " / home / " + "user_001" +" / .ros / maps / warehouse_navigation.pcd". In this way, the tedious modification and compatibility problems caused by writing a fixed (hard-coded) path into the source code or configuration file are avoided.

[0082] When lane line data exists in the pre-constructed global point cloud map, the global point cloud map and the lane line data are cooperatively published. After successfully loading the global point cloud map, the compensated point cloud (i.e., the current frame point cloud after motion distortion compensation and feature extraction) obtained in real time is matched with the loaded global point cloud map. This matching process reuses the optimized iterative extended Kalman filter tightly coupled algorithm in the mapping mode. However, in the localization mode, the key difference in the observation model is that the residual error of point cloud matching is no longer relative to the local map, but is calculated relative to the loaded global prior map. The filter accurately estimates the six-degree-of-freedom global pose information (i.e., position and attitude) of the device in the global map coordinate system by minimizing the residual error, and publishes it through a ROS topic. In this way, the integration of mapping and localization functions can be achieved, and the precise positioning demand based on the prior map can be met.

[0083] In an implementation manner, after the step of compensating the original point cloud data by motion distortion compensation based on inertial data through motion estimation to obtain compensated point cloud, the method further comprises: based on the scan line number, calculating the number of point clouds corresponding to the scan line number, and calculating the curvature feature of the feature point in the point cloud data, wherein the curvature feature is determined by calculating the Euclidean distance between the feature point and the previous adjacent point of the feature point on the same scan line; in the case that the number of point clouds is less than a preset number, the point cloud data corresponding to the scan line number is removed.

[0084] The formula for calculating the curvature Feature(P) of the feature point using the Euclidean distance is: Wherein, v(t) explicitly represents the speed vector at time t, and the components vx, vy, vz correspond to the speed in the x, y, z directions, respectively.

[0085] Wherein, P_prev is the previous adjacent point of point P on the same scan line. Specifically, according to the R (scan line number) information of the point, the point cloud of a frame is grouped according to the beam, and the point cloud of each beam is processed respectively. In the feature point extraction, first, the point cloud after motion compensation is traversed, and each feature point is stored in the corresponding scan line buffer according to the Ring value. Then, each scan line is traversed by loop. The number of point clouds of the current scan line is calculated. In the case that the number of point clouds is less than the preset threshold, the Line feature filtering is performed. In this way, the laser beams with sparse point clouds can be quickly filtered out, and the calculation efficiency can be effectively improved, which can improve the efficiency of the later voxel filtering and outlier removal.

[0086] The present application adopts the Euclidean distance feature extraction, and changes the two-point square sum method for calculating the curvature in the prior art to calculate the Euclidean distance. This makes the feature value have a clear physical meaning (distance), which is convenient for subsequent obstacle distance calculation and improves the calculation efficiency; for the effective scan line grouping filtered, the feature points (such as corner points and plane points) are extracted. In order to extract the features, the curvature of each point needs to be calculated to judge the sharpness. In the prior art, the square sum of coordinate difference is generally used as an approximate value of the curvature. The calculation method is changed to calculate the Euclidean distance between two points as the curvature value. The curvature calculated in this way has a clear physical unit (distance unit), and its value directly represents the distance, so that the subsequent threshold setting, obstacle distance judgment and algorithm debugging become very intuitive and convenient.

[0087] In this way, the invalid scan line point cloud caused by occlusion, too far, etc. is quickly filtered in the preprocessing stage, that is, the noise data is efficiently filtered in the feature extraction front end, the calculation overhead is reduced, and the efficiency and stability of the subsequent processing are improved.

[0088] In an implementation manner, in order to make the mapping method based on fusion of laser radar and inertial measurement unit provided by the embodiment of the present application applicable to real-time map management in a large range scene, the local map is a dynamic local map management based on incremental KD tree and sliding window mechanism. Specifically, as shown in Figure 3 The method comprises steps S310 to S320.

[0089] In step S310, the distance between the current position of the laser radar and the boundary of the current local map is monitored in real time.

[0090] Continuously track the current position of the lidar (based on the pose information estimated by the IEKF) and calculate the nearest distance between the lidar and the current local map boundary (usually a cuboid boundary with the map center as the origin). The local map boundary can be defined by a preset cuboid size, for example, with a side length of 20 meters. The distance calculation uses the Euclidean distance formula to compare the distance between the sensor coordinates and the boundary surface in real time, that is, to monitor the nearest distance between the sensor center position and the current local map cuboid boundary in real time.

[0091] Step S320 triggers map boundary migration when the distance is less than the preset threshold, dynamically adds new observation point clouds and removes redundant point clouds beyond the field of view to generate a new local map.

[0092] A trigger threshold is set, and when the nearest distance is less than the trigger threshold, the map boundary migration is triggered, for example, when it is monitored that the distance between the sensor position and the map boundary is less than the preset threshold (for example, 3 meters), the map boundary migration operation is automatically triggered. When migrating, the boundary migration distance is calculated, new point clouds entering the field of view are dynamically added, and old point clouds sliding out of the window are removed. The KD tree is also incrementally updated instead of globally reconstructed.

[0093] The migration process includes dynamically adding new point clouds, removing redundant point clouds, and incrementally updating the KD tree. Dynamically add new point clouds: insert the latest compensated point cloud data into the incremental KD tree. When inserting, an adaptive filtering strategy (such as voxel filtering) is used to retain only points with a distance greater than a certain threshold from the existing point clouds, avoiding redundancy. Remove redundant point clouds: based on the sliding window principle, delete nodes that are outside the current field of view or have outdated timestamps. For example, remove point clouds that are more than the window radius (such as 15 meters) away from the current position of the sensor. Incremental KD tree update: only rebalance the affected branches instead of globally reconstructing, reducing the calculation delay. After migration is complete, a new local map centered on the sensor is regenerated based on the incremental KD tree, which is used for subsequent point cloud matching. Map data is periodically published in the form of a topic for use by other modules (such as positioning).

[0094] Sliding window mechanism: a resource management strategy that maintains a fixed size or dynamic range window to retain the most recent relevant data. When new data enters, old data is removed from the window, ensuring that the system only processes the most relevant information currently. Combined with the incremental KD tree, this mechanism enables dynamic updating of the local map, avoiding infinite map expansion.

[0095] The incremental KD tree dynamic management and sliding window mechanism work together to ensure that the mapping system only maintains valid map data within the current field of view in large-scale environments, improving real-time performance and accuracy.

[0096] In addition, an adaptive point cloud screening and map incremental updating strategy is adopted. Specifically, a point cloud screening mechanism based on voxel filtering and neighborhood point distance criterion is adopted to control the size of the map while ensuring the accuracy of the map by selectively adding point clouds. The screening condition is that if the distance between the neighborhood point and the center of the voxel is less than the distance between the current point and the center, the point is not added. In this way, the memory occupation and calculation time consumption in a large-scale environment can be reduced.

[0097] In an implementation manner, in order to improve the accuracy and robustness of state estimation, the sensor extrinsic parameter and time deviation are calibrated online, and the fusion accuracy is improved. In the step of performing state estimation by tightly coupling fusion of inertial data and point cloud matching results and outputting the pose information at the current time, the step further includes: online estimation of the extrinsic parameter between the lidar and the inertial measurement unit; and compensation and calibration of the timestamp offset between the lidar and the inertial measurement unit.

[0098] In the state estimation process of the IEKF, multiple optimizations are performed. (a) Tightly coupled state estimation: the prior pose obtained by pre-integration of the inertial measurement unit is tightly coupled with the plane constraint of the point cloud (the distance residual of the laser point to the local map plane), and the pose is corrected through the Jacobian matrix. (b) Time synchronization compensation: the timestamp offset between the lidar and the IMU is estimated in the state vector and is compensated. (c) Online estimation of extrinsic parameters: the extrinsic parameters between the lidar and the inertial measurement unit are taken as state variables, and are optimized in the IEKF iteration process to realize online self-calibration.

[0099] Among them, the IMU coordinate system point cloud publishing function: provides the visualization capability of lidar data in the IMU coordinate system, which is convenient for verifying the accuracy of coordinate transformation, and provides key data support for multi-sensor calibration and fusion algorithm debugging. Let the lidar coordinate system point be , the IMU coordinate system point , the extrinsic parameter be the rotation matrix and the translation vector , and the formula be: , wherein is a 3x3 rotation matrix, satisfies , and .

[0100] In this way, the errors caused by changes in extrinsic parameters and time asynchronization are effectively eliminated, and the accuracy and robustness of long-term operation of the system are improved.

[0101] In an implementation, in order to reduce resource consumption of map publishing, real-time performance of map data is balanced with resource consumption. In the embodiment of the application, a period of publishing local map data is configurable. Specifically, in the code logic of publishing local map, a publishing counter pub_count and a configurable parameter MAP_PUB_PERIOD (publishing period) are set. The code logic is: if (pub_count % MAP_PUB_PERIOD == 0) {publish_map(...);}. That is, the map is published once every MAP_PUB_PERIOD frame processing is completed. The parameter can be dynamically adjusted according to actual needs of a user, for example, the local map can also be published once every 10 frames. In this way, the computing and communication overheads caused by map publishing are greatly reduced.

[0102] The embodiment of the application further provides a mapping device based on fusion of a laser radar and an inertial measurement unit, as shown in the figure, comprising an acquisition module, a compensation module, a local map matching module and a local map updating module. Figure 4

[0103] The acquisition module synchronously acquires measurement data of multiple sensors and pre-processes the measurement data; the multiple sensors at least comprise a laser radar and an inertial measurement unit, and the measurement data of the multiple sensors at least comprises original point cloud data of the laser radar and inertial data of the inertial measurement unit.

[0104] The compensation module compensates motion distortion of the original point cloud data based on the inertial data through motion estimation, to obtain compensated point cloud.

[0105] The local map matching module matches the compensated point cloud with a local map and tightly couples fusion of the inertial data and the point cloud matching result through a filter, to output pose information of the sensor at a current time.

[0106] The local map updating module adds the compensated point cloud into a dynamically managed local map according to the pose information of the sensor, and periodically publishes the updated local map.

[0107] In an implementation, the mapping device based on fusion of the laser radar and the inertial measurement unit provided by the embodiment of the application further comprises a map loading module and a pose output module.

[0108] The acquisition module is further used to acquire identity information of a user in a case where a positioning instruction is received.

[0109] The map loading module is used to dynamically acquire a storage path of a pre-constructed global point cloud map based on the identity information of the user, and load the global point cloud map.

[0110] ​The pose output module is configured to match the real-time acquired compensated point cloud with the global point cloud map, and output global pose information in the global point cloud map, wherein the global pose information is used to represent the position of the device in the global point cloud map.

[0111] The embodiment of the application provides a mapping method based on fusion of a laser radar and an inertial measurement unit, adds dynamic configuration of a map / road line path based on user identity information, solves a hard coding compatibility problem, constructs a point cloud and road line cooperative release architecture, improves data adaptability, and realizes mapping path and data release optimization. A pose matching algorithm is optimized, and functions such as incremental KD tree local map management, IMU-point cloud plane constraint tight coupling estimation, adaptive point cloud screening, time synchronization compensation, residual error convergence judgment, and external parameter online estimation are integrated, which is suitable for large scene real-time mapping and upgrades SLAM pose matching. In the actual application process, by deleting the laser radar identifier definition, multiple radars are adapted based on the data transmission mechanism, and the sensor adaptation capability is enhanced. The IMU median method and forward propagation are added to realize point cloud reverse distortion compensation. The two point square sums of feature extraction are changed to Euclidean distance calculation, scanning line processing and line point cloud filtering are added, the obstacle distance output convenience and calculation efficiency are improved, and feature extraction optimization is realized. The covariance matrix filling is removed to reduce the overhead; the map point cloud periodic release and the IMU coordinate system point cloud release are added to balance resource consumption and help debugging.

[0112] The above detailed description further explains the purpose, technical solutions and beneficial effects of the application. It should be understood that the above is only a specific embodiment of the application and is not used to limit the protection scope of the application. Any modification, equivalent replacement, improvement, etc. made on the basis of the technical solutions of the application should be included in the protection scope of the application.

Claims

1. A mapping method based on fusion of laser radar and inertial measurement unit, characterized in that, The method comprises the following steps: Synchronously acquiring measurement data of multiple sensors, and preprocessing the measurement data; the multiple sensors at least include a laser radar and an inertial measurement unit, and the measurement data of the multiple sensors at least include raw point cloud data of the laser radar and inertial data of the inertial measurement unit; Based on the inertial data, motion distortion compensation is performed on the raw point cloud data through motion estimation, to obtain compensated point cloud; The compensated point cloud is matched with a local map, and a filter is used to tightly couple the inertial data and the point cloud matching result, to output pose information of the sensor at the current time; According to the pose information of the sensor, the compensated point cloud is added to the dynamically managed local map, and an updated local map is periodically published.

2. The mapping method based on fusion of laser radar and inertial measurement unit according to claim 1, characterized in that, Further comprising: In the case of receiving a positioning instruction, obtaining identity information of a user; Based on the identity information of the user, dynamically obtaining a storage path of a pre-constructed global point cloud map, and loading the global point cloud map; The compensated point cloud obtained in real time is matched with the global point cloud map, and global pose information in the global point cloud map is output, which is used to represent the position of the device in the global point cloud map.

3. The mapping method based on fusion of laser radar and inertial measurement unit according to claim 1, characterized in that, The preprocessing of the measurement data comprises: Parsing the raw point cloud data, and converting the parsed raw point cloud data into standardized point cloud data containing three-dimensional coordinates, intensity information, scan line number and timestamp offset.

4. The mapping method based on fusion of laser radar and inertial measurement unit according to claim 3, characterized in that, After the step of compensating the raw point cloud data through motion estimation based on the inertial data to obtain compensated point cloud, the method further comprises: Based on the scan line number, the number of point clouds corresponding to the scan line number is calculated, and the curvature feature of the feature points in the point cloud data is calculated, wherein the curvature feature is determined by calculating the Euclidean distance between the feature points and the previous adjacent points in the same scan line; In the case that the number of point clouds is less than a preset number, the point cloud data corresponding to the scan line number is removed.

5. The mapping method based on fusion of laser radar and inertial measurement unit according to claim 1, characterized in that, In the step of compensating the raw point cloud data through motion estimation based on the inertial data, the method further comprises: Using the timestamp offset T of each feature point and the enhanced inertial data to accurately calculate the pose of the sensor at the sampling time of each feature point; According to the pose of each feature point at the sampling time of the sensor, the continuous motion trajectory of the sensor from the starting sampling time to the ending sampling time of the current frame point cloud is calculated; Based on the continuous motion trajectory, the accurate sampling time of the feature point is determined, and the pose transformation of the sensor from the accurate sampling time to the ending sampling time is calculated; The inverse transformation of the calculated pose transformation is applied to the feature point, and the feature point coordinates are mapped to the sensor coordinate system at the ending sampling time.

6. The mapping method based on fusion of laser radar and inertial measurement unit according to claim 5, characterized in that, In the step of calculating the continuous motion trajectory of the sensor from the starting sampling time to the ending sampling time of the current frame point cloud according to the pose of each feature point at the sampling time of the sensor, it comprises: The median integral method is used to integrate the angular velocity and acceleration data in the inertial data to reconstruct the continuous motion trajectory.

7. The mapping method based on fusion of laser radar and inertial measurement unit according to claim 1, characterized in that, The local map is managed based on a dynamic local map management mechanism of incremental KD tree and sliding window mechanism, and specifically includes: Real-time monitoring of the distance between the current position of the laser radar and the boundary of the current local map; When the distance is less than a preset threshold, triggering map boundary migration, dynamically adding new observation point cloud and removing redundant point cloud beyond the field of view, to generate a new local map.

8. The mapping method based on fusion of lidar and inertial measurement unit according to claim 1, characterized in that, In the step of tightly coupling and fusing the inertial data and the point cloud matching result through the filter, and performing state estimation to output the pose information of the sensor at the current time, further including: Online estimation of the external parameter between the laser radar and the inertial measurement unit; Compensation and calibration of the timestamp offset between the laser radar and the inertial measurement unit.

9. The mapping method based on fusion of lidar and inertial measurement unit according to claim 1, characterized in that, The period of publishing the local map data is configurable.

10. A mapping device based on fusion of lidar and inertial measurement unit, characterized in that, It includes: An acquisition module synchronously acquires measurement data of multiple sensors and pre-processes the measurement data; the multiple sensors at least include a laser radar and an inertial measurement unit, and the measurement data of the multiple sensors at least include raw point cloud data of the laser radar and inertial data of the inertial measurement unit; A compensation module compensates motion distortion of the raw point cloud data based on the inertial data through motion estimation to obtain compensated point cloud; A local map matching module matches the compensated point cloud with a local map, and tightly couples and fuses the inertial data and the point cloud matching result through a filter to output the pose information of the sensor at the current time; A local map updating module adds the compensated point cloud into a dynamically managed local map according to the pose information of the sensor, and periodically publishes the updated local map.

Images