Laser slam method and system based on height information

Abstract

The application relates to a laser SLAM method and system based on height information. The method comprises the following steps: acquiring three-dimensional laser radar data, performing motion distortion and dynamic point filtering processing to obtain first point cloud data; acquiring robot IMU data, performing pre-integration processing to obtain first pose data; extracting features from the first point cloud data to obtain feature point cloud, recording the height of the feature point cloud, and obtaining laser radar odometry data according to the feature point cloud and the height of the feature point cloud; fusing the first pose data and the laser radar odometry data based on a factor graph to obtain second pose data as the robot pose; and determining corresponding point cloud data according to the second pose data and splicing to generate a three-dimensional point cloud map. The application can enable the robot to realize high-precision self-positioning and high-precision mapping in an unfamiliar cluster point scene.

Description

Technical Field

[0001] This invention relates to a laser SLAM method and system based on height information, belonging to the field of robotics technology. Background Technology

[0002] Robot localization and mapping technology is a key technology for mobile robots. Many important application scenarios, such as power inspection, outdoor security patrol, and earthquake damage detection, require robots to be able to autonomously locate and map in unfamiliar environments.

[0003] For example, when conducting triage at earthquake injury collection points, these points are typically chosen as relatively open areas, such as plazas, basketball courts, or football fields. This is because after an earthquake disaster, rescue personnel are often unable to reach the scene immediately, and most rescue efforts rely on self-rescue and mutual aid. Therefore, to prevent further injury to the injured, rescued individuals are placed in relatively open areas. However, traditional laser SLAM (Simultaneous Localization and Mapping) methods cannot reliably map and locate injuries at earthquake injury collection points. Summary of the Invention

[0004] This invention provides a laser SLAM method and system based on height information, aiming to solve at least one of the technical problems existing in the prior art.

[0005] The technical solution of this invention relates to a laser SLAM method based on height information, comprising the following steps:

[0006] S1. Acquire 3D LiDAR data, perform motion distortion and dynamic point filtering processing to obtain the first point cloud data;

[0007] S2. Acquire robot IMU data, perform pre-integration processing, and obtain the first pose data;

[0008] S3. Extract features from the first point cloud data to obtain a feature point cloud, and record the height of the feature point cloud;

[0009] S4. Obtain lidar odometer data based on the feature point cloud and the height of the feature point cloud;

[0010] S5. Based on the factor graph, the first pose data and the lidar odometry data are fused to obtain the second pose data as the robot pose.

[0011] S6. Determine the corresponding point cloud data based on the second pose data, and stitch them together to generate a three-dimensional point cloud map.

[0012] Further, step S1 includes:

[0013] S11. For the three-dimensional lidar data, second point cloud data is obtained by performing motion compensation on the robot;

[0014] S12. Obtain the difference between the second point cloud data in the current frame and the depth image data of the local map, and determine whether the second point cloud data in the current frame is a dynamic point based on the difference; the local map is a map generated by the robot during its movement based on the first point cloud data in the first time period before the current frame.

[0015] S13. Delete the second point cloud data that is determined to be a dynamic point, and obtain the first point cloud data.

[0016] Further, step S12 includes: if the difference is greater than the first threshold, then the current second point cloud data is a dynamic point.

[0017] Further, the feature point cloud includes corner feature point clouds; step S3 includes: calculating the curvature of each point cloud in the first point cloud data in the current frame; and using point clouds with curvature greater than a first curvature threshold as the corner feature point clouds.

[0018] Further, step S3 includes: obtaining ground point cloud data from the first point cloud data in the current frame; performing plane fitting on the ground point cloud data to obtain the ground plane equation of the current frame.

[0019] Further, step S4 includes: detecting corner feature point clouds in the first point cloud data of the current frame; and obtaining map feature points corresponding to the corner feature point clouds; obtaining two coordinate points of the edge features corresponding to the map feature points in the local map, and obtaining a first straight line passing through these two coordinate points; the local map is a map generated by the robot during its movement based on the first point cloud data before the current frame;

[0020] The distance between the map feature points and the first straight line is used as an optimization function to shorten the distance and obtain the robot's true pose.

