A fast laser inertial three-dimensional odometer, implementation method and robot

Abstract

The present invention provides a fast laser inertial three-dimensional odometer, which comprises the following units: an IMU unit for acquiring IMU data; a three-dimensional laser unit for acquiring three-dimensional lidar point cloud data; a three-dimensional lidar frame and corresponding IMU data acquisition unit for acquiring the current three-dimensional lidar frame Scan_i+1 and the three-dimensional lidar point cloud frame Scan_i of the moment before the current frame, and acquiring the current frame IMU data IMU_j and the corresponding frame IMU data IMU_i corresponding to the times of Scan_i and Scan_i+1; a three-dimensional lidar displacement acquisition unit for acquiring the current frame laser line scan point cloud P_0_i+1 at a specific angle in Scan_i and Scan_i+1, and the laser line scan point cloud P_0_i of the moment before the current frame; and calculating the displacement S between P_0_i and P_0_i+1 by using the ICP method; a laser inertial odometry acquisition unit for acquiring the pose of the current frame according to the displacement S, the pose of the moment before the current frame, and the attitude data of the IMU. The present invention only uses a beam of laser scan data, thereby reducing the amount of data used for ICP registration, improving the calculation speed, and using the IMU data to perform decomposition from horizontal two-dimensional to three-dimensional directions, so as to realize a fast laser inertial odometer.

Description

Technical Field

[0001] The present invention relates to the technical field of robots. Specifically, it relates to a fast laser inertial three-dimensional odometer, an implementation method, and a robot. Background Art

[0002] Patrol robots need to conduct autonomous patrols within the working scenario. The prerequisite for realizing this ability is that the mobile robot has high-precision SLAM capabilities. Since lidar has high precision and can work all-weather compared to cameras, the lidar-based SLAM system is undoubtedly more accurate and stable than other systems. As the front-end part of laser SLAM, the laser odometer plays a key role in the robustness of laser SLAM.

[0003] 3D lidar, i.e., multi-line lidar, is divided into 16-line lidar, 32-line lidar, 64-line lidar, and 128-line lidar according to the number of lines. The number of lines of lidar indicates how many laser rays the lidar can emit simultaneously. A single-line lidar is generally called a 2D lidar, which generally only emits one ray parallelly.

[0004] The existing laser odometer technologies mainly include Scan To Scan point cloud registration and Scan To SubMap point cloud registration. Both of these registration methods are time-consuming and do not meet the requirement of being fast.

[0005] The background description provided in this article is for the purpose of presenting the context of the present disclosure generally. Unless otherwise indicated herein, the materials described in this section are not prior art to the claims of this application and should not be admitted as prior art by including them in this section. Summary of the Invention

[0006] In view of the above technical problems in the related art, the present invention proposes a method for implementing a fast laser inertial three-dimensional odometer, which includes the following steps:

[0007] S1. Obtain the current frame Scan_i+1 of the 3D lidar and the point cloud frame Scan_i of the 3D lidar at the moment before the current frame, and obtain the current frame IMU data IMU_j and the corresponding frame IMU data IMU_i corresponding to the current frame Scan_i of the 3D lidar and the point cloud frame Scan_i+1 of the 3D lidar at the moment before the current frame;

[0008] S2. Obtain the current frame laser line scan point cloud P_0_i+1 and the laser line scan point cloud P_0_i at the moment before the current frame at a specific angle in the Scan_i and Scan_i+1; and calculate the displacement S between the P_0_i and P_0_i+1 by using the ICP method;

[0009] S3. Obtain the pose of the current frame based on the displacement S, the pose at the moment before the current frame, and the attitude data of the IMU.

[0010] Specifically, before step S1, it further includes: S0. Obtain IMU data and 3D lidar point cloud data.

[0011] Specifically, the specific angle is a vertical angle of 0 degrees.

[0012] Specifically, use the following formula to extract the point cloud scanned by the laser line with a vertical angle of 0 degrees, denoted as P_0_i and P_0_i+1 respectively;

[0013] ,

[0014] vert_ang = 0,

[0015] where p is the point scanned by the laser, p.x is the coordinate in the forward direction of the lidar, and p.y is the coordinate in the leftward direction of the lidar.

