Robot and multi-sensor-based three-dimensional mapping method, storage medium

Abstract

The application discloses a kind of robots, the robot is equipped with laser radar, odometer and inertial measurement sensor, three sensors collect data at preset frequency, the robot further includes memory that is stored with executable program code, and processor for implementing the following steps when the executable code is executed: the first pre-integral corresponding to each node is calculated according to first data, the second pre-integral corresponding to each node is calculated according to second data;According to the first pre-integral and the second pre-integral, the laser radar target state variable corresponding to each node is calculated;According to the target pose corresponding to each node from laser radar target state variable, according to the target pose, map is constructed.The pre-integral is calculated after the data alignment of multiple sensors in the application, and then the state variable of laser radar is calculated according to the pre-integral, more accurate pose can be obtained, and the mapping precision is improved.

Description

Technical Field

[0001] The embodiments of the present invention relate to the field of three-dimensional positioning technology, specifically to a robot and a three-dimensional mapping method and storage medium based on multiple sensors. Background Technology

[0002] Currently, robots need to be located during operation; the prerequisite for this is that a map of the entire environment must be built in advance. Therefore, before performing localization, the robot needs to create a 3D map of the entire environment.

[0003] Currently, pre-mapping-based solutions primarily rely on LiDAR sensors. For outdoor robots, due to relatively open environments and significant variations in lighting conditions, 3D LiDAR is often used for mapping. LiDAR mapping often depends on environmental geometric features for registration; however, in some open, low-texture areas, the environment's texture representation is weak, easily leading to inaccurate mapping or even failure. To address this, some solutions employ tightly coupled IMU-integrated mapping. Specifically, this involves mapping through IMU pre-integration constraints, which can improve mapping accuracy to some extent. However, the position obtained from accelerometer integration during IMU pre-integration may drift, especially when the IMU is stationary.

[0004] To address these issues, some robot localization companies use data collected from cameras, IMUs, and odometry, and then process it to locate the robot. While this significantly improves the accuracy of the localization, the entire process is cumbersome and slow. Summary of the Invention

[0005] In view of the above problems, embodiments of the present invention provide a robot to solve the problem of low mapping accuracy in the prior art.

[0006] According to one aspect of the present invention, a robot is provided, the robot being equipped with a lidar, an odometer, and an inertial measurement sensor. The inertial measurement sensor acquires multiple frames of first data at a first preset frequency, the odometer acquires multiple frames of second data at a second preset frequency, and the lidar acquires multiple frames of third data at a third preset frequency. Multiple nodes are created, and the creation time of each of the multiple nodes is consistent with the time of acquiring each frame of third data. The first preset frequency is greater than the second preset frequency, and the second preset frequency is greater than the third preset frequency. The robot includes a memory and a processor. The memory stores executable program code, and the processor is used to call and execute the executable code to perform the following steps:

[0007] Based on the acquisition time of each frame of the first data in the multi-frame first data, the first data between the corresponding times of each two adjacent nodes in all nodes is pre-integrated to obtain the first pre-integrated quantity for each node in all nodes.

[0008] Based on the acquisition time of each frame of the first data in the multi-frame first data, the second data between the corresponding times of each two adjacent nodes in all nodes is pre-integrated to obtain the second pre-integrated quantity corresponding to each node in all nodes.

[0009] Based on the first pre-integral quantity and the second pre-integral quantity corresponding to each of all nodes, the target state variable of the lidar corresponding to each of all nodes is calculated;

[0010] Based on the multi-frame third data, pose data is extracted from the lidar target state variables corresponding to each of all nodes to obtain the target pose corresponding to each of all nodes;

[0011] A map is constructed based on the target pose of each node and the third data.

[0012] Optionally, the step of calculating the lidar target state variable corresponding to each of the nodes based on the first pre-integral quantity and the second pre-integral quantity corresponding to each of the nodes includes:

[0013] The target state variable of each node in all nodes is calculated based on the preset state variable of the lidar corresponding to the earliest time node among all nodes, the first pre-integral quantity and the second pre-integral quantity corresponding to each node in all nodes.

