Multi-sensor fusion-based localization and mapping methods, devices, and equipment

Abstract

The application provides a positioning mapping method, device and equipment based on multi-sensor fusion, and belongs to the technical field of robot perception and navigation, wherein the method comprises the following steps: acquiring multi-source sensor observation data of a target device, wherein the multi-source sensor observation data comprises first laser point cloud observed by a laser radar unit, inertial data observed by an inertial measurement unit and wheel speed meter data observed by a wheel speed meter; performing motion compensation on the first laser point cloud based on the inertial data and the wheel speed meter data to obtain second laser point cloud after compensation; performing pose estimation on the target device based on the second laser point cloud, the inertial data and the wheel speed meter data to obtain a pose estimation result; and constructing a map of a space where the target device is located based on the pose estimation result. The application improves the stability and continuity of the positioning result, improves the mapping precision, and can be effectively applied to a corridor, a tunnel and other geometric degeneration environments.

Description

Technical Field

[0001] This invention relates to the field of robot perception and navigation technology, and in particular to a localization and mapping method, apparatus and equipment based on multi-sensor fusion. Background Technology

[0002] With the rapid development of mobile robots, unmanned vehicles and intelligent inspection systems, achieving high-precision positioning and mapping in unknown or semi-unknown environments has become one of the key technologies for improving the autonomous operation capability of systems.

[0003] Currently, traditional SLAM (Simultaneous Localization and Mapping) methods are difficult to apply effectively to geometrically degraded environments such as corridors and tunnels, which can easily lead to insufficient mapping accuracy and the accumulation of positioning errors. Summary of the Invention

[0004] This invention provides a positioning and mapping method, apparatus, and device based on multi-sensor fusion to solve the problem that traditional SLAM methods are difficult to apply effectively to geometrically degraded environments such as corridors and tunnels, which easily leads to insufficient mapping accuracy and accumulation of positioning errors.

[0005] In a first aspect, the present invention provides a localization and mapping method based on multi-sensor fusion, comprising: Acquire multi-source sensor observation data of the target device; wherein, the multi-source sensor observation data includes a first laser point cloud observed by a lidar unit, inertial data observed by an inertial measurement unit, and wheel speed meter data observed by a wheel speed meter; Based on the inertial data and the wheel speed meter data, motion compensation is performed on the first laser point cloud to obtain the compensated second laser point cloud; Based on the second laser point cloud, the inertial data, and the wheel speed meter data, the pose of the target device is estimated to obtain the pose estimation result. Based on the pose estimation results, a map of the space where the target device is located is constructed.

[0006] In some embodiments, the step of performing motion compensation on the first laser point cloud based on the inertial data and the wheel speedometer data to obtain a compensated second laser point cloud includes: The inertial data and the wheel speed meter data are fused to obtain fused data; wherein, the fused data is used to characterize the state changes of the target device during the scanning cycle; Motion compensation is performed on the first laser point cloud using the fused data to obtain the compensated second laser point cloud.

[0007] In some embodiments, the step of using the fused data to perform motion compensation on the first laser point cloud to obtain a compensated second laser point cloud includes: The state changes of the target device during the scanning cycle are integrated to obtain the integral result; Based on the integration results, a continuous-time motion trajectory corresponding to the target device is constructed; Based on the continuous time motion trajectory, time interpolation and coordinate transformation are performed on each laser point cloud in the first laser point cloud to obtain the second laser point cloud.

[0008] In some embodiments, the step of performing pose estimation on the target device based on the second laser point cloud, the inertial data, and the wheel velocity meter data to obtain pose estimation results includes: Point-to-surface residuals are calculated based on the second laser point cloud to obtain the calculation results. State estimation is performed based on the inertial data to obtain the state estimation result; Speed ​​measurement is performed based on the wheel speed gauge data to obtain the speed measurement result; Based on the point-surface residual calculation results, the state estimation results, and the velocity measurement results, the pose estimation of the target device is performed using the Kalman filter method with iterative error state, and the pose estimation results are obtained.

[0009] In some of these embodiments, the map of the space where the target device is located includes a three-dimensional voxel map; The step of constructing a map of the space where the target device is located based on the pose estimation result includes: The OctoMap method is used to perform a voxel representation of the space where the target device is located, and the voxel representation result of the space where the target device is located is obtained. The occupancy probability of each voxel in the voxelized representation result is determined by using a log-odds model. The voxelization representation is updated based on the occupancy probability of each voxel, and the updated voxelization representation is used as the three-dimensional occupancy voxel map.

[0010] In some embodiments, the map of the space where the target device is located further includes a two-dimensional occupancy grid map; after using the updated voxelized representation as the three-dimensional occupancy voxel map, the method further includes: The three-dimensional occupied voxel map is projected along the height direction to generate the two-dimensional occupied grid map; In the two-dimensional occupied grid map, if there is an occupied voxel within the corresponding height range in each two-dimensional grid, the grid is determined to be an obstacle area.

[0011] In some embodiments, acquiring multi-source sensor observation data of the target device includes: Obtain the candidate laser point cloud collected by the lidar unit in the coordinate system where the lidar unit is located; Acquire candidate inertial data collected by the inertial measurement unit in the coordinate system where the inertial measurement unit is located; Acquire candidate wheel speed data collected by the wheel speed meter in the coordinate system where the wheel speed meter is located; The candidate laser point cloud, the candidate inertial data, and the candidate wheel speed meter data are respectively converted to the reference coordinate system of the target device, and after performing a time synchronization operation, the first laser point cloud, the inertial data, and the wheel speed meter data are obtained.

[0012] Secondly, the present invention also provides a positioning and mapping device based on multi-sensor fusion, comprising: The acquisition module is used to acquire multi-source sensor observation data of the target device; wherein, the multi-source sensor observation data includes a first laser point cloud observed by the lidar unit, inertial data observed by the inertial measurement unit, and wheel speed meter data observed by the wheel speed meter; The motion compensation module is used to perform motion compensation on the first laser point cloud based on the inertial data and the wheel speed meter data to obtain the compensated second laser point cloud. The pose estimation module is used to perform pose estimation on the target device based on the second laser point cloud, the inertial data and the wheel speed meter data, and obtain the pose estimation result. A construction module is used to construct a map of the space where the target device is located based on the pose estimation results.

[0013] In some embodiments, the motion compensation module is specifically used for: The inertial data and the wheel speed meter data are fused to obtain fused data; wherein, the fused data is used to characterize the state changes of the target device during the scanning cycle; Motion compensation is performed on the first laser point cloud using the fused data to obtain the compensated second laser point cloud.

[0014] In some embodiments, the motion compensation module is further specifically used for: The state changes of the target device during the scanning cycle are integrated to obtain the integral result; Based on the integration results, a continuous-time motion trajectory corresponding to the target device is constructed; Based on the continuous time motion trajectory, time interpolation and coordinate transformation are performed on each laser point cloud in the first laser point cloud to obtain the second laser point cloud.