[0016] Specifically, step S3 is specifically: construct a rotation matrix based on the attitude data of the IMU , through the rotation matrix Decompose the displacement S and add it to the pose at the moment before the current frame to obtain the pose of the current frame.

[0017] In a second aspect, another embodiment of the present invention discloses a fast laser-inertial 3D odometer, which includes the following units:

[0018] An IMU unit for obtaining IMU data;

[0019] A 3D laser unit for obtaining 3D lidar point cloud data;

[0020] A 3D lidar frame and corresponding IMU data acquisition unit for obtaining the current 3D lidar frame Scan_i+1 and the 3D lidar point cloud frame Scan_i at the moment before the current frame, and obtaining the current frame IMU data IMU_j and the corresponding frame IMU data IMU_i corresponding to the time of the current 3D lidar frame Scan_i and the 3D lidar point cloud frame Scan_i+1 at the moment before the current frame;

[0021] A 3D lidar displacement acquisition unit for obtaining the current frame laser line scan point cloud P_0_i+1 at a specific angle in Scan_i and Scan_i+1, and the laser line scan point cloud P_0_i at the moment before the current frame; and calculating the displacement S between P_0_i and P_0_i+1 using the ICP method;

[0022] The laser inertial odometry acquisition unit acquires the pose of the current frame based on the displacement S, the pose at the moment before the current frame, and the attitude data of the IMU.

[0023] Specifically, the specific angle is a vertical angle of 0 degrees.

[0024] Specifically, the point cloud of the laser line scan with a vertical angle of 0 degrees is extracted using the following formula, denoted as P_0_i and P_0_i+1 respectively;

[0025] ,

[0026] vert_ang = 0,

[0027] where p is the point scanned by the laser, p.x is the coordinate in the forward direction of the lidar, and p.y is the coordinate in the leftward direction of the lidar.

[0028] Specifically, the laser inertial odometry acquisition unit is specifically: constructing a rotation matrix based on the attitude data of the IMU , through the rotation matrix decompose the displacement S and add it to the pose at the moment before the current frame to obtain the pose of the current frame.

[0029] In a third aspect, another embodiment of the present invention discloses a robot, which includes a chassis and also includes the fast laser inertial 3D odometer as described above.

[0030] In a fourth aspect, another embodiment of the present invention discloses a non-volatile memory, on which instructions are stored, and when the instructions are executed by a processor, they are used to implement the method for realizing fast laser inertial 3D odometry as described above.

[0031] The present invention only uses a beam of laser scan data (the 0-degree laser beam), thereby reducing the amount of data used for ICP registration, directly using this beam of laser data for Scan To Scan displacement calculation, improving the calculation speed, and performing decomposition from the horizontal two-dimensional direction to the three-dimensional direction according to the obtained horizontal displacement and using the IMU data, thereby realizing a fast laser inertial odometer. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required to be used in the embodiments. Obviously, the drawings in the following description are only some embodiments of the present invention, and for those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0033] Figure 1It is a flowchart of a method for implementing a fast laser inertial three-dimensional odometer provided by an embodiment of the present invention;

[0034] Figure 2 It is a schematic diagram of a fast laser inertial three-dimensional odometer provided by an embodiment of the present invention;

[0035] Figure 3 It is a schematic diagram of a device for implementing a method of a fast laser inertial three-dimensional odometer provided by an embodiment of the present invention. Specific embodiments

[0036] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art belong to the scope of protection of the present invention. Embodiment 1

[0037] The laser inertial three-dimensional odometer in this embodiment includes an IMU unit and a three-dimensional laser unit; specifically, the IMU unit can be independent of the three-dimensional laser unit or can be packaged together with the three-dimensional laser unit.

[0038] The laser inertial three-dimensional odometer in this embodiment is applied to a robot. This embodiment will be described by taking a discrete IMU and a three-dimensional lidar as an example.

[0039] The robot in this embodiment includes a main body and a chassis. An IMU unit and a three-dimensional lidar unit are installed on the main body, and the chassis is used to carry the main body and can support the robot to move. For example, wheels and drive motors are installed on the chassis.

