System and method for constructing high-precision 2D projection maps of non-flat ground scenes

Abstract

The present invention discloses a system and method for constructing a high-precision 2D projection map of a non-flat ground scene, comprising: a slope state judgment module, which is based on IMU sensor data and uses the inclination angle of the robot to judge whether it has entered a slope state; an internal odometer module, which combines imu and encoder data to calculate the internal odometer when on a plane; a projection odometer module, which combines the inclination angle data of the imu to project the robot's posture change onto a horizontal plane when on a slope, maintains the original three degrees of freedom, and calculates the projection odometer corresponding to the 2D projection map; the present invention can focus on ground scene applications, can effectively solve the problem that traditional 2D laser SLAM cannot adapt to sloped ground, and achieves higher-precision mapping and positioning with a simple system method and lower cost.

Description

Technical Field

[0001] The present invention relates to the field of map construction and positioning technology, and in particular to a system and method for constructing a high-precision 2D projection map of a non-flat ground scene. Background Art

[0002] At present, in scenes with flat ground, such as factories and indoors, the 2D laser-based SLAM (Simultaneous Localization and Mapping) technology has become relatively mature. Robots based on this technology are widely used in factory AGV, hotel delivery, household cleaning and other fields.

[0003] Existing algorithms include:

[0004] (1) Cartographer 2D SLAM:

[0005] Cartographer is one of the mainstream algorithms at present. It is a native 2D laser SLAM algorithm that requires a flat ground as a prerequisite. When the ground is uneven, the inclination of the laser is measured by the imu to obtain a virtual "horizontal laser" to reduce noise. This method can only be used in scenes with vertical walls and cannot adapt to long-distance slope ground conditions.

[0006] (2) gmapping 2D SLAM:

[0007] gmapping is a native 2D laser SLAM algorithm that requires an odometer to build maps. It can only be used in environments with flat ground and cannot be used in environments with uneven ground.

[0008] (3) LOAM-SLAM:

[0009] LOAM is a mainstream 3D laser SLAM algorithm system that builds 3D maps by matching 3D laser point clouds. It can work in 3D situations, including going up and down stairs. However, this method relies on 3D laser radar, which is expensive, requires a lot of computation, and it is difficult to achieve an accuracy below 5 cm (limited by sensor performance).

[0010] In more similar occasions, the ground is usually uneven, with various up and down slopes, thresholds, etc. Typical representatives include: the factory floor has up and down slopes to cover the wiring, there are differences and up and down slopes between the interfaces on the same floor of the building, the entrances and exits / speed bumps of the underground parking lot, etc. In these occasions, the traditional robot 2D laser SLAM algorithm (assuming the environment is a flat ground) is difficult to adapt, and usually has to be combined with vision, or based on 3D laser, or segmented maps for mapping and navigation. On the one hand, these methods make the system more complicated and the cost of software and hardware is greatly increased. On the other hand, the bigger problem is that the introduction of vision or 3D laser for 3D mapping and positioning is limited by the characteristics of the sensor, and it is difficult to achieve high-precision positioning (for example, controlling the positioning accuracy to more than 1cm) at an acceptable software and hardware cost.

[0011] Therefore, how to achieve cost-effective high-precision positioning map construction on uneven terrain has become a difficulty in robot applications. Summary of the invention

[0012] The present invention provides a system for constructing a high-precision 2D projection map of a non-flat ground scene, so as to solve the problems mentioned in the above background.

[0013] A system for constructing high-precision 2D projection maps of non-flat ground scenes, comprising:

[0014] The slope state judgment module is based on the IMU sensor data and the robot's inclination angle to determine whether it has entered a slope state;

[0015] Internal odometer module, when on a flat surface, combines imu and encoder data to calculate the internal odometer;

[0016] The projection odometry module projects the robot’s posture changes onto the horizontal plane in combination with the inclination angle data of the imu when driving on a slope, maintains the original three degrees of freedom, and calculates the projection odometry corresponding to the 2D projection map;