[0015] In some embodiments, the pose estimation module is specifically used for: Point-to-surface residuals are calculated based on the second laser point cloud to obtain the calculation results. State estimation is performed based on the inertial data to obtain the state estimation result; Speed ​​measurement is performed based on the wheel speed gauge data to obtain the speed measurement result; Based on the point-surface residual calculation results, the state estimation results, and the velocity measurement results, the pose estimation of the target device is performed using the Kalman filter method with iterative error state, and the pose estimation results are obtained.

[0016] In some embodiments, the map of the space where the target device is located includes a three-dimensional voxel map; the construction module is specifically used for: The OctoMap method is used to perform a voxel representation of the space where the target device is located, and the voxel representation result of the space where the target device is located is obtained. The occupancy probability of each voxel in the voxelized representation result is determined by using a log-odds model. The voxelization representation is updated based on the occupancy probability of each voxel, and the updated voxelization representation is used as the three-dimensional occupancy voxel map.

[0017] In some embodiments, the building module is further specifically used for: The three-dimensional occupied voxel map is projected along the height direction to generate the two-dimensional occupied grid map; In the two-dimensional occupied grid map, if there is an occupied voxel within the corresponding height range in each two-dimensional grid, the grid is determined to be an obstacle area.

[0018] In some embodiments, the acquisition module is specifically used for: Obtain the candidate laser point cloud collected by the lidar unit in the coordinate system where the lidar unit is located; Acquire candidate inertial data collected by the inertial measurement unit in the coordinate system where the inertial measurement unit is located; Acquire candidate wheel speed data collected by the wheel speed meter in the coordinate system where the wheel speed meter is located; The candidate laser point cloud, the candidate inertial data, and the candidate wheel speed meter data are respectively converted to the reference coordinate system of the target device, and after performing a time synchronization operation, the first laser point cloud, the inertial data, and the wheel speed meter data are obtained.

[0019] Thirdly, the present invention also provides an electronic device, including a memory and a processor, wherein the memory stores a computer program, and the processor is configured to run the computer program to implement the multi-sensor fusion localization and mapping method described in the first aspect above.

[0020] Fourthly, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the multi-sensor fusion localization and mapping method described in the first aspect above.

[0021] Fifthly, the present invention also provides a computer program product, including a computer program that, when executed by a processor, implements the multi-sensor fusion localization and mapping method described in the first aspect above.

[0022] Compared with existing technologies, the multi-sensor fusion positioning and mapping method, apparatus, and device provided by this invention first acquires multi-source sensor observation data obtained from the observations of the target device by the lidar unit, inertial measurement unit, and wheel speedometer. Then, based on the inertial data observed by the inertial measurement unit and the wheel speedometer data observed by the wheel speedometer, motion compensation is performed on the first laser point cloud observed by the lidar unit to obtain a compensated second laser point cloud, thereby improving the consistency and accuracy of the point cloud data. Next, based on the second laser point cloud, inertial data, and wheel speedometer data, pose estimation is performed on the target device to obtain the pose estimation result. Based on the pose estimation result, a map of the space where the target device is located is constructed. By introducing wheel speedometer motion constraints and tightly coupling and fusing them with the observation data from the lidar unit and inertial measurement unit, the observability of the target device in geometrically degraded environments such as corridors and tunnels is effectively enhanced, pose drift and positioning error accumulation are effectively suppressed, and the stability and continuity of the positioning results are improved, thereby improving mapping accuracy. This method is effectively applicable to positioning and mapping in geometrically degraded environments such as corridors and tunnels.

[0023] Details of one or more embodiments of the present invention are set forth in the following drawings and description to make other features, objects and advantages of the invention more readily apparent. Attached Figure Description

[0024] The accompanying drawings, which are included to provide a further understanding of the invention and constitute a part of this invention, illustrate the invention. Those skilled in the art will recognize that other drawings can be derived from these drawings without any inventive effort. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an undue limitation of the invention. In the drawings: Figure 1 This is one of the flowcharts of the localization and mapping method based on multi-sensor fusion provided by the present invention.

[0025] Figure 2 This is the second flowchart of the localization and mapping method based on multi-sensor fusion provided by the present invention.

[0026] Figure 3 This is a schematic diagram of data fusion in the multi-sensor fusion-based localization and mapping method provided by the present invention.

[0027] Figure 4 This is the third flowchart of the localization and mapping method based on multi-sensor fusion provided by the present invention.

[0028] Figure 5 This is a schematic diagram of point cloud distortion removal in the multi-sensor fusion-based localization and mapping method provided by the present invention.

[0029] Figure 6 This is a schematic diagram of a three-dimensional occupied voxel map in the localization and mapping method based on multi-sensor fusion provided by the present invention.

[0030] Figure 7 This is a schematic diagram of the process for determining the occupying voxel in the localization and mapping method based on multi-sensor fusion provided by the present invention.

[0031] Figure 8 This is a schematic diagram illustrating the localization effect of the FAST-LIO2 method in a degraded environment.

[0032] Figure 9 This is a schematic diagram illustrating the positioning effect of the multi-sensor fusion-based positioning and mapping method provided by this invention in a degraded environment.

[0033] Figure 10 This is a schematic diagram illustrating the mapping effect of the FAST-LIO2 method in a degraded environment.

[0034] Figure 11 This is a schematic diagram illustrating the mapping effect of the multi-sensor fusion-based localization and mapping method provided by the present invention in a degraded environment.

[0035] Figure 12 This is a schematic diagram of the positioning and mapping device based on multi-sensor fusion provided by the present invention.

[0036] Figure 13 This is a schematic diagram of the physical structure of an electronic device provided by the present invention. Detailed Implementation

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

[0038] Unless otherwise defined, the technical or scientific terms used in this invention shall have the general meaning understood by one of ordinary skill in the art to which this invention pertains. Words such as “a,” “an,” “an,” “the,” “the,” and “these” used in this invention do not indicate quantitative limitation and may be singular or plural. The terms “comprising,” “including,” “having,” and any variations thereof used in this invention are intended to cover non-exclusive inclusion; for example, a process, method, system, product, or device that comprises a series of steps or modules (units) is not limited to the listed steps or modules (units) but may include steps or modules (units) not listed, or may include other steps or modules (units) inherent to these processes, methods, products, or devices. The terms “connected,” “linked,” and “coupled” used in this invention are not limited to physical or mechanical connections but may include electrical connections, whether direct or indirect. “A plurality” used in this invention refers to two or more. “And / or” describes the relationship between related objects, indicating that three relationships may exist; for example, “A and / or B” can represent: A alone, A and B simultaneously, and B alone. Normally, the character " / " indicates that the objects before and after it are in an "or" relationship. The terms "first," "second," "third," etc., used in this invention are merely for distinguishing similar objects and do not represent a specific ordering of the objects.