[0040] This embodiment only introduces the components related to the laser inertial three-dimensional odometer. For other components used to implement the functions of the robot, such as a central processing unit, a robot operating system, a vision sensor, etc., this embodiment will not be elaborated.

[0041] Specifically, during the walking process of the robot, its IMU unit continuously collects IMU data. Generally, the IMU data has a relatively high frame rate, generally above 100HZ, and the collected IMU data is stored in the storage unit. In this embodiment, an IMU variable IMU_data is used to store the collected IMU data.

[0042] Similarly, during the walking process of the robot, its 3D lidar continuously acquires point cloud data. However, the scanning frame rate of the lidar is generally 10HZ. Generally, 2 frames of lidar scanning point cloud data are collected, and at the same time, 10 frames of IMU data can be collected. Specifically, in this embodiment, the point cloud data obtained by the 3D lidar is stored in the storage unit, or stored in a 3D laser point cloud scanning variable Scan.

[0043] Generally, the continuous variables in IMU_data and Scan represent the data collected in chronological order. For example, Scan_i, Scan_i+1, Scan_i represents the 3D lidar frame collected at time i, and Scan_i+1 represents the 3D lidar frame collected at time i+1. Among them, time i and time i+1 are consecutive times relative to the lidar acquisition time.

[0044] Reference Figure 1 , this embodiment provides a method for implementing fast lidar-inertial 3D odometry, which includes the following steps:

[0045] S1, obtain the current frame Scan_i+1 of the 3D lidar and the 3D lidar point cloud frame Scan_i of the moment before the current frame, and obtain the current frame IMU data IMU_j and the corresponding frame IMU data IMU_i corresponding to the time of the current frame Scan_i and the 3D lidar point cloud frame Scan_i+1 of the moment before the current frame;

[0046] Specifically, before step S1, it further includes: S0, obtain IMU data and 3D lidar point cloud data;

[0047] The current frame Scan_i+1 and the 3D lidar point cloud frame Scan_i of the moment before the current frame are two consecutive frames Scan_i, Scan_i+1.

[0048] The current frame IMU data IMU_j and the corresponding frame IMU data IMU_i are not consecutive relative to the sampling frequency of the IMU.

[0049] The IMU data has a faster frame rate, generally above 100HZ, while the scanning frame of the 3D lidar is generally 10HZ. Generally, 2 frames of lidar scanning point cloud data are collected, and at the same time, 10 frames of IMU data can be collected. In this example, two consecutive frames Scan_i, Scan_i+1 in the 3D lidar point cloud data are obtained. First, the IMU data IMU_j at the time point corresponding to Scan_i+1 is obtained.

[0050] For example, if Scan_0 represents the 3D lidar data obtained at the 0th moment (i.e., when the 3D lidar is started), then the IMU data at the moment of IMU_0 (i.e., when the IMU is started) is acquired at this time. Scan_1 represents the data at the 1st moment (for a 10HZ 3D lidar, it is the data at 100ms), then at this time, IMU_100 (corresponding to a 100HZ IMU, also the data at 100ms) needs to be acquired, and at this time, IMU-j is IMU_100.

[0051] In this embodiment, the time alignment between the IMU data and the 3D lidar data can be performed according to the sampling frequency of the IMU and the sampling frequency of the 3D lidar.

[0052] In this embodiment, the 3D lidar point cloud frame Scan_i at the moment immediately before the current frame already has corresponding pose data 。

[0053] S2. Obtain the current frame laser line scan point cloud P_0_i+1 and the laser line scan point cloud P_0_i at a specific angle in Scan_i and Scan_i+1; and calculate the displacement S between P_0_i and P_0_i+1 using the ICP method;

[0054] Specifically, the specific angle in this embodiment is a vertical angle of 0 degrees, that is, a horizontal scan line;

[0055] Specifically, in this embodiment, the following formula is used to extract the point cloud of the laser line scan with a vertical angle of 0 degrees, denoted as P_0_i and P_0_i+1 respectively;

[0056]

[0057] vert_ang = 0,

[0058] where p is the point scanned by the laser, and each point is represented as (x, y, z) in the 3D lidar coordinate system, where x represents the forward direction of the lidar, y represents the left direction of the lidar, and z represents the upward direction of the lidar; p.x is the coordinate in the forward direction of the lidar, and p.y is the coordinate in the left direction of the lidar.