[0017] The laser data projection module projects the laser raw data currently measured by the robot vertically on the horizontal plane when on a slope, and screens according to conditions to remove the error areas to obtain the projected laser data;

[0018] The SLAM mapping module for switching between plane and slope, and the flat ground SLAM mapping module adopts traditional 2D laser SLAM technology, with additional functions of temporarily storing robot position, temporarily storing map, pausing mapping, starting mapping, and continuing mapping at a given position.

[0019] A method for constructing a high-precision 2D projection map of a non-flat ground scene, characterized by comprising:

[0020] S1: When the robot is on a plane, the traditional planar SLAM algorithm is used to build the map. Based on the input odometer data and 2D laser data, the 2D grid map and the current position of the robot are obtained synchronously through matching;

[0021] S2: When the robot starts to enter the slope, the planar SLAM algorithm is paused, and the robot's current position, map, and intermediate state are recorded, and the slope SLAM mapping is switched;

[0022] S3: Slope SLAM initialization. When entering the slope SLAM, read the 2D grid map saved by the robot's plane SLAM as the map; read the posture saved by the robot's plane SLAM as the initial posture;

[0023] S4: Slope SLAM map update. In the map, with the current position of the robot as the center, the currently measured LiDAR data is projected to obtain Dt, which is converted into pose and marked in the 2D grid map.

[0024] S5: When the robot walks out of the slope, the current position and posture of the robot, as well as the temporarily stored grid map, are recorded and used as the initial state; they are input into the planar SLAM algorithm, and the planar SLAM algorithm is continued to be used to continue to build the map.

[0025] Preferably, the plane and slope switching SLAM mapping module mainly uses the 2D occupancy grid map; at the slope position, it is represented by the occupancy grid projected from 3D to 2D and represented by special marks.

[0026] The present invention focuses on the above-mentioned scenario applications, and can effectively solve the problem that traditional 2D laser SLAM cannot adapt to sloping ground, and achieves more high-precision mapping and positioning with a simple system method and lower cost. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] The specific implementation modes of the present invention are further described in detail below in conjunction with the accompanying drawings.

[0028] Figure 1 The framework and flow chart of the method of the present invention are shown in FIG. DETAILED DESCRIPTION

[0029] In order to make the objects and advantages of the present invention more clearly understood, the present invention is further described below in conjunction with embodiments; it should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0030] The preferred embodiments of the present invention are described below with reference to the accompanying drawings. It should be understood by those skilled in the art that these embodiments are only used to explain the technical principles of the present invention and are not intended to limit the protection scope of the present invention.

[0031] The present invention relates to a system and method for constructing a high-precision 2D projection map of a non-flat ground scene. The following takes indoor mapping with a slope by a mobile robot as an example to illustrate the specific technical solution of the present invention.

[0032] Typical sensor configurations for mobile robots include: 2D laser radar (range measurement > 10 meters), IMU, encoder, visual odometer (real-sense T265), etc. Position relationship conversion configurations have been made between the relevant sensor coordinate systems.

[0033] Indoor environment example: It consists of several flat ground areas and large slopes connecting two areas. The slope height is higher than the installation height of the robot laser radar, the slope inclination is about 30°, and the slope length is greater than 1 meter.

[0034] (1) Internal odometer

[0035] In a planar mobile robot (slopes are not considered for now), the internal odometer can be expressed as: (x_o, y_o, theta_o); where x_o represents the x-coordinate of the current robot in the coordinate system of the robot's starting point; y_o represents the y-coordinate of the current robot in the coordinate system of the robot's starting point; theta_o represents the direction angle of the current robot in the coordinate system of the robot's starting point.

[0036] It is an existing technology to calculate the internal odometer by combining the imu data and the code disk encoder data. In order to explain the process of the present invention in detail, a method for calculating the internal odometer of a robot driven by two wheels with differential speed is briefly introduced here:

[0037] Assume that at the kth moment, the mileage counted by the encoders of the left and right wheels are: dlk and drk respectively;