[0039] In related technologies, traditional SLAM methods mostly rely on two-dimensional LiDAR units or visual sensors, which have certain application effects in structured or planar environments. However, in complex indoor environments, underground spaces, and industrial scenarios, the environment usually has significant three-dimensional features. Relying solely on two-dimensional information is insufficient to accurately describe the spatial structure, which can easily lead to problems such as insufficient mapping accuracy and accumulation of positioning errors.

[0040] In recent years, SLAM methods based on 3D lidar units have been extensively studied. By introducing an inertial measurement unit (IMU) to achieve laser-inertial fusion positioning, the real-time performance and robustness of the system have been improved to some extent. However, in geometrically degraded environments such as corridors and tunnels, due to the simple distribution of environmental features or insufficient constraint directions, laser SLAM systems are prone to insufficient observability, leading to unstable pose estimation and cumulative drift.

[0041] On the other hand, IMUs typically rely on integral calculations to obtain pose information in state estimation. Their errors accumulate over time, and especially under low dynamic or long-term operating conditions, they are prone to large drifts, making it difficult to effectively compensate for the lack of constraints of lidar units in degraded environments.

[0042] Furthermore, although wheel speed meters can provide motion information independent of environmental geometry and have good short-term stability in ground mobile platforms, existing methods mostly use them as auxiliary information and lack a deep fusion mechanism with lidar units and IMUs. They fail to form unified constraints in state estimation and point cloud processing, thus making it difficult to fully leverage their advantages in degraded environments.

[0043] Therefore, there is an urgent need for a localization and mapping method that can effectively integrate information from lidar units, IMUs, and wheel velocity meters in geometrically degraded environments to improve system observability, suppress pose drift, and enhance mapping accuracy.

[0044] To address the aforementioned issues, this invention proposes a localization and mapping method based on multi-sensor fusion in degraded environments. By introducing wheel velocity measurement motion constraints and constructing a unified state estimation framework based on Iterated Error-State Kalman Filter (IESKF), stable estimation of the pose of robots and other equipment and high-precision map construction can be achieved.

[0045] The following description, in conjunction with the accompanying drawings, describes the multi-sensor fusion-based localization and mapping method of the present invention, which is effectively applicable to device localization and mapping in geometrically degraded scenarios.

[0046] Figure 1 This is one of the flowcharts illustrating the localization and mapping method based on multi-sensor fusion provided by the present invention, such as... Figure 1 As shown, the localization and mapping method based on multi-sensor fusion includes the following steps: Step S101: Acquire multi-source sensor observation data of the target device; The multi-source sensor observation data includes the first laser point cloud observed by the lidar unit, the inertial data observed by the inertial measurement unit, and the wheel speed meter data observed by the wheel speed meter.

[0047] Optionally, the target device can be a robot, a vehicle, or other mobile device that needs to be located and / or mapped, so as to dynamically locate and / or map such mobile devices to facilitate effective navigation.

[0048] It should be noted that multi-source sensor observation data can also include data from other sensors, such as images acquired by cameras. This example only uses multi-source sensor observation data including data from lidar units, inertial measurement units, and wheel speedometers, and there are no restrictions on this.

[0049] Step S102: Based on inertial data and wheel speed meter data, motion compensation is performed on the first laser point cloud to obtain the compensated second laser point cloud.

[0050] It should be noted that the motion compensation of the first laser point cloud is performed using inertial data and wheel speed meter data to correct the distortion caused by the high-speed non-uniform motion of the lidar during the acquisition period, thereby improving the accuracy of point cloud correction and the consistency of map construction.

[0051] Step S103: Based on the second laser point cloud, inertial data and wheel speed meter data, the pose of the target device is estimated to obtain the pose estimation result.

[0052] Step S104: Based on the pose estimation results, construct a map of the space where the target device is located.

[0053] Specifically, firstly, multi-source sensor observation data obtained from the LiDAR unit, inertial measurement unit, and wheel speedometer are acquired from the target device. Then, based on the inertial data observed by the inertial measurement unit and the wheel speedometer data observed by the wheel speedometer, motion compensation is performed on the first laser point cloud observed by the LiDAR unit to obtain a compensated second laser point cloud, thereby improving the consistency and accuracy of the point cloud data. Then, based on the second laser point cloud, inertial data, and wheel speedometer data, pose estimation is performed on the target device to obtain the pose estimation result. Based on the pose estimation result, a map of the space where the target device is located is constructed. For example, a map of the robot in the corridor space is constructed, which can show the robot's location and movement trajectory in the corridor, as well as the positions of walls, obstacles, etc. in the corridor, which helps to use the map for precise navigation of the robot later.

[0054] In this embodiment of the invention, by introducing wheel speed measurement motion constraints and tightly coupling and fusing them with the observation data of the lidar unit and the inertial measurement unit, the observability of the target device in geometrically degraded environments such as corridors and tunnels is effectively enhanced, pose drift and positioning error accumulation are effectively suppressed, the stability and continuity of positioning results are improved, and the mapping accuracy is improved. It can be effectively applied to positioning and mapping in geometrically degraded environments such as corridors and tunnels.

[0055] In some of these embodiments, a specific implementation of motion compensation for laser point clouds is provided. Figure 2 This is the second flowchart of the localization and mapping method based on multi-sensor fusion provided by the present invention, as shown below. Figure 2 As shown, in Figure 1 Based on this, step S102 above may include the following steps: Step S1021: Perform data fusion on the inertial data and wheel speed meter data to obtain fused data; Among them, the fused data is used to characterize the state changes of the target device during the scanning cycle.

[0056] Step S1022: Motion compensation is performed on the first laser point cloud using the fused data to obtain the compensated second laser point cloud.

[0057] Specifically, inertial data from IMU observations and wheel speedometer data from wheel speedometer observations can be fused to construct unified angular velocity and linear velocity information, which can be used to reduce the cumulative impact of IMU integration errors.

[0058] For example, inertial data may include angular velocity information collected by an IMU, and wheel speed meter data may include linear velocity information collected by a wheel speed meter. The angular velocity information provided by the IMU and the linear velocity information provided by the wheel speed meter are fused to construct a unified motion information representation, thereby improving the stability of motion estimation.

[0059] The overall score of the fused data is as follows:

[0060]

[0061] in, For the first i Rotation increment of frame fusion data, Angular velocity, For speed, For position increment, Indicates the sampling time index of a discrete-time sequence. This represents the exponential mapping from Lie algebras to Lie groups. Sampling time, This indicates that the gyroscope has zero bias.

[0062] In some embodiments, a specific implementation of motion compensation using fused data is provided. Step S1022 above may include the following steps: Step S1022-1: Perform integral calculation on the state changes of the target device during the scanning cycle to obtain the integral result; Step S1022-2: Based on the integration results, construct the continuous-time motion trajectory corresponding to the target device; Step S1022-3: Based on the continuous time motion trajectory, perform time interpolation and coordinate transformation on each laser point cloud in the first laser point cloud to obtain the second laser point cloud.