[0059] S3. Obtain the pose of the current frame according to the displacement S, the pose at the moment immediately before the current frame, and the attitude data of the IMU;

[0060] Specifically, in this embodiment, the displacement S is decomposed onto the pose at the moment immediately before the current frame according to the attitude data of the IMU. The pose at the moment immediately before the current frame is a three-dimensional space, which includes data in three directions: x, y, and z.

[0061] In this embodiment, a rotation matrix is constructed according to the attitude data of the IMU , decompose the displacement S through the rotation matrix and add it to the pose at the moment before the current frame to obtain the pose of the current frame. The specific implementation method is as follows:

[0062] Combine the data of the IMU to decompose it into three-dimensional space to implement a three-dimensional lidar odometry. Then the three-dimensional odometry can be expressed as

[0063] ,

[0064] where odom-x-i, odom-y-i, and odom-z-i represent the previous pose data, that is, the pose at the moment before the current frame;

[0065] ,

[0066] where the output of the IMU is three-dimensional attitude angles, denoted as , which is the measurement at the corresponding IMU_i moment, where roll angle, pitch angle, yaw angle; rotation matrix.

[0067] In this embodiment, only a beam of laser scan data (the 0-degree laser beam line) is used, thereby reducing the amount of data used for ICP registration. Directly use this beam of laser data to calculate the displacement in Scan To Scan, improve the calculation speed, obtain the displacement in the current horizontal direction, and use the data of the IMU to decompose from the horizontal two-dimensional to the three-dimensional direction, thereby realizing a fast lidar inertial odometer. Embodiment 2

[0068] Refer to Figure 2 , this embodiment discloses a fast lidar inertial three-dimensional odometer, which includes the following units:

[0069] An IMU unit for obtaining IMU data;

[0070] A three-dimensional laser unit for obtaining three-dimensional lidar point cloud data;

[0071] The lidar inertial three-dimensional odometer of this embodiment includes an IMU unit and a three-dimensional laser unit; specifically, the IMU unit can be independent of the three-dimensional laser unit or packaged together with the three-dimensional laser unit.

[0072] The lidar inertial three-dimensional odometer of this embodiment is applied to a robot. This embodiment is described by taking a discrete IMU and a three-dimensional lidar as an example.

[0073] The robot in this embodiment includes a main body and a chassis. An IMU unit and a 3D lidar unit are installed on the main body. The chassis is used to carry the main body and support the robot to move. For example, wheels, drive motors, etc. are installed on the chassis.

[0074] This embodiment only introduces the components related to the laser inertial 3D odometer. For other components used to implement the functions of the robot, such as the central processing unit, robot operating system, vision sensor, etc., this embodiment will not elaborate.

[0075] Specifically, during the walking process of the robot, its IMU unit continuously collects IMU data. Generally, the IMU data has a relatively high frame rate, usually above 100HZ. The collected IMU data is stored in the storage unit. In this embodiment, an IMU variable IMU_data is used to store the collected IMU data.

[0076] Similarly, during the walking process of the robot, its 3D lidar also continuously obtains point cloud data. However, the scan frame of the lidar is generally 10HZ. Generally, 2 frames of lidar scan point cloud data are collected, and at the same time, 10 frames of IMU data can be collected. Specifically, in this embodiment, the point cloud data obtained by the 3D lidar is stored in the storage unit, or stored in a 3D laser point cloud scan variable Scan.

[0077] Generally, the continuous variables in IMU_data and Scan represent the data collected in chronological order. For example, Scan_i, Scan_i+1, Scan_i represent the 3D lidar frames collected at time i, and Scan_i+1 represents the 3D lidar frame collected at time i+1. Among them, time i and time i+1 are consecutive times relative to the lidar acquisition time.

[0078] The 3D lidar frame and the corresponding IMU data acquisition unit are used to acquire the current 3D lidar frame Scan_i+1 and the previous 3D lidar point cloud frame Scan_i of the current frame, and acquire the current frame IMU data IMU_j and the corresponding frame IMU data IMU_i corresponding to the time of the current 3D lidar frame Scan_i+1 and the previous 3D lidar point cloud frame Scan_i of the current frame;