[0014] Optionally, the step of extracting pose data from the lidar target state variables corresponding to each of the nodes based on the third data to obtain the target pose corresponding to each of the nodes includes:

[0015] The target state variables of the corresponding LiDAR nodes are solved based on the first pre-integral quantity, the second pre-integral quantity, the third data, and the pre-constructed sliding window for each node in all nodes, so as to obtain the target pose for each node in all nodes.

[0016] Optionally, the step of solving for the target state variables of the corresponding LiDAR nodes in all nodes based on the first pre-integral quantity, the second pre-integral quantity, the third data, and the pre-constructed sliding window for each node in all nodes to obtain the target pose corresponding to each node in all nodes includes:

[0017] Calculate the first residual obtained by feature matching for the third data corresponding to each node;

[0018] Calculate the second residual formed by the first pre-integral quantity and the lidar state variables for each node;

[0019] Calculate the third residual formed by the second pre-integral quantity and the lidar state variables for each node;

[0020] Marginalized residuals are calculated using a pre-constructed sliding window based on the Shure complement;

[0021] Based on the first residual, the second residual, the third residual, and the marginalized residual, the pose of each node is extracted to obtain the target pose corresponding to each node among all the nodes.

[0022] Optionally, based on the acquisition time of each frame of second data in the multi-frame second data, the step of pre-integrating the second data between corresponding times of every two adjacent nodes in all nodes to obtain the second pre-integrated quantity corresponding to each node in all nodes includes:

[0023] Based on the acquisition time of each frame of the second data in the multi-frame second data, and based on the linear velocity measurement value in the second data between the corresponding times of each two adjacent nodes in all nodes, the position pre-integration quantity in the second pre-integration quantity corresponding to each node in all nodes is calculated;

[0024] Based on the acquisition time of each frame of the second data in the multi-frame second data, and based on the gyroscope measurement value in the first data between the corresponding times of each two adjacent nodes in all nodes, synchronize the angular velocity measurement value in the second data between the corresponding times of each two adjacent nodes in all nodes.

[0025] Based on the angular velocity measurement values ​​in the second data between each pair of adjacent nodes in all nodes, the pre-integral quantity representing rotation in the second pre-integral quantity corresponding to each of the nodes is calculated.

[0026] Optionally, the step of synchronizing the angular velocity measurement values ​​in the second data between each pair of adjacent nodes based on the gyroscope measurement values ​​in the first data between each pair of adjacent nodes in all nodes includes:

[0027] Obtain the raw angular velocity measurement value from the second data between each pair of adjacent nodes in all the nodes;

[0028] The original angular velocity measurement value is aligned and adjusted based on the gyroscope measurement value in the first data between the corresponding times of every two adjacent nodes in all nodes, to obtain the angular velocity measurement value.

[0029] Optionally, based on the acquisition time of each frame of the first data in the multi-frame first data, the first data between corresponding times of every two adjacent nodes in all nodes is pre-integrated to obtain the first pre-integrated quantity corresponding to each node in all nodes:

[0030] Based on the acquisition time of each frame of the first data in the multi-frame first data, and based on the acceleration measurement value in the first data between each two adjacent nodes in all nodes, the pre-integral quantity representing position and the pre-integral quantity representing velocity in the first pre-integral quantity corresponding to each node in all nodes are calculated, including;

[0031] Based on the gyroscope measurements in the first data between each two adjacent nodes in all nodes, the pre-integral quantity representing rotation in the first pre-integral quantity corresponding to each of the nodes is calculated.

[0032] According to another aspect of the present invention, a three-dimensional mapping method based on multiple sensors is provided, characterized in that the method includes:

[0033] Data is collected from a lidar, an odometer, and an inertial measurement sensor. The inertial measurement sensor collects data at a first preset frequency to obtain multiple frames of first data. The odometer collects data at a second preset frequency to obtain multiple frames of second data. The lidar collects data at a third preset frequency to obtain multiple frames of third data. Multiple nodes are created, and the creation time of each node is consistent with the time when each frame of third data is acquired. The first preset frequency is greater than the second preset frequency, and the second preset frequency is greater than the third preset frequency.