[0021] Further, step S4 includes: matching the ground plane equation of the current frame with the ground plane equation of a local map; the local map is a map generated by the robot during its movement based on the first point cloud data before the current frame; and obtaining the difference in normal vector angle between the ground plane equation of the current frame and the matched ground plane equation as an optimization function.

[0022] Further, step S4 includes: obtaining the height matching difference between the feature point cloud of the first point cloud data in the current frame and the local map, as an optimization function; the local map is a map generated by the robot during its movement based on the first point cloud data before the current frame.

[0023] Further, step S5 includes: using the first pose as the current robot pose; updating the current robot pose based on the factor graph using the LiDAR odometry data to obtain the second pose as the current robot pose; the factors of the factor graph include the first pose data, the LiDAR odometry data, and the height of the feature point cloud.

[0024] The present invention also relates to a computer-readable storage medium storing computer program instructions thereon, which, when executed by a processor, implement the above-described method.

[0025] The technical solution of the present invention also relates to a laser SLAM system based on height information, the system comprising:

[0026] The robot is equipped with an inertial measurement unit (IMU) and a lidar device; wherein the IMU is used to acquire machine IMU data, and the lidar device is used to acquire three-dimensional lidar data; and,

[0027] A computer device connected to the robot, the computer device including a processor and a storage medium, the processor being configured to execute a sequence of instructions stored in the storage medium to perform the aforementioned laser SLAM method based on height information.

[0028] The beneficial effects of this invention are as follows.

[0029] The laser SLAM method and system based on height information proposed in this invention acquire point cloud data using lidar instead of visual sensors, reducing the reliance on ambient lighting. Furthermore, this invention integrates with an IMU (Inertial Measurement Unit) sensor and incorporates environmental height information for matching and localization. Compared to traditional laser SLAM methods, which are designed for static environments and scenarios with numerous spatial feature points, this method is more suitable for scenarios where casualties are concentrated on relatively wide and flat ground lacking feature points, while dynamic rescue personnel are present. This enables the robot to achieve high-precision self-localization and mapping in unfamiliar casualty concentration scenarios. Based on this, the robot can autonomously, efficiently, and safely triage casualties, thereby maximizing the treatment of the injured with limited medical resources. Attached Figure Description

[0030] Figure 1 This is a basic flowchart of the laser SLAM method based on height information according to the present invention.

[0031] Figure 2 This is a schematic diagram of data processing according to an embodiment of the present invention.

[0032] Figure 3 This is a schematic diagram of point cloud distortion according to an embodiment of the present invention.

[0033] Figure 4 This is a schematic diagram showing the distance from a map feature point to a first straight line according to an embodiment of the present invention.

[0034] Figure 5 This is a schematic diagram of the included angle of the plane normal vectors according to an embodiment of the present invention.

[0035] Figure 6 This is a schematic diagram of the factors of fused data according to an embodiment of the present invention.

[0036] Figure 7 This is an example of a generated 3D point cloud map according to an embodiment of the present invention.

[0037] Figure 8 This is a basic block diagram of a laser SLAM system based on height information according to the present invention. Detailed Implementation

[0038] The following will provide a clear and complete description of the concept, specific structure, and technical effects of the present invention in conjunction with the embodiments and accompanying drawings, so as to fully understand the purpose, solution, and effects of the present invention.

[0039] It should be noted that, unless otherwise specified, when a feature is referred to as "fixed" or "connected" to another feature, it can be directly fixed or connected to the other feature, or indirectly fixed or connected to the other feature. The singular forms "a," "described," and "the" used herein are also intended to include the plural forms, unless the context clearly indicates otherwise. Furthermore, unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art. The terminology used in this specification is for the purpose of describing particular embodiments only and not for limiting the invention. The term "and / or" as used herein includes any combination of one or more of the associated listed items.

[0040] Reference Figure 1 In some embodiments, the method according to the present invention includes the following steps:

[0041] S1. Acquire 3D LiDAR data, perform motion distortion and dynamic point filtering processing to obtain the first point cloud data;

[0042] S2. Acquire robot IMU data, perform pre-integration processing, and obtain the first pose data;

[0043] S3. Extract features from the first point cloud data to obtain the feature point cloud, and record the height of the feature point cloud;

[0044] S4. Obtain lidar odometer data based on the feature point cloud and its height;