[0038] Assume that at the kth moment, the fusion direction angle (yaw angle) representing the robot's orientation in the imu data is: angk;

[0039] At time k+1, remember:

[0040] s k+1 =((dl k+1 -dl k )+(dr k+1 -dr k )) / 2

[0041] Then, from time k to time k+1, the cumulative iteration of the internal odometer is: (x_o, y_o, theta_o)

[0042] x_o k+1 =x_o k +s k+1 *cos(ang k+1 )

[0043] y_o k+1 =y_o k +s k+1 *sin(ang k+1 )

[0044] theta_o k+1 =ang k+1

[0045] In the above formula, we use imu with fusion calculation angle function to provide the direction angle of the robot. We can also use angular velocity integration to do it, which will not be introduced in detail here.

[0046] For laser SLAM, most of them use the incremental data of the odometer to participate in the matching:

[0047] dx_o k+1 =s k+1 *cos(ang k+1 )

[0048] dy_o k+1 =s k+1 *sin(ang k+1 )

[0049] d_theta_o k+1 =ang k+1 -ang k

[0050] (2) Projection Odometry

[0051] The internal odometer is mainly used for calculations when the robot is driving on horizontal ground. It does not take into account the tilt angle when the robot is running on a slope. In other words, in the process of calculating the robot's posture, the vertical coordinate changes: z_o, the tilt angle ang_x (roll angle) around its own x-axis and the tilt angle ang_y (pitch angle) around its own y-axis are not considered. When the robot is driving on a slope, the above values ​​will change, and only using the internal odometer to calculate will lead to errors.

[0052] The conventional method is to design the robot's posture as a six-degree-of-freedom model, which ultimately corresponds to a 3D map. However, in view of the situation we are targeting, the robot only moves on the ground, and the slope is only a small area. Implementing a positioning and navigation system according to the above method will become too complicated and impractical. Therefore, the present invention proposes a new method here, in which the robot's posture design maintains the original three degrees of freedom, corresponding to a 2D projection map, which is called: projection odometer in terms of odometer.

[0053] When the robot determines that it is on a slope (how to determine it is described in detail later), a projection odometer is used to replace the odometer. The calculation method is as follows:

[0054] Assume that at the kth moment, when the robot is on the slope, the rolling angle measured by its imu is: ang_x k The pitch angle is: ang_y k The yaw angle is: ang k ; Similarly, the mileage counted by the encoders of the left and right wheels are: dl k and Dr. k ;

[0055] At time k+1, remember:

[0056] s k+1 =((dl k+1 -dl k )+(dr k+1 -dr k )) / 2

[0057] s k+1 It represents the micro distance that the robot runs on the slope.

[0058] Here, we consider the situation where the robot is on a slope, and the actual "plane projection" distance of the robot is:

[0059] s' k+1 =s k+1 *cos(ang_y k )