[0063] Specifically, a pre-integral model can be constructed based on the fused data to perform integral calculations on the state changes within the scanning period, and a continuous-time motion trajectory model can be established to determine the continuous-time motion trajectory. The specific point cloud distortion correction process can be represented as follows:

[0064] in, and These measurements correspond to the two most recent adjacent fused data points. For self-time Start of experiencing time The subsequent consecutive time positions, For the system in Location at any given moment This represents the transformation from body coordinates to world coordinates, and is a rotation matrix. For the system in The angle of orientation at any moment The initial velocity within the bodyframe of the object. The initial acceleration of the object within its own system. For constant jerk, For self-time Start of experiencing time The subsequent continuous time attitude quaternion, For the system at time The posture quaternion, The angular velocity during that time period. To characterize the ontological coordinate system, The quaternion expression for angular acceleration. This is the timestamp between the current point and the nearest previous fused data. As shown in the formula above, the refined prediction equation has only one parameter. Therefore, it is possible to query the transformation within any time period, construct a continuous time trajectory to output the distortion-free point cloud, and then perform time interpolation and coordinate transformation on each laser point based on the continuous time motion trajectory to unify the point cloud to the same reference time coordinate system, thereby achieving point cloud distortion removal.

[0065] For example, the inertial data of the IMU and the wheel speed meter data can be time-aligned before data fusion.

[0066] Figure 3 This is a schematic diagram of data fusion in the multi-sensor fusion-based localization and mapping method provided by the present invention, as shown below. Figure 3 As shown, since the frequency of the IMU's inertial data is higher than that of the wheel speed meter data, interpolation can be used to embed the inertial data into the time series of the wheel speed meter data. Based on this, the fused inertial and wheel speed meter time series are timestamped with the acquired raw LiDAR data, thereby achieving data synchronization within the laser frame time range.

[0067] In some embodiments, a specific implementation of pose estimation is provided. Step S103 above may include the following steps: Step S1031: Calculate the point-to-surface residual based on the second laser point cloud to obtain the point-to-surface residual calculation result; Step S1032: Perform state estimation based on inertial data to obtain state estimation results; Step S1033: Measure the speed based on the wheel speed gauge data to obtain the speed measurement result; Step S1034: Based on the point-surface residual calculation results, state estimation results, and velocity measurement results, the Kalman filter method of iterative error state is used to estimate the pose of the target device to obtain the pose estimation result.

[0068] It should be noted that the embodiments of the present invention do not limit the order in which steps S1031, S1032 and S1033 are performed. Point and surface residual calculation can be performed first based on the second laser point cloud, state estimation can be performed first based on inertial data, or speed measurement can be performed first based on wheel speed meter data. The embodiments of the present invention are only illustrative examples and are not limited in this respect.

[0069] For example, Figure 4 This is the third flowchart of the localization and mapping method based on multi-sensor fusion provided by the present invention, as shown below. Figure 4As shown, the multi-source sensor observation data may include: the first laser point cloud observed by the lidar unit, the inertial data observed by the inertial measurement unit, and the wheel speed meter data observed by the wheel speed meter.

[0070] For the first laser point cloud observed by the lidar unit, the inertial data observed by the inertial measurement unit and the wheel speed data observed by the wheel speed meter are first fused together. Then, the motion compensation of the first laser point cloud is performed using the fused data. After that, the point-to-surface residual is calculated using the second laser point cloud after motion compensation. The result of the point-to-surface residual calculation is used as part of the input of the Kalman filtering method for the iterative error state.

[0071] For the inertial data obtained by the inertial measurement unit, the inertial data obtained by the inertial measurement unit and the wheel speed meter data obtained by the wheel speed meter are first fused together. Then, the fused data is used to perform state estimation to obtain the state estimation result, which is used as part of the input of the Kalman filter method for iterative error state.

[0072] The wheel speed data obtained from wheel speed meter observations can be directly used to measure speed, and the speed measurement results can be used as part of the input of the Kalman filter method for iterative error state.

[0073] Based on the above inputs, the Kalman filter method with iterative error state can be used to estimate the pose of the target device and obtain the pose estimation result.

[0074] The following explanation will illustrate the point cloud distortion correction process. Figure 5 This is a schematic diagram of point cloud distortion removal in the multi-sensor fusion-based localization and mapping method provided by the present invention, as shown below. Figure 5 As shown in the figure, the orange cube represents the current laser scan frame. The blue, red, and green coordinate axes represent the reference coordinate system for sensor pose estimation at different times. These represent the poses of the lidar at the start and end of the scan, respectively. This represents the motion state of the IMU at different time points. The entire process can be represented as uniformly projecting or correcting all distorted points (projection at point A) belonging to the same scan cycle onto the reference frame (orange square position), suppressing the stretching or compression of the point cloud caused by the carrier motion, thereby restoring the true spatial geometry of the feature points.

[0075] This can effectively reduce point cloud distortion caused by target device motion, improving the accuracy of subsequent mapping and positioning. In this embodiment, IESKF is used as the core state estimation framework for multi-sensor fusion. The process may include the following steps: Step 1-1: Establish a system state prediction model. Define system states including variables such as position, velocity, attitude, and IMU bias, and complete state prediction based on IMU pre-integration results.

[0076] Let the truth state of IESKF be: This state changing over time is denoted as:

[0077]

[0078]

[0079] in, This indicates the robot's position in the world coordinate system. express The result after differentiation For linear velocity, express The result after differentiation Here is the attitude rotation matrix. express The result after differentiation The cross product operator transforms the entire vector into a matrix. and These represent the zero bias of the accelerometer and gyroscope, respectively. and They represent and The result after differentiation The gravity vector The value is the accelerometer measurement. To measure noise with an accelerometer, To measure noise for the gyroscope, This refers to the zero-bias random walk noise of the gyroscope. This refers to the random walk noise of the accelerometer zero bias.

[0080] Steps 1-2: Construct a joint observation model. Point-to-surface geometric constraints are constructed using laser point clouds, while velocity and position observation information provided by wheel speedometers are incorporated to form a multi-source observation fusion model.

[0081] Specifically: The point-to-surface residual is constructed as follows, assuming the feature points in the lidar coordinate system are... Its coordinates in the world coordinate system are: ,in, The transformation matrix represents the relationship between the lidar and the world coordinate system. Then, five coordinates are found on the map. The corresponding planar feature points are used to fit the plane. , that is Solving for the normalized normal vector of the plane yields the solution. and intercept Therefore, the observation residual from a point to a plane is defined as: .

[0082] exist After performing first-order linearization, the corresponding observation Jacobian matrix is: .

[0083] After incorporating wheel speed meter information, the system introduces additional velocity observation constraints in three directions. Therefore, it is necessary to add partial derivatives of the velocity residuals with respect to the state variables to the original observation Jacobian matrix. The velocity residuals are constructed from the differences between velocities obtained by integrating the velocity measured by the wheel speed meter IMU. In the error state... Performing a first-order Taylor expansion on the observation model yields the linearized form: .