[0034] Based on the acquisition time of each frame of the first data in the multi-frame first data, the first data between the corresponding times of each two adjacent nodes in all nodes is pre-integrated to obtain the first pre-integrated quantity for each node in all nodes.

[0035] Based on the acquisition time of each frame of the first data in the multi-frame first data, the second data between the corresponding times of each two adjacent nodes in all nodes is pre-integrated to obtain the second pre-integrated quantity corresponding to each node in all nodes.

[0036] Based on the first pre-integral quantity and the second pre-integral quantity corresponding to each of all nodes, the target state variable of the lidar corresponding to each of all nodes is calculated;

[0037] Based on the multi-frame third data, pose data is extracted from the lidar target state variables corresponding to each of all nodes to obtain the target pose corresponding to each of all nodes;

[0038] A map is constructed based on the target pose of each node and the third data.

[0039] According to another aspect of the present invention, a multi-sensor-based three-dimensional mapping device is provided. The device includes multiple functional modules, which are stored in the memory of a robot and, when executed by the robot's processor, implement the multi-sensor-based three-dimensional mapping method as described above.

[0040] According to another aspect of the present invention, a computer-readable storage medium is provided having a computer program stored thereon, which, when executed by a processor, implements the aforementioned multi-sensor-based three-dimensional mapping method.

[0041] This invention integrates data from lidar, odometer, and inertial measurement sensor, aligns these data, calculates the pre-integration of the odometer and inertial measurement sensor, and then calculates the state variables of lidar based on the pre-integration. This results in a more accurate pose and improved mapping accuracy.

[0042] The above description is merely an overview of the technical solutions of the embodiments of the present invention. In order to better understand the technical means of the embodiments of the present invention and to implement them in accordance with the contents of the specification, and to make the above and other objects, features and advantages of the embodiments of the present invention more apparent and understandable, specific embodiments of the present invention are described below. Attached Figure Description

[0043] The accompanying drawings are for illustrative purposes only and are not intended to limit the invention. Furthermore, the same reference numerals denote the same parts throughout the drawings. In the drawings:

[0044] Figure 1 The diagram shows an optimized front-end sliding window in one embodiment of the present invention. Detailed Implementation

[0045] Exemplary embodiments of the invention will now be described in more detail with reference to the accompanying drawings. While exemplary embodiments of the invention are shown in the drawings, it should be understood that the invention can be implemented in various forms and should not be limited to the embodiments set forth herein.

[0046] This invention discloses a robot equipped with a lidar, an odometer, and an inertial measurement sensor. The inertial measurement sensor acquires multiple frames of first data at a first preset frequency, the odometer acquires multiple frames of second data at a second preset frequency, and the lidar acquires multiple frames of third data at a third preset frequency. Multiple nodes are created, and the creation time of each node is consistent with the time of acquiring each frame of third data. The first preset frequency is greater than the second preset frequency, and the second preset frequency is greater than the third preset frequency. The robot includes a memory and a processor. The memory stores executable program code, and the processor is used to call and execute the executable code to achieve the following steps:

[0047] Based on the acquisition time of each frame of first data in multiple frames of first data, the first data between the corresponding times of each two adjacent nodes in all nodes is pre-integrated to obtain the first pre-integrated quantity for each node in all nodes.

[0048] Based on the acquisition time of each frame of second data in multiple frames of second data, the second data between the corresponding times of each two adjacent nodes in all nodes is pre-integrated to obtain the second pre-integrated quantity corresponding to each node in all nodes.

[0049] Based on the first pre-integral quantity and the second pre-integral quantity corresponding to each of all nodes, the target state variable of the lidar corresponding to each of all nodes is calculated;

[0050] Based on multiple frames of third data, pose data is extracted from the LiDAR target state variables corresponding to each of all nodes to obtain the target pose corresponding to each of all nodes.

[0051] A map is constructed based on the target pose of each node and the third data.