[0060] Furthermore, from the kth moment to the k+1th moment, the cumulative iteration of the projection odometry is: (x_o', y_o', theta_o')

[0061] x_o' k+1 =x_o' k +s k+1 *cos(ang' k+1 )*cos(ang_y k )

[0062] y_o’ k+1 = y_o’ k + s k+1 * sin(ang’ k+1 ) * cos(ang_y k )

[0063] theta_o’ k+1 = ang’ k+1

[0064] where ang’ k+1 is the projection angle of the current orientation of the robot on the horizontal plane and can be measured from the robot's imu data.

[0065] The projected odometer represents the distance traveled by the projection of the robot on the horizontal plane when the robot moves on a slope.

[0066] The corresponding projected increment is:

[0067] dx_o’ k+1 = s k+1 * cos(ang’ k+1 ) * cos(ang_y k )

[0068] dy_o’ k+1 = s k+1 * sin(ang’ k+1 ) * cos(ang_y k )

[0069] d_theta_o’ k+1 = ang’ k+1

[0070] (3) Slope state judgment

[0071] In terms of judging the slope ground, in addition to manually or other means to inform the robot that it is currently on a slope, the following method can also be used to judge based on the data change of the built-in IMU:

[0072] Condition 1: The absolute value of the acceleration of the Z axis measured by the IMU: |acc_z| < th_g and the duration t > 0.5 s, where th_g can take the value of 9 m / s2;

[0073] Condition 2: The absolute values of the pitch angle and roll angle measured by the IMU: |ang_x| > th_x or |ang_y| > th_x; and the duration t > 0.5 s, where th_x can take the value of 5°;

[0074] When both condition 1 and condition 2 are met at the same time, the robot is considered to have entered the ramp state; after entering the ramp state, if the robot does not meet both condition 1 and condition 2 at the same time, the robot exits the ramp state.

[0075] (4) Projection laser data acquisition

[0076] The projected laser data used is the vertical projection data of the laser raw data currently measured by the robot on the horizontal plane.

[0077] On the slope, the robot measures a frame of laser data:

[0078] Data={(d0,θ0),(d1,θ1),...,(d i ,θ i ),...,(d n ,θ n )}

[0079] Where n represents the number of points scanned by the laser in one frame, θ i Indicates the current direction angle of the laser scan, d i Indicates the distance of the obstacle measured in this direction. Generally speaking, θ i The spacing between them is equal, and here it is normalized to the angle with the robot facing the positive x direction. Suppose the roll angle currently measured by imu is: ang_x and the pitch angle is: ang_y.

[0080] Then, for a certain laser point data (d i ,θ i ), its projection data is (d' i ,θ' i ),in:

[0081] d' i =sqrt{[d i *cos(θ i )*cos(ang_x)] 2 +[d i *sin(θ

[0082] i )*cos(ang_y)] 2}

[0083] θ' i =θ i

[0084] Among them, sqrt{} means taking the square root.

[0085] Correspondingly, the set of laser points after projection is recorded as: Data'.

[0086] Furthermore, considering that when the robot is on a slope, the projection data within a certain angle range of its forward direction is even more unreliable, that is, it may hit the ground or the roof, forming a false obstacle reflection, the data in Data' that meets the following conditions 1 or 2 is removed:

[0087] Condition 1: |θ’ i |<alpha, where alpha can be 30°

[0088] Condition 2: d’ i > D1, or d’ i < D2, D1 can be taken as 5 meters, D2 can be taken as 0.3 meters

[0089] Thus, a set of m projected laser points is obtained and recorded as:

[0090] Dt={(d' i ,θ' i )}, i=1,2,..,m

[0091] This is the projected laser data.

[0092] (5) Slope SLAM Mapping

[0093] When the robot is on a plane, a traditional planar SLAM algorithm (such as cartographer or gmapping) is used to build a map. Since the robots are in adjacent positions, the heights illuminated by their horizontally installed laser radars are basically the same, so matching can be performed. Based on the input odometer data and 2D laser data, the 2D grid map and the current position of the robot are obtained synchronously through matching;

[0094] When the robot starts to enter the slope, the planar SLAM algorithm can be paused through external algorithm management, and the robot's current position, map, and intermediate state can be recorded, and the slope SLAM mapping can be switched. The specific technical steps include the following:

[0095] Slope SLAM initialization: When entering the slope SLAM, read the 2D grid map saved by the robot's plane SLAM as the map; read the posture saved by the robot's plane SLAM as the initial posture.

[0096] Assume that the plane SLAM pauses at the kth moment and then enters the slope SLAM state. Then, the new starting position of the robot is:

[0097] x_o' k =x_o k

[0098] y_o' k =y_o k

[0099] theta_o' k =ang'k

[0100] Robot position update for slope SLAM:

[0101] In slope SLAM mapping, the laser radar is no longer horizontal. Since the objects illuminated by the laser radar are inconsistent at adjacent positions, it has no effect on the robot's position calculation. Here, the projection odometer is directly used to update the robot's position and posture. Specifically:

[0102] x_o' k+1 =x_o' k +dx_o' k+1