[0084] in, It is the speed measured by the wheel speedometer. It is the speed obtained by integrating the IMU and converting it to the speed in the wheel speedometer coordinate system. It is the observed Jacobian matrix.

[0085] Through derivation, we can obtain: , , It is a parameter in the state variables, representing the speed obtained by IMU integration.

[0086] Therefore, the Jacobian matrix of the observation model needs to include velocity constraint terms for the wheel speed gauge in three directions, and the corresponding Jacobian matrix is ​​expressed as follows: .

[0087] in, express speed The partial derivative of .

[0088] The overall observation matrix can be represented as: .

[0089] Steps 1-3 construct multiple residual functions, including point-to-surface residuals, velocity residuals, and position residuals, and use the residual terms to reflect the differences between observed and predicted values.

[0090] Steps 1-4 involve updating the system state using an iterative error state Kalman filter method. In each iteration, the error state is linearized and the nominal state is updated, resulting in a more convergent and numerically stable state estimate. The covariance matrix during the iteration is as follows: .

[0091] in, For the first The original state error covariance matrix at the next iteration For the first The Jacobian matrix of the state transformation function corresponding to the next iteration.

[0092] The state iteration update formula can be written as: .

[0093] in, For the first State estimate after the next iteration For the first The state estimate of the next iteration. Here is the Kalman gain matrix. For the first Measurement residuals / news at time.

[0094] In some embodiments, a specific implementation of constructing a map of the space where the target device is located is provided. The map of the space where the target device is located may include a three-dimensional voxel map, and correspondingly, step S104 above may include the following steps: Step S1041: The OctoMap method is used to perform voxel representation of the space where the target device is located, and the voxel representation result of the space where the target device is located is obtained. Step S1042: Determine the occupancy probability of each voxel in the voxelized representation result using a log-odds model. Step S1043: Update the voxelization representation results based on the occupancy probability of each voxel, and use the updated voxelization representation results as a three-dimensional occupancy voxel map.

[0095] In some embodiments, another specific implementation of constructing a map of the space where the target device is located is provided. The map of the space where the target device is located may also include a two-dimensional occupancy grid map; after step S1043 above, the following steps may also be included: Step S1044: Project the three-dimensional occupancy voxel map along the height direction to generate a two-dimensional occupancy grid map. In the two-dimensional occupied grid map, if there is an occupied voxel within the corresponding height range in each two-dimensional grid, the grid is determined to be an obstacle area.

[0096] Specifically, Figure 6 This is a schematic diagram of a three-dimensional occupancy voxel map in the localization and mapping method based on multi-sensor fusion provided by the present invention, as shown below. Figure 6The diagram illustrates the hierarchical spatial partitioning process of an octree. The octree uses a recursive approach to partition the three-dimensional space: first, the entire space is taken as the root node; when the spatial information contained within a node is complex, it is evenly divided into eight child nodes along the three coordinate axes; then, the child nodes are recursively subdivided until the set resolution requirement is met or the termination condition is reached.

[0097] Figure 7 This is a schematic diagram of the process for determining the occupying voxel in the localization and mapping method based on multi-sensor fusion provided by the present invention, as shown below. Figure 7 The diagram illustrates the update process of an occupied raster map based on log-odds representation. First, the raster cell coordinates are input. Read the current log-odds value of this unit. Then, based on the sensor observation results, the observation type of the grid is determined, which can be either occupied or idle. If the observation type is occupied, the corresponding occupancy increment is increased. That is, the updated If the observation type is idle, then increase the corresponding idle increment. That is, the updated After increasing the increment, further restrict the range of log-odds to make... Then restore the occupancy probability. .

[0098] Specifically, the following steps are included: Step 2-1: Construct an occupancy voxel map based on the optimized 3D point cloud data. The OctoMap method is used to represent the space in voxels, and the voxel occupancy probability is recursively updated using a log-odds model to obtain a stable 3D environment representation.

[0099] Step 2-2: Project the 3D voxel map along the height direction to generate a 2D occupied grid map. In each 2D grid, if there is an occupied voxel within the corresponding height range, the grid is determined to be an obstacle area, thus obtaining a 2D map suitable for navigation.

[0100] In some embodiments, a specific implementation is provided for integrating data observed separately by multiple sensors. Step S101 above may include the following steps: Step S1011: Obtain the candidate laser point cloud collected by the lidar unit in the coordinate system where the lidar unit is located; Step S1012: Obtain candidate inertial data collected by the inertial measurement unit in the coordinate system where the inertial measurement unit is located; Step S1013: Obtain candidate wheel speed meter data collected by the wheel speed meter in the coordinate system where the wheel speed meter is located; In step S1014, the candidate laser point cloud, candidate inertial data, and candidate wheel speed meter data are converted to the reference coordinate system of the target device, and after performing time synchronization, the first laser point cloud, inertial data, and wheel speed meter data are obtained.

[0101] It should be noted that the embodiments of the present invention do not limit the order in which steps S1011, S1012 and S1013 are performed, that is, they do not limit the order in which data observed by multiple source sensors are acquired. The embodiments of the present invention are only illustrative and do not impose any restrictions.

[0102] The following example illustrates a localization and mapping method based on multi-sensor fusion, relating to the field of autonomous navigation and environmental perception technology for mobile robots. Specifically, it relates to a localization and mapping method and system based on multi-sensor fusion of lidar, inertial measurement unit, and wheel velocity sensor in degraded environments, falling within the scope of multi-sensor fusion SLAM technology. This method mainly involves the following aspects: In a first aspect, embodiments of the present invention provide a multi-sensor fusion positioning and mapping system, comprising: 1) Data acquisition and calibration module; 2) Front-end fusion and correction module; 3) Fusion state estimation module; 4) Map building and projection module.

[0103] Specifically, such as Figure 4 As shown, the data acquisition and calibration module can acquire observation data from multiple sensors, including LiDAR, IMU, and wheel speedometer. During system initialization, the spatial extrinsic parameter relationships between the sensors are calibrated, and time synchronization is performed based on timestamps to align the data from different sensors in time and space, and unify them into the robot coordinate system or world coordinate system.

[0104] For the front-end fusion and correction module, motion compensation processing is performed on the raw point cloud data. First, the IMU and wheel velocity meter data are fused to construct unified angular velocity and linear velocity information, which is used to reduce the cumulative effect of IMU integration error. Then, distortion correction processing is performed on the point cloud based on the fused information to improve the consistency and accuracy of the point cloud data.

[0105] For the fusion state estimation module, a unified state estimation model is constructed based on multi-sensor observation information. Combining the geometric constraints of laser point cloud and the motion constraints of wheel velocity meter, the robot pose is continuously optimized and updated to obtain a stable pose estimation result.