[0045] S5. Based on the factor graph, the first pose data and the lidar odometry data are fused to obtain the second pose data as the robot pose.

[0046] S6. Determine the corresponding point cloud data based on the second pose data, and stitch them together to generate a three-dimensional point cloud map.

[0047] In some embodiments of the present invention, in scenarios such as damage collection points, the robot equipped with an inertial measurement unit and a lidar device, during its movement, refers to... Figure 2 The robot acquires its own IMU data via an inertial measurement unit (IMU) and 3D LiDAR data of the scene space via a LiDAR device. Then, the IMU data is processed to obtain IMU odometry data (equivalent to the first pose), and the LiDAR data is processed to obtain LiDAR odometry data. Next, the IMU odometry data and LiDAR data are fused using a factor map to obtain the second pose data, which is used as the current robot pose. Finally, based on the current robot pose and the corresponding point cloud data acquired, the 3D point cloud map is updated.

[0048] That is, in the embodiments of the present invention, the 3D point cloud map is updated in real time during the robot's movement. Taking the current frame as an example, the local map represents a map generated based on point cloud data within a first time period prior to the current frame during the robot's movement. The first time period can be customized; for example, it can be the length of time prior to the current frame, or it can be a configuration of a start time (e.g., when the robot begins collecting 3D LiDAR data) and an end time set to the frame prior to the current frame. In this embodiment, the local map... Figure 1 Generally, this can be understood as setting a region. If the distance (x, y plane) between the pose of the current frame and the pose of the first frame is greater than a set threshold, then all frames between the current frame and the first frame are combined into a local map, and frames after the current frame belong to the next local map. Figure 1 Generally, it is divided by distance.

[0049] The specific implementation of the above-described steps of the present invention will be described in detail below.

[0050] Detailed implementation of step S1

[0051] In embodiments of the present invention, the laser point data is not acquired instantaneously; laser measurement is performed concurrently with the robot's movement. Within a data acquisition cycle, when the lidar moves to different positions, the robot may mistakenly assume the laser point data was measured at the same position, leading to distortion. (Refer to...) Figure 3 .

[0052] To remove point cloud distortion, in some embodiments of the present invention, for example, by obtaining the start and end times of the current frame point cloud and assuming that the robot moves at a constant speed within that time interval, motion compensation is performed on the robot to obtain second point cloud data as the real point cloud data. It should be understood that the method for motion compensation of the robot is not limited to this.

[0053] Furthermore, the second point cloud data after distortion removal still contains dynamic rescue personnel. If the dynamic points are not removed, and step S3 is directly performed to generate corner feature points, and then the corner feature points are used for matching, it may lead to certain errors and thus affect the positioning results. Therefore, in the embodiments of the present invention, a step of filtering out dynamic points from the distortion-removed point cloud is also included.

[0054] In some embodiments, dynamic point filtering is performed, for example, using a RangeImage-based method. Specifically, depth image data is calculated for the second point cloud data and the local map of the current frame to obtain corresponding depth image data (RangeImage); the difference between these two (Diff) is obtained and compared with a configured first threshold λ. If it is greater than λ, the point cloud corresponding to the second point cloud data is determined to be a dynamic point. Refer to the following formula:

[0055] Diff = Range (current) -Range (map)

[0056]

[0057] Detailed implementation of step S2

[0058] Due to issues such as motion blur, rapid motion, and pure rotation in images, a single LiDAR is often insufficient to meet the application requirements of real-world scenarios. In contrast, an IMU can directly obtain measurement data of the angular velocity and acceleration of the moving subject, thereby constraining the motion and enabling rapid localization of robot motion and processing of pure rotation of the subject. By integrating the IMU measurements, the robot's pose information can be obtained, further improving the reliability of the SLAM method.

[0059] If an IMU can measure the linear acceleration *a* and angular velocity *w* of its own body at any given time, then its measurement model is as follows:

[0060]

[0061]

[0062] In the formula, the subscript t represents the coordinate system of the IMU body, and is subject to an acceleration bias b.a gyroscope bias b w and additional noise n a n w The influence of linear acceleration. Linear acceleration is the resultant vector of gravitational acceleration and object acceleration; the superscript ^ indicates that it is the measurement value of the IMU. This indicates the transformation from the world coordinate system to the IMU coordinate system.