[0052] This invention integrates data from LiDAR, odometer, and inertial measurement sensor, aligns these data, calculates the pre-integrated values ​​of the odometer and inertial measurement sensor, and then calculates the state variables of the LiDAR based on the pre-integrated values. This results in a more accurate pose and improved mapping accuracy. To facilitate the alignment of data from multiple sensors, multiple nodes are created, each corresponding one-to-one with each frame of third data in multiple frames. The creation time of each node is consistent with the time when each frame of third data is acquired.

[0053] In one optional approach, each frame of first data includes acceleration measurements and gyroscope measurements. Based on the acquisition time of each frame of first data across multiple frames, the step of pre-integrating the first data between corresponding times of every two adjacent nodes in all nodes to obtain the first pre-integrated quantity corresponding to each node includes: calculating the pre-integrated quantity representing the position in the first pre-integrated quantity corresponding to each node based on the acquisition time of each frame of first data across multiple frames and the acceleration measurements in the first data between corresponding times of every two adjacent nodes in all nodes. and the pre-integral quantity representing velocity And based on the gyroscope measurements in the first data between each pair of adjacent nodes in all nodes, the pre-integral quantity representing rotation in the first pre-integral quantity corresponding to each of the all nodes is calculated.

[0054] In a specific embodiment of the present invention, the first pre-integral quantity is calculated using the following formula:

[0055]

[0056] In the formula, l and l+1 represent the times when the two inertial measurement units (IMUs) collect the first data between the i-th and j-th times of two adjacent nodes, respectively. The node created at the i-th time is earlier than the node created at the j-th time. The formula is recursively applied from l=i to l=j-1. a and ω are the acceleration measurement value of the IMU and the gyroscope measurement value, respectively. Thus, the first pre-integral quantity corresponding to the node with the later creation time among the two adjacent nodes is obtained by pre-integrating the first data between the corresponding times of any two adjacent nodes.

[0057] In one optional approach, each frame of second data includes linear velocity measurements and angular velocity measurements; based on the acquisition time of each frame of second data in multiple frames of second data, the step of pre-integrating the second data between corresponding times of every two adjacent nodes in all nodes to obtain the second pre-integrated quantity corresponding to each node in all nodes includes:

[0058] Based on the acquisition time of each frame of second data in multiple frames of second data, and the linear velocity measurement value in the second data between each pair of adjacent nodes in all nodes, the position pre-integral quantity in the second pre-integral quantity corresponding to each of the all nodes is calculated. Based on the gyroscope measurements in the first data between each pair of adjacent nodes across all nodes, synchronize the angular velocity measurements in the second data between each pair of adjacent nodes across all nodes; based on the angular velocity measurements in the second data between each pair of adjacent nodes across all nodes, calculate the pre-integral quantity representing rotation in the second pre-integral quantity corresponding to each of the aforementioned nodes.

[0059] In a specific embodiment of the present invention, the odometer is a wheel-type odometer, and the second pre-integral quantity is calculated using the following formula:

[0060]

[0061] In the formula, l and l+1 represent the times when the wheel odometers collect the second data between the i-th and j-th times corresponding to the two nodes, respectively. The node created at the i-th time is earlier than the node created at the j-th time. The formula is recursively applied from l=i to l=j-1. v is the measured linear velocity of the wheel odometer, and ω is the measured angular velocity. Thus, by pre-integrating the second data between any two adjacent nodes, the second pre-integrated quantity corresponding to the node with the later creation time among the two adjacent nodes is obtained.

[0062] In one alternative approach, synchronizing the angular velocity measurements in the second data between each pair of adjacent nodes based on the gyroscope measurements in the first data between each pair of adjacent nodes at corresponding times includes:

[0063] Based on the acquisition time of each frame of second data in multiple frames of second data, the original angular velocity measurement value in the second data between corresponding times of each two adjacent nodes in all nodes is obtained;

[0064] Based on the acquisition time of each frame of the second data in the multi-frame second data, and based on the gyroscope measurement value in the first data between the corresponding times of each two adjacent nodes in all nodes, the original angular velocity measurement value is aligned and adjusted to obtain the angular velocity measurement value.