[0106] For the map building projection module, the optimized 3D point cloud data is used to build a 3D occupied voxel map, and then a 2D occupied raster map is generated through height projection to meet the needs of path planning and navigation tasks.

[0107] Secondly, embodiments of the present invention provide a front-end optimized fusion motion compensation method, which includes the following steps: Step 3-1: Align the IMU and wheel speed meter time; Step 3-2: Constructing integrated information; Step 3-3: Pre-integration and trajectory estimation; Steps 3-4: Point cloud motion compensation and distortion correction.

[0108] Specifically, such as Figure 3 As shown, for step 3-1, the inertial data of the IMU and the wheel speed meter data are time-aligned. Since the frequency of the inertial data is higher than that of the wheel speed meter data, an interpolation method is used to embed the inertial data into the time series of the wheel speed meter data, and data synchronization is completed within the laser frame time range.

[0109] For step 3-2, the angular velocity information provided by the IMU and the linear velocity information provided by the wheel speedometer are fused to construct a unified motion information representation (i.e., fused data) to improve the stability of motion estimation. The fusion process can be referred to the implementation details in the above embodiments, and will not be repeated here.

[0110] For step 3-3, a pre-integral model is constructed based on the fused data, the state changes within the scanning period are integrally calculated, and a continuous-time motion trajectory model is established. For specific details, please refer to the implementation details in the above embodiments, which will not be repeated here.

[0111] For steps 3-4, based on the continuous time motion trajectory, time interpolation and coordinate transformation are performed on each laser point to unify the point cloud to the same reference time coordinate system, thereby realizing the point cloud distortion removal process.

[0112] Thirdly, embodiments of the present invention provide a multi-sensor tightly coupled positioning method based on IESKF, which includes the following steps: Step 4-1: Predictive model construction; Step 4-2: Construction of the joint observation model; Step 4-3: Construction of multiple residuals; Step 4-4: Iterative error state Kalman filter update.

[0113] Specifically, for step 4-1, a system state prediction model can be established, defining system states including variables such as position, velocity, attitude, and IMU bias, and state prediction can be completed based on the IMU pre-integration results.

[0114] For step 4-2, a joint observation model is constructed. Point-to-surface geometric constraints are constructed using laser point clouds, while velocity and position observation information provided by wheel speedometers are introduced to form a multi-source observation fusion model.

[0115] For step 4-3, construct multiple residual functions, including point-to-surface residuals, velocity residuals, and position residuals, and use the residual terms to reflect the difference between the observed and predicted values.

[0116] For step 4-4, the iterative error state Kalman filter method is used to update the system state. In each iteration, the error state is linearized and the nominal state is updated, thereby obtaining a more convergent and numerically stable state estimation result.

[0117] Fourthly, embodiments of the present invention provide a method for constructing a three-dimensional voxel map, projecting a two-dimensional raster, and conducting experimental verification, which includes the following steps: Step 5-1: Voxel map construction and update; Step 5-2: Two-dimensional raster projection; Step 5-3: Experimental verification under degraded conditions; Step 5-4: Dataset comparison and verification.

[0118] Specifically, such as Figure 6 and Figure 7 As shown, for step 5-1, an occupancy voxel map can be constructed based on the optimized 3D point cloud data. The OctoMap method is used to represent the space in voxels, and the voxel occupancy probability is recursively updated using a log-odds model, thereby obtaining a stable 3D environment representation.

[0119] For step 5-2, the 3D voxel map is projected along the height direction to generate a 2D occupied grid map. In each 2D grid, if there is an occupied voxel within the corresponding height range, the grid is determined to be an obstacle area, thus obtaining a 2D map suitable for navigation.

[0120] For step 5-3, static and dynamic experiments were conducted to verify the results under a degraded environment. Figure 8 This is a schematic diagram illustrating the localization performance of the FAST-LIO2 method in a degraded environment. Figure 9 This is a schematic diagram illustrating the localization effect of the multi-sensor fusion-based localization and mapping method provided by this invention in a degraded environment. Figure 10 This is a schematic diagram illustrating the mapping effect of the FAST-LIO2 method in a degraded environment. Figure 11 This is a schematic diagram illustrating the mapping effect of the multi-sensor fusion-based localization and mapping method provided by the present invention in a degraded environment.

[0121] like Figure 8As shown, the vehicle's pose in the RViz exhibits significant jitter and drift. The main reason for this is the lack of geometric features in the degraded environment, making some pose degrees of freedom, especially translation and heading, unobservable under lidar observation. In this situation, FAST-LIO2 primarily relies on IMU integration for state propagation. However, the IMU has zero bias error and is sensitive to noise, causing the error to accumulate over time, ultimately resulting in significant drift.

[0122] In comparison, Figure 9 The LVO-fusion algorithm shown exhibits higher stability under the same conditions. When laser observation degrades, this method introduces a wheel velocity sensor as an additional constraint, providing effective motion information in the planar direction. The forward velocity observation of the wheel velocity sensor and the nonholonomic constraint compensate for the lack of laser information, thus significantly improving the stability and continuity of pose estimation. Experimental results show that this method still possesses reliable positioning performance in degraded environments.

[0123] contrast Figure 10 and Figure 11 It can be clearly observed that in the mapping results of FAST-LIO2, the area marked by the red box shows obvious map overlap and misalignment. This indicates that in degraded environments such as long corridors, due to insufficient constraints on forward displacement and heading angle by the lidar, the system gradually accumulates errors during movement, ultimately leading to map distortion.

[0124] In the mapping results generated by the LVO-fusion algorithm, the overall map structure is more continuous and regular, with no obvious overlap or misalignment. This is due to the continuous constraint of the vehicle's planar motion state by the wheel speed sensor information during dynamic movement, which effectively suppresses the drift problem in the forward displacement and heading angle directions.

[0125] For steps 5-4, mapping accuracy was compared and verified on public datasets. LVO-fusion achieved superior positioning accuracy and global consistency across all sequences. In the Urban16 sequence with frequent turns, its ATE (Absolute Trajectory Error), RPE (Relative Pose Error), and drift rate were reduced by approximately 29%, 22%, and 33% respectively compared to FAST-LIO2, while the closed-loop error was reduced by approximately 37%, indicating that multi-sensor constraints can effectively suppress the cumulative error caused by rapid attitude changes. In the Urban09 sequence with long straight roads and tall buildings obstructing the view, LVO-fusion achieved improvements of over 30% in ATE, drift rate, and closed-loop error, demonstrating that its fusion strategy significantly enhances system robustness and global consistency in geometrically degraded and observationally limited scenarios. For the short-range Urban15 sequence containing sharp turns, although both methods had relatively small errors, LVO-fusion still maintained a performance advantage of approximately 20%–35%, demonstrating stability under conditions of intense motion. It should be noted that the aforementioned accuracy improvement comes at the cost of some computational overhead; the single-frame runtime of LVO-fusion increases by approximately 30%–40% compared to FAST-LIO2. Overall, FAST-LIO2 has an advantage in real-time performance, while LVO-fusion performs better in terms of accuracy and long-term stability in complex urban scenarios.