[0063] In addition, there is additional noise n a n w It follows a Gaussian distribution, as follows.

[0064]

[0065] Detailed implementation of step S3

[0066] The first point cloud data after distortion correction is processed to obtain the corner feature point cloud and ground point cloud corresponding to the current frame; and the height corresponding to the corner feature points is recorded.

[0067] Based on the first point cloud data after motion compensation, feature extraction is required to obtain the current robot pose. In some embodiments of the present invention, considering real-time requirements, for example, by calculating the curvature of each point in the first point cloud data of the current frame, points with curvature greater than a certain threshold are used as feature points in the scene environment to obtain corner feature point clouds.

[0068] Furthermore, the planar equation of the ground can be obtained by fitting the ground point cloud of the current frame, for example, using the RANSAC (Random Sample Consensus) algorithm:

[0069] AX+BY+CZ+D=0

[0070] In the formula, (A,B,C) represents the normal vector of the plane; D represents the distance from the origin of the current coordinate system to the plane. That is, A, B, C, and D are the plane parameters of the plane equation of the ground.

[0071] The method for obtaining the height corresponding to corner feature points is as follows:

[0072] For the first point cloud data after distortion removal in step S1, its corresponding height map is created and saved. Then, the height information of the corner feature point cloud is calculated and saved. In some embodiments of the present invention, considering the real-time requirements, the simplest method is used to take the z value of the corner feature point cloud P as its corresponding height elevation.

[0073] elevation = P Z

[0074] Detailed implementation of step S4

[0075] Based on the feature point cloud and its height, the lidar odometry data is obtained.

[0076] To obtain the robot pose corresponding to the first point cloud data in the current frame, for the corner feature points of the first point cloud data, we need to find their corresponding map feature points in the local map. Theoretically, the distance between a corner feature point and its corresponding map feature point should be 0. However, in practical applications, there is a certain deviation between these two distances. To obtain the robot's true pose, the distance between the two can be used as an optimization function.

[0077] In some embodiments of the present invention, considering the fastest way to shorten the distance, the distance from the corner feature point to the first straight line is selected as the optimization function; wherein the first straight line is a straight line passing through the following two coordinate points, which are the two coordinate points of the edge feature corresponding to the corner feature point in the map:

[0078]

[0079] In the formula x i This represents the i-th corner feature point detected in the first point cloud data of the current frame; x j and x l Let represent the two coordinates of the edge feature corresponding to the i-th corner feature point on the map. The distance d from the corner feature point to the first straight line is as follows: Figure 4 As shown.

[0080] In addition, in some embodiments, considering that the environment of the injury collection point is a relatively open ground, a large pose estimation deviation will occur when the robot detects fewer feature points. Therefore, in view of this characteristic, the plane obtained by fitting in step S3 is used as a feature.

[0081] That is, the ground equation of the current frame is matched with the ground equation of the local map. Considering that the ground is relatively flat and the robot's pose deviation in the Z direction is small and negligible, the angle difference of the normal vector between the two is used (refer to...). Figure 5 As an optimization function:

[0082]

[0083] In the formula, a and b represent the normal vectors of the two planes, respectively, and θ e The angle between the normal vectors.

[0084] For the height information of the feature point cloud, the height matching difference between the feature point cloud in the first point cloud data of the current frame and the local map is used as its optimization function:

[0085] e h =||P z -(P z) map ||

[0086] In the formula, e h Let P be the height of the feature point cloud, where P is the height difference. z ) map The height of the points matched in the feature point cloud on the local map.

[0087] Detailed implementation of step S5

[0088] In some embodiments of the present invention, step 5 includes:

[0089] S51. Use the IMU pre-integration result recorded in step S2, i.e. the first pose data, as the robot pose of the current frame (equivalent to making the robot pose of the current frame the first pose data).

[0090] S52. The robot pose is updated using the LiDAR odometry data from step S4 to obtain a second pose as the robot's final pose for the current frame. The LiDAR odometry data and the robot pose for the current frame are fused using a factor graph, as shown in the reference... Figure 6 . Figure 6 The factor diagram includes three factors: MIU factor, radar odometry factor, and altitude factor, which correspond to the altitude of the first pose data, lidar odometry data, and feature point cloud, respectively.