[0079] The current frame Scan_i+1 and the previous 3D lidar point cloud frame Scan_i of the current frame are two consecutive frames Scan_i, Scan_i+1.

[0080] The current frame IMU data IMU_j and the corresponding frame IMU data IMU_i are not consecutive relative to the sampling frequency of the IMU.

[0081] The IMU data has a relatively high frame rate, generally above 100HZ, while the scan frame of the 3D lidar is generally 10HZ. Generally, when 2 frames of lidar scan point cloud data are collected, 10 frames of IMU data can be collected simultaneously. In this example, two consecutive frames Scan_i and Scan_i+1 of the 3D lidar point cloud data are obtained. First, the IMU data IMU_j at the time point corresponding to Scan_i+1 is obtained.

[0082] For example, if Scan_0 represents the 3D lidar data obtained at the 0th moment (i.e., when the 3D lidar starts), then the IMU data at the IMU_0 moment (i.e., when the IMU starts) is obtained at this time. Scan_1 represents the data at the 1st moment (for a 10HZ 3D lidar, it is the data at 100ms). Then, IMU_100 (corresponding to a 100HZ IMU and also the data at 100ms) needs to be obtained at this time, and IMU-j is IMU_100 at this time.

[0083] In this embodiment, the time alignment between the IMU data and the 3D lidar data can be performed according to the sampling frequency of the IMU and the sampling frequency of the 3D lidar.

[0084] In this embodiment, the 3D lidar point cloud frame Scan_i at the moment before the current frame already has corresponding pose data .

[0085] The 3D lidar displacement acquisition unit acquires the current frame laser line scan point cloud P_0_i+1 at a specific angle and the laser line scan point cloud P_0_i at the moment before the current frame in Scan_i and Scan_i+1; and calculates the displacement S between P_0_i and P_0_i+1 using the ICP method.

[0086] Specifically, the specific angle in this embodiment is that the vertical angle is 0 degrees, that is, the horizontal scan line;

[0087] Specifically, in this embodiment, the following formula is used to extract the point cloud of the laser line scan with a vertical angle of 0 degrees, which are respectively denoted as P_0_i and P_0_i+1;

[0088] ,

[0089] vert_ang = 0,

[0090] where p is the point scanned by the laser, and each point is represented as (x, y, z) in the 3D lidar coordinate system, where x represents the forward direction of the lidar, y represents the left direction of the lidar, and z represents the upward direction of the lidar; p.x is the coordinate in the forward direction of the lidar, and p.y is the coordinate in the left direction of the lidar.

[0091] The laser inertial odometry acquisition unit obtains the pose of the current frame based on the displacement S, the pose at the moment immediately before the current frame, and the attitude data of the IMU.

[0092] Specifically, in this implementation, the displacement S is decomposed onto the pose at the moment immediately before the current frame according to the attitude data of the IMU. The pose at the moment immediately before the current frame is a three-dimensional space, which includes data in the x, y, and z directions.

[0093] In this embodiment, a rotation matrix is constructed according to the attitude data of the IMU , and the displacement S is decomposed through the rotation matrix and added to the pose at the moment immediately before the current frame to obtain the pose of the current frame. The specific implementation method is as follows:

[0094] Combined with the data of the IMU, it is decomposed onto a three-dimensional space to implement a three-dimensional laser odometer. Then the three-dimensional odometer can be expressed as

[0095] ,

[0096] where odom-x-i, odom-y-i, and odom-z-i represent the previous pose data, that is, the pose at the moment immediately before the current frame;

[0097] ,

[0098] where the output of the IMU is a three-dimensional attitude angle, denoted as , which is the measurement at the corresponding IMU_i moment, where roll angle, pitch angle, yaw angle; rotation matrix.

[0099] In this embodiment, only a beam of laser scan data (the 0-degree laser beam) is used, thereby reducing the amount of data used for ICP registration. The displacement is directly calculated using this beam of laser data for Scan To Scan, improving the calculation speed. According to the obtained displacement in the current horizontal direction and using the data of the IMU, it is decomposed from the horizontal two-dimensional to the three-dimensional direction, thereby realizing a fast laser inertial odometer. Embodiment Three