[0065] In one optional approach, calculating the lidar target state variable corresponding to each of the all nodes based on the first pre-integral value and the second pre-integral value corresponding to each of the all nodes includes:

[0066] The target state variable of each node in all nodes is calculated based on the preset state variable of the lidar corresponding to the earliest time node among all nodes, the first pre-integral quantity and the second pre-integral quantity corresponding to each node in all nodes.

[0067] In existing technologies, when calculating the target state variables of a lidar, without knowing the first and second pre-integral values ​​corresponding to each node, it is impossible to determine the predicted value of the lidar state variable for each node. This requires extensive iterative calculations to solve for the target state variables, a very lengthy process. In this invention, the predicted state variables for each node are calculated based on the preset state variables of the earliest node, the first and second pre-integral values, and then used as initial values ​​for calculation. This significantly reduces the computational load and shortens the calculation time for the lidar target state variables. Furthermore, for ease of calculation, all variables in the preset state variables of the earliest node are set to their first preset values.

[0068] In one optional approach, the step of calculating the predicted state variable of the lidar corresponding to each node based on the preset state variable of the lidar corresponding to the earliest time point among all nodes and the first pre-integral quantity and the second pre-integral quantity corresponding to each node among all nodes includes:

[0069] Obtain the pre-integral quantity representing velocity from the first pre-integral quantity corresponding to the current node. and the pre-integral quantity representing rotation And the position pre-integral quantity in the second pre-integral quantity corresponding to the current node. Let the current node's lidar target state variable be: The following formula is used to calculate the predicted state variable of the lidar corresponding to the next node of the current node.

[0070]

[0071] In the formula, g w Let represent the gravity vector in the world coordinate system, which is a known quantity; the p-component represents the position of each node in the world coordinate system, the v-component represents the velocity of each node in the world coordinate system, the q-component represents the rotation of each node in the world coordinate system, and b... a and b w These represent the acceleration measurement value from the inertial measurement sensor and the gyroscope measurement value, respectively.

[0072] In one optional approach, the step of extracting pose data from the lidar target state variables corresponding to each of the all nodes based on the third data to obtain the target pose corresponding to each of the all nodes includes:

[0073] The target pose of each node is obtained by solving the LiDAR prediction state variables of the corresponding nodes in all nodes based on the first pre-integral quantity, the second pre-integral quantity, the third data, and the pre-constructed sliding window corresponding to each node in all nodes.

[0074] Specifically, the lidar target state variables for each of the nodes are represented as follows:

[0075] χ=[χ0,χ1,…,χ K-1 ];

[0076]

[0077] Where the size of the sliding window is K, χ n This represents the state of the preset lidar target state variable corresponding to the nth node within the sliding window. The schematic diagram of the sliding window is shown below. Figure 1 As shown.

[0078] In an optional embodiment of the present invention, the step of solving the lidar prediction state variables corresponding to the corresponding nodes in all nodes based on the first pre-integral quantity, the second pre-integral quantity, the third data, and the pre-constructed sliding window for each node in all nodes to obtain the target pose corresponding to each node in all nodes includes:

[0079] Calculate the first residual obtained by feature matching of the third data corresponding to each node; calculate the second residual formed by the first pre-integral and the lidar state variables corresponding to each node; calculate the third residual formed by the second pre-integral and the lidar state variables corresponding to each node; calculate the marginalized residual calculated based on the Shure complement using a pre-constructed sliding window; extract the pose of each node using the following formula based on the first residual, second residual, third residual, and marginalized residual, to obtain the target pose corresponding to each node among all nodes:

[0080] χ * =argmin{∑ρ(‖r L || 2 )+∑‖r B || 2 +∑‖r o || 2 +||r p || 2};

[0081] In the formula, χ * It is the target pose, r L Let ρ() be the first residual, and r be the robust kernel function. B It is the second residual, r o It is the third residual, r p It is a marginalized residual.

[0082] In a more specific embodiment of the present invention, the second residual r B Calculate using the following formula:

[0083]

[0084] In the formula, [] xyz This represents a three-dimensional vector composed of the imaginary parts of quaternions.