[0091] Detailed implementation of step S6

[0092] In some embodiments of the present invention, for example, the corresponding three-dimensional LiDAR point cloud data is obtained from the stored point cloud data based on the robot's second pose data, and then the point cloud data corresponding to the robot's pose at different times are stitched together to obtain a three-dimensional point cloud map of the scene.

[0093] In other embodiments of the present invention, for example, a complete 3D point cloud map of the current scene may be generated by updating the local map based on the second pose data of the current frame and the first point cloud data obtained in the current frame. Examples of 3D point cloud maps generated by embodiments of the present invention are as follows: Figure 7 As shown. Map updates can be performed at a preset frequency (e.g., at a certain time period) or according to preset rules. For example, if it is determined that the robot has traveled a distance greater than a certain length, a map update is triggered.

[0094] Reference Figure 8In some embodiments, a height-information-based laser SLAM system according to the present invention includes: a robot 100 and a computer device 200. The robot 100 is equipped with an inertial measurement unit (IMU) and a lidar device; wherein the IMU is used to acquire machine IMU data, and the lidar device is used to acquire three-dimensional lidar data. The computer device 200 is connected to the robot and includes a processor and a storage medium. The processor is used to execute a sequence of instructions stored in the storage medium to perform the aforementioned height-information-based laser SLAM method. It should be understood that the computer device 200 can be built into the robot and can be, for example, a programmable logic chip; or it can be a computer device communicatively connected to the robot, such as a computer, tablet, or smartphone.

[0095] Therefore, in the solution proposed in this invention, by using a robot equipped with an inertial measurement unit and lidar, compared to the traditional laser SLAM method, the accuracy loss caused by the lack of feature points due to the concentration of casualties on a relatively wide and flat ground at the injury collection point can be effectively reduced. Simultaneously, it eliminates data interference from dynamic rescue personnel at the injury collection point, enabling the robot to achieve high-precision self-localization and high-precision mapping in unfamiliar injury collection point scenarios. Based on this, the robot can autonomously, efficiently, and safely perform triage of casualties, thereby maximizing the treatment of casualties with limited medical resources.

[0096] It should be understood that the method steps in all embodiments of the present invention can be implemented or carried out by computer hardware, a combination of hardware and software, or by computer instructions stored in a non-transitory computer-readable storage medium. The methods can use standard programming techniques. Each program can be implemented in a high-level procedural or object-oriented programming language to communicate with the computer system. However, if necessary, the program can be implemented in assembly or machine language. In any case, the language can be a compiled or interpreted language. Furthermore, for this purpose, the program can run on a programmed application-specific integrated circuit (ASIC).

[0097] Furthermore, the procedures described herein may be performed in any suitable order unless otherwise indicated herein or otherwise clearly contradicted by the context. The procedures described herein (or variations and / or combinations thereof) may be executed under the control of one or more computer systems configured with executable instructions, and may be implemented by hardware or a combination thereof as code (e.g., executable instructions, one or more computer programs, or one or more applications) that commonly executes on one or more processors. The computer program comprises a plurality of instructions executable by one or more processors.

[0098] Furthermore, the method can be implemented in any suitable type of computing platform, including but not limited to personal computers, minicomputers, mainframes, workstations, networked or distributed computing environments, standalone or integrated computer platforms, or in communication with charged particle tools or other imaging devices, etc. Aspects of the invention can be implemented as machine-readable code stored on a non-transitory storage medium or device, whether removable or integrated into a computing platform, such as a hard disk, optical read and / or write storage medium, RAM, ROM, etc., such that it is readable by a programmable computer, and when the storage medium or device is read by the computer, it can be used to configure and operate the computer to perform the processes described herein. Furthermore, the machine-readable code, or portions thereof, can be transmitted via wired or wireless networks. The invention described herein includes these and other different types of non-transitory computer-readable storage media when such media comprises instructions or programs that implement the steps described above in conjunction with a microprocessor or other data processor. When programmed according to the methods and techniques described in the invention, the invention may also include the computer itself.

[0099] A computer program can be applied to input data to perform the functions described herein, thereby transforming the input data to generate output data stored in non-volatile memory. The output information can also be applied to one or more output devices, such as a display. In a preferred embodiment of the invention, the transformed data represents physical and tangible objects, including specific visual depictions of physical and tangible objects generated on the display.