[0126] The method provided in this embodiment of the invention has at least the following beneficial effects: 1) By introducing wheel velocity measurement motion constraints and tightly coupling and fusing them with lidar and IMU, the observability of the system in geometrically degraded environments such as corridors and tunnels is enhanced, pose drift is effectively suppressed, and the stability and continuity of positioning results are improved.

[0127] 2) By constructing a front-end motion compensation model based on the fusion information of IMU and wheel speed meter, and combining pre-integration and continuous time trajectory estimation, fine distortion removal processing of laser point cloud was achieved, which improved the accuracy of point cloud correction and the consistency of map construction.

[0128] 3) By establishing a unified state estimation framework based on iterative error state Kalman filtering, and jointly utilizing the geometric constraints of laser point clouds, the speed constraints of wheel speedometers, and the position constraints for state updates, the error accumulation caused by step-by-step processing in loosely coupled methods is avoided, and the robustness and global consistency in complex environments are improved.

[0129] 4) By constructing a three-dimensional occupied voxel map and projecting it along the height direction to generate a two-dimensional occupied raster map, the ability to express the three-dimensional environment is preserved, and the applicability of the map to path planning, obstacle avoidance and navigation tasks is improved.

[0130] 5) Through comparative verification using degraded environment experiments and publicly available datasets, it is shown that the proposed method has good application value in terms of positioning accuracy, mapping quality, and engineering applications.

[0131] It should be noted that the steps shown in the above process or in the flowchart of the accompanying figures can be executed in a computer system such as a set of computer-executable instructions, and 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.

[0132] This invention also provides a localization and mapping device based on multi-sensor fusion, which is used to implement the above embodiments and preferred embodiments; details already described will not be repeated. The terms "module," "unit," and "subunit," etc., used below refer to combinations of software and / or hardware that implement a predetermined function. Although the device described in the following embodiments is preferably implemented in software, hardware implementation, or a combination of software and hardware, is also possible and contemplated. The localization and mapping device based on multi-sensor fusion provided by this invention will be described below; the localization and mapping device based on multi-sensor fusion described below corresponds to the localization and mapping method based on multi-sensor fusion described above.

[0133] Figure 12 This is a schematic diagram of the positioning and mapping device based on multi-sensor fusion provided by the present invention, as shown below. Figure 12 As shown, the multi-sensor fusion-based localization and mapping device 1200 includes: The acquisition module 1201 is used to acquire multi-source sensor observation data of the target device; wherein, the multi-source sensor observation data includes a first laser point cloud observed by the lidar unit, inertial data observed by the inertial measurement unit, and wheel speed meter data observed by the wheel speed meter; The motion compensation module 1202 is used to perform motion compensation on the first laser point cloud based on inertial data and wheel speed meter data to obtain the compensated second laser point cloud. The pose estimation module 1203 is used to estimate the pose of the target device based on the second laser point cloud, inertial data and wheel speed meter data, and obtain the pose estimation result. Module 1204 is used to construct a map of the space where the target device is located based on the pose estimation results.

[0134] In some embodiments, the motion compensation module 1202 is specifically used for: Inertial data and wheel speed meter data are fused to obtain fused data; the fused data is used to characterize the state changes of the target device during the scanning cycle. Motion compensation is performed on the first laser point cloud using fused data to obtain the compensated second laser point cloud.

[0135] In some embodiments, the motion compensation module 1202 is further specifically used for: The state changes of the target device during the scanning cycle are integrated to obtain the integral result; Based on the integration results, construct the continuous-time motion trajectory of the target device; Based on the continuous time motion trajectory, time interpolation and coordinate transformation are performed on each laser point cloud in the first laser point cloud to obtain the second laser point cloud.

[0136] In some embodiments, the pose estimation module 1203 is specifically used for: Point-to-surface residuals are calculated based on the second laser point cloud to obtain the calculation results. State estimation is performed based on inertial data to obtain state estimation results; Speed ​​measurement is performed based on wheel speed gauge data to obtain speed measurement results; Based on the point-to-surface residual calculation results, state estimation results, and velocity measurement results, the Kalman filter method of iterative error state is used to estimate the pose of the target device, and the pose estimation results are obtained.

[0137] In some embodiments, the map of the space where the target device is located includes a three-dimensional voxel map; the construction module 1204 is specifically used for: The OctoMap method is used to perform a voxel representation of the space where the target device is located, and the voxel representation result of the space where the target device is located is obtained. The probability of each voxel being occupied in the voxelized representation is determined by using a log-odds model. The voxelization representation is updated based on the occupancy probability of each voxel, and the updated voxelization representation is used as a 3D occupancy voxel map.

[0138] In some embodiments, the construction module 1204 is further specifically used for: The three-dimensional occupied voxel map is projected along the height direction to generate a two-dimensional occupied raster map; In the two-dimensional occupied grid map, if there is an occupied voxel within the corresponding height range in each two-dimensional grid, the grid is determined to be an obstacle area.

[0139] In some embodiments, the acquisition module 1201 is specifically used for: Obtain candidate laser point clouds collected by the lidar unit in the coordinate system where the lidar unit is located; Acquire candidate inertial data collected by the inertial measurement unit in the coordinate system in which the inertial measurement unit is located; Acquire candidate wheel speed meter data collected by the wheel speed meter in the coordinate system where the wheel speed meter is located; The candidate laser point cloud, candidate inertial data, and candidate wheel speed meter data are converted to the reference coordinate system of the target device, and after performing time synchronization, the first laser point cloud, inertial data, and wheel speed meter data are obtained.

[0140] It should be noted that the above modules can be functional modules or program modules, and can be implemented through software or hardware. For modules implemented through hardware, the above modules can reside in the same processor; or the above modules can be located in different processors in any combination.

[0141] Figure 13 This is a schematic diagram of the physical structure of an electronic device provided by the present invention, such as... Figure 13 As shown, the electronic device may include: a processor 1310, a communications interface 1320, a memory 1330, and a communication bus 1340, wherein the processor 1310, the communications interface 1320, and the memory 1330 communicate with each other via the communication bus 1340. The processor 1310 can call logical instructions in the memory 1330 to execute a multi-sensor fusion localization and mapping method, which includes: Acquire multi-source sensor observation data of the target device; wherein, the multi-source sensor observation data includes the first laser point cloud observed by the lidar unit, the inertial data observed by the inertial measurement unit, and the wheel speed meter data observed by the wheel speed meter; Based on inertial data and wheel speed meter data, motion compensation is performed on the first laser point cloud to obtain the compensated second laser point cloud; Based on the second laser point cloud, inertial data, and wheel speed meter data, the pose of the target device is estimated, and the pose estimation result is obtained. Based on the pose estimation results, a map of the space where the target device is located is constructed.

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