[0085] In a more specific embodiment of the present invention, the third residual r o Calculate using the following formula:

[0086]

[0087] Finally, in this embodiment of the invention, a map is constructed based on the target pose corresponding to each of all nodes.

[0088] This invention provides a multi-sensor-based 3D mapping method, the method comprising:

[0089] Data is collected from a lidar, an odometer, and an inertial measurement sensor. The inertial measurement sensor collects data at a first preset frequency to obtain multiple frames of first data. The odometer collects data at a second preset frequency to obtain multiple frames of second data. The lidar collects data at a third preset frequency to obtain multiple frames of third data. Multiple nodes are created, and the creation time of each node is consistent with the time when each frame of third data is acquired. The first preset frequency is greater than the second preset frequency, and the second preset frequency is greater than the third preset frequency.

[0090] Based on the acquisition time of each frame of first data in multiple frames of first data, the first data between the corresponding times of each two adjacent nodes in all nodes is pre-integrated to obtain the first pre-integrated quantity for each node in all nodes.

[0091] Based on the acquisition time of each frame of first data in multiple frames of first data, the second data between the corresponding times of each two adjacent nodes in all nodes is pre-integrated to obtain the second pre-integrated quantity corresponding to each node in all nodes.

[0092] Based on the first pre-integration quantity and the second pre-integration quantity corresponding to each of the nodes, the target state variable of the lidar corresponding to each of the nodes is calculated.

[0093] Based on multiple frames of third data, pose data is extracted from the LiDAR target state variables corresponding to each of all nodes to obtain the target pose corresponding to each of all nodes.

[0094] A map is constructed based on the target pose corresponding to each of all nodes.

[0095] This invention also provides a multi-sensor-based 3D mapping device. The 3D mapping device can be a mini-program or an APP, and includes multiple functional modules. The multiple functional modules are stored in the robot's memory, and when the robot's processor executes them, they implement the multi-sensor-based 3D mapping method described above.

[0096] The invention also provides a computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements the multi-sensor-based 3D mapping method described above.

[0097] This invention integrates data from lidar, odometer, and inertial measurement sensor, aligns these data, calculates the pre-integration of the odometer and inertial measurement sensor, and then calculates the state variables of lidar based on the pre-integration. This results in a more accurate pose and improved mapping accuracy.

[0098] This invention provides a computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements the aforementioned multi-sensor-based 3D mapping method.

[0099] The integration of a wheeled odometer and an IMU (Integrated Device Unit) forms a new odometer. In applications where high precision is not required, the calculated pose can even be used directly as an odometer without the need for a wheeled odometer. Simultaneously, the tightly coupled optimization of the wheeled odometer, IMU, and LiDAR at the front end compensates for the limitations of a single sensor, further improving the accuracy of front-end odometer estimation. The LiDAR can be either a 3D or 2D LiDAR.

[0100] The algorithms or displays provided herein are not inherently related to any particular computer, virtual system, or other device. Various general-purpose systems can also be used in conjunction with the teachings herein. The required structure for constructing such systems is apparent from the above description. Furthermore, the embodiments of the present invention are not directed to any particular programming language. It should be understood that the content of the invention described herein can be implemented using various programming languages, and the above description of specific languages ​​is for the purpose of disclosing the best mode of implementation of the invention.

[0101] Numerous specific details are set forth in the specification provided herein. However, it will be understood that embodiments of the invention may be practiced without these specific details. In some instances, well-known methods, structures, and techniques have not been shown in detail so as not to obscure the understanding of this specification.

[0102] Similarly, it should be understood that, in order to simplify the invention and aid in understanding one or more of the various inventive aspects, features of the embodiments of the invention are sometimes grouped together in a single embodiment, figure, or description thereof in the above description of exemplary embodiments of the invention. However, this disclosure should not be construed as reflecting an intention that the claimed invention requires more features than are expressly recited in each claim. Rather, as reflected in the following claims, inventive aspects lie in fewer than all features of a single foregoing disclosed embodiment. Therefore, the claims following the detailed description are hereby expressly incorporated into this detailed description, wherein each claim itself is a separate embodiment of the invention.