[0100] The above description is merely a preferred embodiment of the present invention. The present invention is not limited to the above-described embodiments. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention, as long as they achieve the technical effects of the present invention by the same means, should be included within the scope of protection of the present invention. Within the scope of protection of the present invention, the technical solutions and / or implementation methods can have various modifications and variations.

Claims

1. A laser SLAM method based on height information, comprising the following steps: S1, obtaining three-dimensional laser radar data, performing motion distortion and dynamic point filtering processing to obtain first point cloud data; S2, obtaining robot IMU data, performing pre-integration processing to obtain first pose data; S3, extracting features from the first point cloud data to obtain a feature point cloud, and recording the height of the feature point cloud; wherein the feature point cloud comprises a corner feature point cloud; the step S3 comprises: calculating the curvature of each point cloud in the first point cloud data in the current frame; point clouds with a curvature greater than a first curvature threshold are taken as the corner feature point cloud; S4, obtaining laser radar odometry data according to the feature point cloud and the height of the feature point cloud; wherein the step S4 comprises: detecting the corner feature point cloud in the first point cloud data of the current frame; and obtaining the map feature point corresponding to the corner feature point cloud; obtaining two coordinate points of the edge feature corresponding to the map feature point in the local map to obtain a first straight line passing through the two coordinate points; the local map is a map generated according to the first point cloud data within a first time before the current frame during the robot travels; taking the distance between the map feature point and the first straight line as an optimization function to shorten the distance to obtain the real pose of the robot, wherein the first straight line is a straight line passing through the following two coordinate points, which are two coordinate points of the edge feature corresponding to the corner feature point in the map: In the formula, x i represents the i-th corner feature point detected in the first point cloud data of the current frame, x j and x i respectively represent two coordinate points of the corresponding edge feature of the i-th corner feature point in the map; S5, based on a factor graph, fusing the first pose data and the laser radar odometry data to obtain second pose data as the robot pose; S6, determining the corresponding point cloud data according to the second pose data, and splicing to generate a three-dimensional point cloud map.

2. The method of claim 1, wherein, The step S1 comprises: S11, for the three-dimensional laser radar data, obtaining second point cloud data by performing motion compensation on the robot; S12, obtaining the difference between the second point cloud data of the current frame and the depth image data of the local map, and determining whether the second point cloud data of the current frame is a dynamic point according to the difference; the local map is a map generated according to the first point cloud data within a first time before the current frame during the robot travels; S13, deleting the second point cloud data determined as a dynamic point to obtain the first point cloud data. 3.The method of claim 1, wherein the step S3 comprises: obtaining ground point cloud data from the first point cloud data in the current frame; performing plane fitting on the ground point cloud data to obtain a ground plane equation of the current frame.

4. The method of claim 1, wherein, The step S4 comprises: matching the ground plane equation of the current frame with the ground plane equation of the local map as a feature; the local map is a map generated according to the first point cloud data within a first time before the current frame during the robot travels; obtaining the normal vector angle difference between the ground plane equation of the current frame and the matched ground plane equation as an optimization function.

5. The method of claim 1, wherein, The step S4 comprises: A height matching difference between a feature point cloud of the first point cloud data of the current frame and a local map is obtained as an optimization function; the local map is a map generated according to the first point cloud data before the current frame during robot travel.

6. The method of claim 1, wherein, The step S5 comprises: The first pose is taken as a current robot pose; The current robot pose is updated based on a factor graph to obtain the second pose as the current robot pose; factors of the factor graph include the first pose data, the laser radar odometry data and the height of the feature point cloud. 7.A computer readable storage medium having program instructions stored thereon, the program instructions being executed by a processor to implement the method of any one of claims 1 to 6.

8. A laser SLAM system based on height information, characterized in that, The system comprises: a robot provided with an inertial measurement unit and a laser radar device; wherein the inertial measurement unit is used to obtain machine IMU data, and the laser radar device is used to obtain three-dimensional laser radar data; and a computer device connected with the robot, the computer device comprising a processor and a storage medium, the processor being used to execute an instruction sequence stored in the storage medium to execute the method of any one of claims 1 to 6.

Images