[0143] The present invention also provides an electronic device including a memory and a processor, the memory storing a computer program and the processor being configured to run the computer program to perform the steps in any of the above method embodiments.

[0144] Optionally, the electronic device may further include a transmission device and an input / output device, wherein the transmission device is connected to the processor and the input / output device is connected to the processor.

[0145] Optionally, in this embodiment, the processor can be configured to perform the following steps via a computer program: Step 1: Acquire multi-source sensor observation data of the target device; wherein, the multi-source sensor observation data includes the first laser point cloud observed by the lidar unit, the inertial data observed by the inertial measurement unit, and the wheel speed meter data observed by the wheel speed meter.

[0146] Step 2: Based on inertial data and wheel speed meter data, perform motion compensation on the first laser point cloud to obtain the compensated second laser point cloud.

[0147] Step 3: Based on the second laser point cloud, inertial data, and wheel speed meter data, perform pose estimation on the target device to obtain the pose estimation result.

[0148] Step 4: Based on the pose estimation results, construct a map of the space where the target device is located.

[0149] It should be noted that the specific examples in this embodiment can refer to the examples described in the above embodiments and optional implementations, and will not be repeated in this embodiment.

[0150] On the other hand, in conjunction with the multi-sensor fusion-based localization and mapping method provided in the above embodiments, the present invention also provides a non-transitory computer-readable storage medium storing a computer program, which, when executed by a processor, implements the steps in any of the above embodiments of the multi-sensor fusion-based localization and mapping method.

[0151] In another aspect, in conjunction with the multi-sensor fusion-based localization and mapping method provided in the above embodiments, the present invention also provides a computer program product, which includes a computer program that, when executed by a processor, implements the steps in any of the above method embodiments.

[0152] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without any creative effort.

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

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

Claims

1. A localization and mapping method based on multi-sensor fusion, characterized in that, include: Acquire multi-source sensor observation data of the target device; wherein, the multi-source sensor observation data includes a first laser point cloud observed by a lidar unit, inertial data observed by an inertial measurement unit, and wheel speed meter data observed by a wheel speed meter; Based on the inertial data and the wheel speed meter data, motion compensation is performed on the first laser point cloud to obtain the compensated second laser point cloud; Based on the second laser point cloud, the inertial data, and the wheel speed meter data, the pose of the target device is estimated to obtain the pose estimation result. Based on the pose estimation results, a map of the space where the target device is located is constructed.

2. The localization and mapping method based on multi-sensor fusion according to claim 1, characterized in that, The step of performing motion compensation on the first laser point cloud based on the inertial data and the wheel speedometer data to obtain the compensated second laser point cloud includes: The inertial data and the wheel speed meter data are fused to obtain fused data; wherein, the fused data is used to characterize the state changes of the target device during the scanning cycle; Motion compensation is performed on the first laser point cloud using the fused data to obtain the compensated second laser point cloud.

3. The localization and mapping method based on multi-sensor fusion according to claim 2, characterized in that, The step of performing motion compensation on the first laser point cloud using the fused data to obtain a compensated second laser point cloud includes: The state changes of the target device during the scanning cycle are integrated to obtain the integral result; Based on the integration results, a continuous-time motion trajectory corresponding to the target device is constructed; Based on the continuous time motion trajectory, time interpolation and coordinate transformation are performed on each laser point cloud in the first laser point cloud to obtain the second laser point cloud.

4. The localization and mapping method based on multi-sensor fusion according to any one of claims 1 to 3, characterized in that, The step of performing pose estimation on the target device based on the second laser point cloud, the inertial data, and the wheel velocity meter data to obtain pose estimation results includes: Point-to-surface residuals are calculated based on the second laser point cloud to obtain the calculation results. State estimation is performed based on the inertial data to obtain the state estimation result; Speed ​​measurement is performed based on the wheel speed gauge data to obtain the speed measurement result; Based on the point-to-surface residual calculation results, the state estimation results, and the velocity measurement results, the pose estimation of the target device is performed using the Kalman filter method with iterative error state, and the pose estimation results are obtained.

5. The localization and mapping method based on multi-sensor fusion according to any one of claims 1 to 3, characterized in that, The map of the space where the target device is located includes a three-dimensional voxel map; The step of constructing a map of the space where the target device is located based on the pose estimation result includes: The OctoMap method is used to perform a voxel representation of the space where the target device is located, and the voxel representation result of the space where the target device is located is obtained. The occupancy probability of each voxel in the voxelized representation result is determined by using a log-odds model. The voxelization representation is updated based on the occupancy probability of each voxel, and the updated voxelization representation is used as the three-dimensional occupancy voxel map.

6. The localization and mapping method based on multi-sensor fusion according to claim 5, characterized in that, The map of the space where the target device is located also includes a two-dimensional occupancy grid map; after using the updated voxelized representation as the three-dimensional occupancy voxel map, the method further includes: The three-dimensional occupied voxel map is projected along the height direction to generate the two-dimensional occupied grid map; In the two-dimensional occupied grid map, if there is an occupied voxel within the corresponding height range in each two-dimensional grid, the grid is determined to be an obstacle area.

7. The localization and mapping method based on multi-sensor fusion according to any one of claims 1 to 3, characterized in that, The acquisition of multi-source sensor observation data of the target device includes: Obtain the candidate laser point cloud collected by the lidar unit in the coordinate system where the lidar unit is located; Acquire candidate inertial data collected by the inertial measurement unit in the coordinate system where the inertial measurement unit is located; Acquire candidate wheel speed data collected by the wheel speed meter in the coordinate system where the wheel speed meter is located; The candidate laser point cloud, the candidate inertial data, and the candidate wheel speed meter data are respectively converted to the reference coordinate system of the target device, and after performing a time synchronization operation, the first laser point cloud, the inertial data, and the wheel speed meter data are obtained.

8. A positioning and mapping device based on multi-sensor fusion, characterized in that, include: The acquisition module is used to acquire multi-source sensor observation data of the target device; wherein, the multi-source sensor observation data includes a first laser point cloud observed by the lidar unit, inertial data observed by the inertial measurement unit, and wheel speed meter data observed by the wheel speed meter; The motion compensation module is used to perform motion compensation on the first laser point cloud based on the inertial data and the wheel speed meter data to obtain the compensated second laser point cloud. The pose estimation module is used to perform pose estimation on the target device based on the second laser point cloud, the inertial data and the wheel speed meter data, and obtain the pose estimation result. A construction module is used to construct a map of the space where the target device is located based on the pose estimation results.

9. An electronic device comprising a memory and a processor, characterized in that, The memory stores a computer program, and the processor is configured to run the computer program to perform the localization and mapping method based on multi-sensor fusion as described in any one of claims 1 to 7.

10. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the localization and mapping method based on multi-sensor fusion as described in any one of claims 1 to 7.

Images