[0103] y_o' k+1 =y_o' k +dy_o' k+1

[0104] theta_o' k+1 =ang' k+1

[0105] And so on.

[0106] Slope SLAM map update:

[0107] In the map, with the current position of the robot as the center, the currently measured laser radar data is projected and processed to obtain Dt, which is then converted into a 2D grid map after posture conversion. Specifically, for each projected laser point in Dt, it is converted into a map point through the projection posture of the robot in the map, and expanded to the sector area in the grid map according to Δθ (Δθ is the laser radar angle resolution); the grids on the arc outside the sector area are marked as increasing obstacle values ​​(maximum 255), and the inside of the sector area is marked as decreasing obstacle values ​​(minimum 0); after a period of iteration, the grids with obstacle values ​​greater than the threshold are obstacle areas, and the grids with obstacle values ​​less than the threshold are non-obstacle areas.

[0108] Slope SLAM status saving and exit:

[0109] When the robot walks out of the slope, it records the current position and posture of the robot, as well as the temporarily stored grid map, and uses it as the initial state and inputs it into the planar SLAM algorithm, and continues to use the planar SLAM algorithm to continue to build the map.

[0110] Understandably,

[0111] The SLAM mapping module switches between plane and slope, and the map is mainly a 2D occupancy grid map; in particular, at the slope position, it is represented by the occupancy grid projected from 3D to 2D and represented by a special mark, but it does not affect its participation in the robot's path planning as a part of the 2D map.

[0112] So far, the technical solutions of the present invention have been described in conjunction with the preferred embodiments shown in the accompanying drawings. However, it is easy for those skilled in the art to understand that the protection scope of the present invention is obviously not limited to these specific embodiments. Without departing from the principle of the present invention, those skilled in the art can make equivalent changes or substitutions to the relevant technical features, and the technical solutions after these changes or substitutions will fall within the protection scope of the present invention.

Claims

1. A system for constructing high-precision 2D projection maps of non-flat ground scenes, characterized in that: include: The slope state judgment module is based on the IMU sensor data and the robot's inclination angle to determine whether it has entered a slope state; Internal odometer module, when on a flat surface, combines imu and encoder data to calculate the internal odometer; The projection odometry module projects the robot’s posture changes onto the horizontal plane in combination with the inclination angle data of the imu when driving on a slope, maintains the original three degrees of freedom, and calculates the projection odometry corresponding to the 2D projection map; The laser data projection module projects the laser raw data currently measured by the robot vertically on the horizontal plane when on a slope, and screens according to conditions to remove the error areas to obtain the projected laser data; The plane and slope switching SLAM mapping module, the flat ground SLAM mapping module adopts the traditional 2D laser SLAM technology, and has the additional functions of temporarily storing the robot position, temporarily storing the map, pausing the mapping, starting the mapping, and continuing the mapping at a given position; the plane and slope switching SLAM mapping module mainly uses the map 2D occupying grid map; At the slope position, it is represented by the occupancy grid projected from 3D to 2D and indicated by a special marker; The steps include: S1: When the robot is on a plane, the traditional planar SLAM algorithm is used to build the map. Based on the input odometer data and 2D laser data, the 2D grid map and the current position of the robot are obtained synchronously through matching; S2: When the robot starts to enter the slope, the planar SLAM algorithm is paused, and the robot's current position, map, and intermediate state are recorded, and the slope SLAM mapping is switched; S3: Slope SLAM initialization. When entering the slope SLAM, read the 2D grid map saved by the robot's plane SLAM as the map; read the posture saved by the robot's plane SLAM as the initial posture; S4: Slope SLAM map update. In the map, with the current position of the robot as the center, the currently measured LiDAR data is projected to obtain Dt, which is converted into pose and marked in the 2D grid map. S5: When the robot walks out of the slope, the current position and posture of the robot, as well as the temporarily stored grid map, are recorded and used as the initial state; they are input into the planar SLAM algorithm, and the planar SLAM algorithm is continued to be used to continue to build the map.

Images