[0100] This embodiment discloses a robot, which includes a fast laser inertial three-dimensional odometer and a chassis. Embodiment Four

[0101] Reference Figure 3 , Figure 3It is a schematic structural diagram of a device for implementing a method of a fast laser inertial three-dimensional odometer according to this embodiment. The device 20 for implementing the method of a fast laser inertial three-dimensional odometer according to this embodiment includes a processor 21, a memory 22, and a computer program stored in the memory 22 and executable on the processor 21. When the processor 21 executes the computer program, the steps in the above method embodiment are implemented. Alternatively, when the processor 21 executes the computer program, the functions of each module / unit in the above device embodiments are implemented.

[0102] Exemplarily, the computer program may be divided into one or more modules / units. The one or more modules / units are stored in the memory 22 and executed by the processor 21 to complete the present invention. The one or more modules / units may be a series of computer program instruction segments capable of performing specific functions, and these instruction segments are used to describe the execution process of the computer program in the device 20 for implementing the method of a fast laser inertial three-dimensional odometer. For example, the computer program may be divided into the respective modules in Embodiment 2. For the specific functions of each module, please refer to the working process of the device described in the above embodiment, and details are not described herein again.

[0103] The device 20 for implementing the method of a fast laser inertial three-dimensional odometer may include, but is not limited to, a processor 21 and a memory 22. Those skilled in the art can understand that the schematic diagram is only an example of the device 20 for implementing the method of a fast laser inertial three-dimensional odometer, and does not constitute a limitation on the device 20 for implementing the method of a fast laser inertial three-dimensional odometer. It may include more or fewer components than shown in the figure, or combine certain components, or different components. For example, the device 20 for implementing the method of a fast laser inertial three-dimensional odometer may further include input / output devices, network access devices, a bus, etc.

[0104] The processor 21 may be a Central Processing Unit (CPU), or may also be other general-purpose processors, Digital Signal Processors (DSPs), Application Specific Integrated Circuits (ASICs), Field-Programmable Gate Arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor, or the processor may also be any conventional processor, etc. The processor 21 is the control center of the device 20 for implementing the fast laser inertial three-dimensional odometry method, and connects various parts of the entire device 20 for implementing the fast laser inertial three-dimensional odometry method through various interfaces and lines.

[0105] The memory 22 can be used to store the computer programs and / or modules. The processor 21 realizes various functions of the device 20 for implementing the fast laser inertial three-dimensional odometry method by running or executing the computer programs and / or modules stored in the memory 22, and by calling the data stored in the memory 22. The memory 22 may mainly include a program storage area and a data storage area. Among them, the program storage area can store an operating system, application programs required for at least one function (such as a sound playback function, an image playback function, etc.); the data storage area can store data created according to the use of the mobile phone (such as audio data, phone book, etc.). In addition, the memory 22 may include high-speed random access memory, and may also include non-volatile memory, such as a hard disk, a memory, a plug-in hard disk, a Smart Media Card (SMC), a Secure Digital (SD) card, a Flash Card, at least one magnetic disk storage device, a flash memory device, or other volatile solid-state storage devices.

[0106] Among them, if the modules / cells integrated in the device 20 for implementing the method of fast laser inertial three-dimensional odometer are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on such an understanding, to implement all or part of the processes in the above-described embodiment methods of the present invention, it can also be completed by a computer program instructing relevant hardware. The computer program can be stored in a computer-readable storage medium. When the computer program is executed by the processor 21, the steps of the above-described method embodiments can be implemented. Among them, the computer program includes computer program code, and the computer program code can be in the form of source code, object code, executable file, or some intermediate form, etc. The computer-readable medium can include: any entity or device capable of carrying the computer program code, recording medium, USB flash drive, mobile hard disk, magnetic disk, optical disk, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signal, telecommunication signal, and software distribution medium, etc. It should be noted that the content included in the computer-readable medium can be appropriately increased or decreased according to the requirements of legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, the computer-readable medium does not include electrical carrier signals and telecommunication signals.

[0107] It should be noted that the device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separated. The components shown as units may or may not be physical units, that is, they may be located in one place or distributed to multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the solution of this embodiment. In addition, in the attached drawings of the device embodiments provided by the present invention, the connection relationship between the modules indicates that there is a communication connection between them, which can be specifically implemented as one or more communication buses or signal lines. Those of ordinary skill in the art can understand and implement it without creative efforts.