[0103] Those skilled in the art will understand that modules in the device of the embodiments can be adaptively changed and placed in one or more devices different from that embodiment. Modules, units, or components in the embodiments can be combined into a single module, unit, or component, and further, they can be divided into multiple sub-modules, sub-units, or sub-components. Except where at least some of such features and / or processes or units are mutually exclusive, any combination can be used to combine all features disclosed in this specification (including the accompanying claims, abstract, and drawings) and all processes or units of any method or device so disclosed. Unless expressly stated otherwise, each feature disclosed in this specification (including the accompanying claims, abstract, and drawings) may be replaced by an alternative feature that serves the same, equivalent, or similar purpose.

[0104] Furthermore, those skilled in the art will understand that although some embodiments herein include certain features included in other embodiments but not others, combinations of features from different embodiments are intended to be within the scope of the invention and form different embodiments. For example, in the following claims, any of the claimed embodiments can be used in any combination.

[0105] It should be noted that the above embodiments are illustrative of the invention and not restrictive, and that those skilled in the art can devise alternative embodiments without departing from the scope of the appended claims. In the claims, any reference signs placed between parentheses should not be construed as limiting the claims. The word "comprising" does not exclude the presence of elements or steps not listed in the claims. The word "a" or "an" preceding an element does not exclude the presence of a plurality of such elements. The invention can be implemented by means of hardware comprising several different elements and by means of a suitably programmed computer. In the unit claims enumerating several means, several of these means may be embodied by the same item of hardware. The use of the words first, second, and third, etc., does not indicate any order. These words can be interpreted as names. The steps in the above embodiments, unless otherwise specified, should not be construed as limiting the order of execution.

Claims

1. A robot, characterized by: The robot is provided with a laser radar, an odometer and an inertial measurement sensor, the inertial measurement sensor collects multiple frames of first data at a first preset frequency, the odometer collects multiple frames of second data at a second preset frequency, the laser radar collects multiple frames of third data at a third preset frequency, multiple nodes are created, and the creation time of each node in the multiple nodes is consistent with the time of obtaining each frame of third data in the multiple frames of third data, the first preset frequency is greater than the second preset frequency, and the second preset frequency is greater than the third preset frequency, the robot comprises a memory and a processor, the memory stores executable program code, and the processor is used to call and execute the executable program code to implement the following steps: According to the acquisition time of each frame of first data in the multiple frames of first data, the first data between each adjacent two nodes in all nodes is pre-integrated to obtain the first pre-integrated quantity corresponding to each node in the all nodes, According to the acquisition time of each frame of second data in the multiple frames of second data, the second data between each adjacent two nodes in the all nodes is pre-integrated to obtain the second pre-integrated quantity corresponding to each node in the all nodes; According to the laser radar preset state variable corresponding to the node with the earliest time in the all nodes, the first pre-integrated quantity and the second pre-integrated quantity corresponding to each node in the all nodes, the laser radar prediction state variable corresponding to each node in the all nodes is calculated; The first residual of the third data corresponding to each node obtained through feature matching is calculated; The second residual formed by the first pre-integrated quantity and the laser radar prediction state variable corresponding to each node is calculated; The third residual formed by the second pre-integrated quantity and the laser radar prediction state variable corresponding to each node is calculated; The marginalized residual calculated by the sliding window pre-constructed according to the Schur complement is used; According to the first residual, the second residual, the third residual and the marginalized residual, the pose of each node is extracted to obtain the target pose corresponding to each node in the all nodes; According to the target pose corresponding to each node in the all nodes and the third data, a map is constructed.