[0108] The above is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A method for implementing a fast laser inertial three-dimensional odometer, which comprises the following steps: S1. Obtain the current frame Scan_i+1 of the 3D lidar and the point cloud frame Scan_i of the 3D lidar at the moment before the current frame, and obtain the current frame IMU data IMU_j and the corresponding frame IMU data IMU_i corresponding to the time of the current frame Scan_i of the 3D lidar and the point cloud frame Scan_i+1 of the 3D lidar at the moment before the current frame; S2. Obtain the current frame laser line scan point cloud P_0_i+1 and the laser line scan point cloud P_0_i at the moment before the current frame at a specific angle in the Scan_i and Scan_i+1; and calculate the displacement S between the P_0_i and P_0_i+1 by using the ICP method; S3. Obtain the pose of the 3D lidar in the current frame according to the displacement S, the pose of the 3D lidar at the moment before the current frame, and the attitude data of the IMU. The specific steps of step S3 are as follows: construct a rotation matrix according to the attitude data of the IMU , and decompose the displacement S through the rotation matrix , and add it to the pose of the 3D lidar at the moment before the current frame to obtain the pose of the 3D lidar in the current frame, where is the roll angle, is the pitch angle, is the yaw angle.

2. The method according to claim 1 further includes, before step S1: S0. Obtain IMU data and 3D lidar point cloud data.

3. The method according to claim 1, wherein the specific angle is a vertical angle of 0 degrees.

4. The method according to claim 3, wherein the point clouds of the laser line scan with a vertical angle of 0 degrees are extracted by using the following formula and are respectively denoted as P_0_i and P_0_i+1; ， vert_ang = 0, where p is the point scanned by the laser, p.x is the coordinate in the forward direction of the lidar, p.y is the coordinate in the left direction of the lidar, and p.z is the coordinate in the vertical direction of the lidar, is the vertical angle.

5. A fast laser inertial three-dimensional odometer, which comprises the following units: An IMU unit for obtaining IMU data; A 3D laser unit for obtaining 3D lidar point cloud data; A 3D lidar frame and corresponding IMU data acquisition unit for obtaining the current frame Scan_i+1 of the 3D lidar and the point cloud frame Scan_i of the 3D lidar at the moment before the current frame, and obtaining the current frame IMU data IMU_j and the corresponding frame IMU data IMU_i corresponding to the time of the current frame Scan_i of the 3D lidar and the point cloud frame Scan_i+1 of the 3D lidar at the moment before the current frame; A 3D lidar displacement acquisition unit for obtaining the current frame laser line scan point cloud P_0_i+1 and the laser line scan point cloud P_0_i at the moment before the current frame at a specific angle in the Scan_i and Scan_i+1; and calculating the displacement S between the P_0_i and P_0_i+1 by using the ICP method; The laser inertial odometry acquisition unit obtains the pose of the current frame of the 3D lidar based on the displacement S, the pose of the previous moment of the current frame of the 3D lidar, and the attitude data of the IMU; specifically, the laser inertial odometry acquisition unit constructs a rotation matrix according to the attitude data of the IMU , and decomposes the displacement S through the rotation matrix , and adds it to the pose of the previous moment of the current frame of the 3D lidar to obtain the pose of the current frame of the 3D lidar, where is the roll angle, is the pitch angle, is the yaw angle.

6. The fast laser inertial three-dimensional odometer according to claim 5, wherein the specific angle is a vertical angle of 0 degrees.

7. The fast laser inertial three-dimensional odometer according to claim 6, wherein the point clouds of the laser line scan with a vertical angle of 0 degrees are extracted by using the following formula and are respectively denoted as P_0_i and P_0_i+1; ， vert_ang = 0, where p is the point scanned by the laser, p.x is the coordinate in the forward direction of the lidar, p.y is the coordinate in the left direction of the lidar, and p.z is the coordinate in the vertical direction of the lidar, is the vertical angle.

8. A robot, which comprises a chassis and further comprises the fast laser inertial three-dimensional odometer according to any one of claims 5-7.

Images