2. The robot of claim 1, wherein: According to the acquisition time of each frame of second data in the multiple frames of second data, the second data between each adjacent two nodes in the all nodes is pre-integrated to obtain the second pre-integrated quantity corresponding to each node in the all nodes, which comprises: According to the acquisition time of each frame of second data in the multiple frames of second data, and according to the linear velocity measurement value in the second data between each adjacent two nodes in the all nodes, the position pre-integrated quantity corresponding to each node in the second pre-integrated quantity of the all nodes is calculated; According to the acquisition time of each frame of second data in the multiple frames of second data, and according to the acquisition time of each frame of second data in the multiple frames of second data, the angular velocity measurement value in the second data between each adjacent two nodes in the all nodes is synchronized according to the gyroscope measurement value in the first data between each adjacent two nodes in the all nodes. According to the angular velocity measurement in the second data between the corresponding time of each adjacent two nodes in all nodes, a pre-integral quantity representing rotation corresponding to each node in all nodes is calculated.

3. The robot of claim 2, wherein: The synchronization of the angular velocity measurement in the second data between the corresponding time of each adjacent two nodes in all nodes according to the gyro measurement in the first data between the corresponding time of each adjacent two nodes in all nodes includes: The original angular velocity measurement in the second data between the corresponding time of each adjacent two nodes in all nodes is obtained. The original angular velocity measurement is adjusted according to the gyro measurement in the first data between the corresponding time of each adjacent two nodes in all nodes to obtain the angular velocity measurement.

4. The robot of claim 1, wherein: According to the acquisition time of each frame of first data in the plurality of frames of first data, the pre-integration of the first data between the corresponding time of each adjacent two nodes in all nodes is performed to obtain a first pre-integral quantity corresponding to each node in all nodes. According to the acquisition time of each frame of first data in the plurality of frames of first data, and according to the acceleration measurement in the first data between the corresponding time of each adjacent two nodes in all nodes, a pre-integral quantity representing position and a pre-integral quantity representing velocity in the first pre-integral quantity corresponding to each node in all nodes are calculated. According to the gyro measurement in the first data between the corresponding time of each adjacent two nodes in all nodes, a pre-integral quantity representing rotation in the first pre-integral quantity corresponding to each node in all nodes is calculated.

5. A multi-sensor based three-dimensional mapping method, characterized by, The method comprises: Collecting data of a laser radar, an odometer and an inertial measurement sensor, the inertial measurement sensor performing data collection at a first preset frequency to obtain a plurality of frames of first data, the odometer performing data collection at a second preset frequency to obtain a plurality of frames of second data, the laser radar performing data collection at a third preset frequency to obtain a plurality of frames of third data, a plurality of nodes are created, and the creation time of each node in the plurality of nodes is consistent with the time of obtaining each frame of third data in the plurality of frames of third data, the first preset frequency is greater than the second preset frequency, and the second preset frequency is greater than the third preset frequency; According to the acquisition time of each frame of first data in the plurality of frames of first data, the pre-integration of the first data between the corresponding time of each adjacent two nodes in all nodes is performed to obtain a first pre-integral quantity corresponding to each node in all nodes. According to the acquisition time of each frame of first data in the plurality of frames of first data, the pre-integration of the first data between the corresponding time of each adjacent two nodes in all nodes is performed to obtain a first pre-integral quantity corresponding to each node in all nodes. According to the laser radar preset state variable corresponding to the node at the earliest time in all nodes, the first pre-integral quantity and the second pre-integral quantity corresponding to each node in all nodes, a laser radar prediction state variable corresponding to each node in all nodes is calculated. A first residual of the third data corresponding to each node obtained through feature matching is calculated. A second residual formed by the first pre-integral quantity and the laser radar prediction state variable corresponding to each node is calculated. calculating a third residual formed by a second pre-integrated quantity corresponding to each node and a lidar predicted state variable; calculating an edge residual according to a pre-constructed sliding window and a Schur complement; extracting a pose of each node according to the first residual, the second residual, the third residual and the edge residual to obtain a target pose corresponding to each node in all nodes; constructing a map according to the target pose corresponding to each node in all nodes and the third data.

6. A multi-sensor based three-dimensional mapping device, characterized by The device includes a plurality of functional modules stored in the memory of the robot and, when executed by the processor of the robot, implements the multi-sensor-based three-dimensional mapping method of claim 5.

7. A computer-readable storage medium, characterized in that, A computer program is stored thereon, and when executed by a processor, implements the multi-sensor-based three-dimensional mapping method of claim 5.

Images