Hidden area hybrid topological map construction method

Abstract

The invention discloses a hidden area hybrid topological map construction method, and relates to the technical field of robot perception and mapping, and the method comprises the steps: S1, inputting a laser point cloud, and building an index through a k-dimensional tree; s2, filtering the ground of the input laser point cloud; s3, calculating the normal of each point in the filtered laser point cloud, inputting the normal into Region Growing clustering, and clustering to obtain a point cloud cluster Ci; s4, according to the point clouds in the point cloud cluster Ci, screening the point cloud clusters with the point cloud number greater than 100 as candidate point cloud clusters; s5, establishing a hidden area prior map; s6, acquiring longitude and latitude coordinates (lat0, lon0) at the beginning of mapping in the step 5, and establishing a local Cartesian according to the original point; s7, performing least square estimation on the position data under the six groups of local Cartesian coordinate systems and mapping synchronization positions to obtain a group of transformation matrixes; and S8, generating a hidden area hybrid map. By adopting the hidden area hybrid topological map construction method, priori hidden area information is provided for the robot, and occupation of a storage medium is reduced.

Description

Technical Field

[0001] The present invention relates to the field of robot perception and mapping technology, and in particular to a method for constructing a hidden area hybrid topology map. Background Art

[0002] Mobile robots are usually used to perform various mobile tasks, such as inspection, patrol, food delivery, and delivery. Mobile robots with patrol and inspection functions are usually equipped with high-value sensors and usually operate in unstructured, semi-dynamic scenarios. If they encounter suspicious persons and are discovered during patrol and inspection, it is easy to cause high asset losses and usually takes a long time to repair them before they can resume work. In addition, in open outdoor areas and large maps, pure laser mapping is not effective and requires more storage media space.

[0003] Therefore, the present invention provides a method for constructing a hybrid topological map of a hidden area, which can provide the robot with a priori hidden area information, provide the robot with a reference hidden area location when encountering a suspicious person, reduce the probability of the robot being damaged, and at the same time provide the robot with an OSM-based topological map in an open area with good GPS, reduce the robot's energy consumption, and reduce the space occupied by the storage medium. Summary of the invention

[0004] The purpose of the present invention is to provide a method for constructing a hybrid topological map of a hidden area to solve the problems raised in the above-mentioned background technology. By constructing a hidden area map under a GPS-based space, a priori map of the hidden area is provided for the robot in a GPS-based environment, which is convenient for it to plan tracking or escape routes when encountering suspicious persons and reduce the probability of being destroyed; in areas with good GPS, a topological map based on OSM is provided to reduce the energy consumption of the robot and reduce the space occupied by the storage medium.

[0005] To achieve the above object, the present invention provides a method for constructing a hidden area hybrid topology map, comprising the following steps:

[0006] Step S1, input laser point cloud and use k-dimensional tree to create index;

[0007] Step S2, filtering the ground of the input laser point cloud to remove the interfering laser point cloud to ensure the accuracy of segmentation, and filtering the laser point cloud above the ground H using a straight-through filter;

[0008] Step S3: Calculate the normal of each point in the filtered laser point cloud and input it into Region Growing clustering to obtain point cloud cluster C. i , where i is the point cloud cluster sequence, and the typical value smoothness S tis 10 degrees, the curvature C t is 1;

[0009] Step S4: According to the point cloud cluster C i In the point cloud, the point cloud clusters with more than 100 points are selected as candidate point cloud clusters, and the minimum point p(x min ,y0,z0) and the maximum point p(x1,y max ,z1) and the minimum point p(x2,y min ,z2), generate candidate hidden points to be processed Use a double random algorithm to take values ​​within a preset range, typically [0.5, 1], and calculate the position of the hidden point relative to the radar;

[0010] Step S5: Use the mapping algorithm in the outdoor environment to obtain the real-time pose estimation of the robot at the current coordinates, generate a transformation matrix based on the pose, and the transformation matrix between the laser radar and the vehicle body coordinate system is obtained by measurement. l Register to the global coordinate system to get the global hidden point p w , repeat the process to build a priori map of hidden areas;

[0011] Step S6: Obtain the latitude and longitude coordinates (lat0, lon0) at the beginning of the map construction in step 5 as the origin of the initial coordinate system of the OSM map, establish a local Cartesian coordinate system based on this origin, and then obtain the robot's six sets of local Cartesian coordinate system pose data at different times in the RTK differential mode during the robot's mapping motion interval. And the pose estimation aligned with this time in laser mapping Among them, i∈[1,6], k∈[1,6];

[0012] Step S7, performing least square estimation on the six sets of position data in the local Cartesian coordinate system and the synchronous positions of the map construction to obtain a set of transformation matrices;

[0013] Step S8: Apply the transformation matrix to the hidden area prior map and the laser point cloud map respectively, and convert all laser point clouds to a map based on the local coordinate system of the OSM map to generate a hidden area hybrid map.

[0014] Preferably, the size of the laser point cloud input in step S1 is L×P, wherein L is the laser beam and P is the point on each laser beam that is scanned once.

[0015] Preferably, in step S2, is to satisfy the inequality constraint The minimum approximate rational number of , where R is the height of the robot itself and L is the length of the robot itself.

[0016] Preferably, in step S4, the specific method for calculating the relative position of the hidden point is as follows:

[0017] S41. Use a Gaussian distribution function with a mean of 0 and a variance of 1 to randomly generate and truncate to obtain a random value a, a∈(-1,1);

[0018] S42, the random value is a positive number. At this time, a>0, then the uniform distribution random generation method is selected to generate a uniform distribution random number u in the interval [0.5,1] to obtain the candidate hidden point

[0019] S43, the random value is a non-positive number. In this case, a≤0, and the truncated exponential distribution algorithm is used to generate a random value within a given range.

[0020] Preferably, the step S43 specifically includes:

[0021] Calculate the cumulative distribution values ​​F(a) and F(b) in the interval [a,b]. The expressions are as follows:

[0022] F(a)=1-e -λa ;

[0023] F(b)=1-e -λb ;

[0024] After obtaining the cumulative distribution values ​​F(a) and F(b), a uniformly distributed random number u is generated in the interval [F(a), F(b)], and an exponentially distributed random number x is obtained. The expression of x is as follows:

[0025]

[0026] Among them, the typical values ​​are a=0.5, b=1, λ=1;

[0027] Finally, we get the candidate hidden points

[0028] Preferably, in step S5, p w The expression is as follows:

[0029]

[0030] Among them, R bl and t bl are the rotation and translation matrices that transform a point in the radar coordinate system to the vehicle chassis coordinate system, R wb , and t bl They are the rotation and translation matrices that transform a point under the vehicle chassis to the map coordinate system.

[0031] Therefore, the present invention adopts the above-mentioned method for constructing a hidden area hybrid topology map, which has the following beneficial effects:

[0032] (1) This method is based on LiDAR and uses the point cloud Region Growing clustering method to find hidden areas. It can work under different lighting and weather conditions and segment the hidden areas to be screened.

[0033] (2) This method screens the point cloud clusters obtained by clustering based on the number of points in the point cloud clusters, and then uses a double random algorithm to obtain the local relative position of the hidden points, thereby increasing the randomness of the hidden point selection.

[0034] (3) This method realizes hidden area mapping and SLAM mapping simultaneously, and fuses the hidden area point cloud map, laser SLAM point cloud map and OSM topology map into a hidden area hybrid topology map using a transformation matrix, which provides the robot with a priori hidden area information and reduces the occupancy of storage media.

[0035] The technical solution of the present invention is further described in detail below through the accompanying drawings and embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 A flowchart of an embodiment of a method for constructing a hidden area hybrid topology map according to the present invention;

[0037] Figure 2 The hidden area mapping effect of a hidden area hybrid topological map construction method of the present invention;

[0038] Figure 3 The hidden area mapping and laser mapping effects of a hidden area hybrid topological map construction method of the present invention;

[0039] Figure 4 The invention discloses a hidden area hybrid topology map effect of a hidden area hybrid topology map construction method. DETAILED DESCRIPTION

[0040] The technical solution of the present invention is further described below through the accompanying drawings and embodiments.

[0041] Unless otherwise defined, the technical terms or scientific terms used in the present invention should be understood by people with ordinary skills in the field to which the present invention belongs. The words "first", "second" and similar words used in the present invention do not indicate any order, quantity or importance, but are only used to distinguish different components. "Include" or "comprise" and similar words mean that the elements or objects appearing before the word include the elements or objects listed after the word and their equivalents, without excluding other elements or objects. "Connect" or "connected" and similar words are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect. "Up", "down", "left", "right" and the like are only used to indicate relative positional relationships. When the absolute position of the described object changes, the relative positional relationship may also change accordingly.

[0042] Example

[0043] See also Figure 1-4 The present invention provides a method for constructing a hidden area hybrid topology map, comprising the following steps:

[0044] Step S1: Input a laser point cloud of size L×P and use a k-dimensional tree to create an index, where L is a laser beam and P is a point on each laser beam that is scanned around.

[0045] Step S2: filter the ground of the input laser point cloud to remove the interfering laser point cloud to ensure the accuracy of segmentation, and use straight-through filtering to filter the laser point cloud above the ground H. is to satisfy the inequality constraint The minimum approximate rational number of , where R is the height of the robot itself and L is the length of the robot itself.

[0046] Step S3: Calculate the normal of each point in the filtered laser point cloud and input it into Region Growing clustering to obtain point cloud cluster C. i , where i is the point cloud cluster sequence, and the typical value smoothness S t is 10 degrees, the curvature C t is 1.

[0047] Step S4: According to the point cloud cluster C i In the point cloud, the point cloud clusters with more than 100 points are selected as candidate point cloud clusters, and the minimum point p(x min ,y0,z0) and the maximum point p(x1,y max ,z1) and the minimum point p(x2,y min ,z2), generate candidate hidden points to be processed Use a double random algorithm to take values ​​within a preset range, typically [0.5, 1], and calculate the relative position of the hidden point to the radar. The specific method for calculating the relative position of the hidden point is as follows:

[0048] S41. Use a Gaussian distribution function with a mean of 0 and a variance of 1 to randomly generate and truncate to obtain a random value a, a∈(-1,1);

[0049] S42, the random value is a positive number. At this time, a>0, then the uniform distribution random generation method is selected to generate a uniform distribution random number u in the interval [0.5,1] to obtain the candidate hidden point

[0050] S43, if the random value is a non-positive number and a≤0, the truncated exponential distribution algorithm is used to generate random values ​​within a given range. Specifically, it includes:

[0051] Calculate the cumulative distribution values ​​F(a) and F(b) in the interval [a,b]. The expressions are as follows:

[0052] F(a)=1-e -λa ;

[0053] F(b)=1-e -λb ;

[0054] After obtaining the cumulative distribution values ​​F(a) and F(b), a uniformly distributed random number u is generated in the interval [F(a), F(b)], and an exponentially distributed random number x is obtained. The expression of x is as follows:

[0055]

[0056] Among them, the typical values ​​are a=0.5, b=1, λ=1;

[0057] Finally, we get the candidate hidden points

[0058] Step S5: Use the mapping algorithm in the outdoor environment to obtain the real-time pose estimation of the robot at the current coordinates, generate a transformation matrix based on the pose, and the transformation matrix between the laser radar and the vehicle body coordinate system is obtained by measurement. l Register to the global coordinate system to get the global hidden point p w , p w The expression is as follows:

[0059]

[0060] Among them, R bl and t bl are the rotation and translation matrices that transform a point in the radar coordinate system to the vehicle chassis coordinate system, R wb, and t bl They are the rotation and translation matrices that transform a point under the vehicle chassis to the map coordinate system.

[0061] Repeat this process to build a priori map of the hidden area.

[0062] Step S6: Obtain the latitude and longitude coordinates (lat0, lon0) at the beginning of the map construction in step 5 as the origin of the initial coordinate system of the OSM map, establish a local Cartesian coordinate system based on this origin, and then obtain the robot's six sets of local Cartesian coordinate system pose data at different times in the RTK differential mode during the robot's mapping motion interval. , and the pose estimation aligned with this time in laser mapping Among them, i∈[1,6], k∈[1,6].

[0063] Step S7: perform least square estimation on the six sets of position data in the local Cartesian coordinate system and the synchronous positions of the map construction to obtain a set of transformation matrices.

[0064] Step S8: Apply the transformation matrix to the hidden area prior map and the laser point cloud map respectively, and convert all laser point clouds to a map based on the local coordinate system of the OSM map to generate a hidden area hybrid map.

[0065] Therefore, the present invention adopts the above-mentioned method for constructing a hybrid topological map of a hidden area, based on laser radar, using the point cloud Region Growing clustering method to find hidden areas, can work under different lighting and weather conditions, and segment the hidden areas to be screened; based on the number of points in the point cloud cluster, the point cloud clusters obtained by clustering are screened, and then the local relative positions of the hidden points are obtained using a double random algorithm, which increases the randomness of the hidden point selection; the hidden area mapping and SLAM mapping are realized simultaneously, and the hidden area point cloud map, laser SLAM point cloud map and OSM topological map are fused into a hidden area hybrid topological map using a conversion matrix. The robot is provided with a priori hidden area information and the storage medium occupancy is reduced.

[0066] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that they can still modify or replace the technical solution of the present invention with equivalents, and these modifications or equivalent replacements cannot cause the modified technical solution to deviate from the spirit and scope of the technical solution of the present invention.

Claims

1. A method for constructing a hidden area hybrid topology map, characterized in that: The following steps are involved: Step S1, input laser point cloud and use k-dimensionaltree to create index; Step S2, filtering the ground of the input laser point cloud to remove the interfering laser point cloud to ensure the accuracy of segmentation, and filtering the laser point cloud above the ground H using a straight-through filter; Step S3: Calculate the normal of each point in the filtered laser point cloud and input it into Region Growing clustering to obtain point cloud cluster C. i , where i is the point cloud cluster sequence, and the typical value smoothness S t is 10 degrees, the curvature C t is 1; Step S4: According to the point cloud cluster C i In the point cloud, the point cloud clusters with more than 100 points are selected as candidate point cloud clusters, and the minimum point p(x min ,y0,z0) and the maximum point p(x1,y max ,z1) and the minimum point p(x2,y min ,z2), generate candidate hidden points to be processed Use a double random algorithm to take values ​​within a preset range, typically [0.5, 1], and calculate the position of the hidden point relative to the radar; Step S5: Use the mapping algorithm in the outdoor environment to obtain the real-time pose estimation of the robot at the current coordinates, generate a transformation matrix based on the pose, and the transformation matrix between the laser radar and the vehicle body coordinate system is obtained by measurement. l Register to the global coordinate system to get the global hidden point p w , repeat the process to build a priori map of hidden areas; Step S6: Obtain the longitude and latitude coordinates (lat0, lon0) at the beginning of the mapping in step 5 as the origin of the initial coordinate system of the OSM map, establish a local Cartesian coordinate system based on this origin, and then obtain the robot's six sets of local Cartesian coordinate system pose data at different times in the RTK differential mode during the robot's mapping motion interval. And the pose estimation aligned with this time in laser mapping Among them, i∈[1,6], k∈[1,6]; Step S7, performing least square estimation on the six sets of position data in the local Cartesian coordinate system and the synchronous positions of the map construction to obtain a set of transformation matrices; Step S8: Apply the transformation matrix to the hidden area prior map and the laser point cloud map respectively, and convert all laser point clouds to a map based on the local coordinate system of the OSM map to generate a hidden area hybrid map.

2. A method for constructing a hidden area hybrid topology map according to claim 1, characterized in that: The size of the laser point cloud input in step S1 is L×P, where L is the laser beam and P is the point on each laser beam that is scanned once.

3. A method for constructing a hidden area hybrid topology map according to claim 2, characterized in that: In the step S2, is to satisfy the inequality constraint The minimum approximate rational number of , where R is the height of the robot itself and L is the length of the robot itself.

4. A method for constructing a hidden area hybrid topology map according to claim 3, characterized in that: In step S4, the specific method for calculating the relative position of the hidden point is as follows: S41. Use a Gaussian distribution function with a mean of 0 and a variance of 1 to randomly generate and truncate to obtain a random value a, a∈(-1,1); S42, the random value is a positive number. At this time, a>0, then the uniform distribution random generation method is selected to generate a uniform distribution random number u in the interval [0.5,1] to obtain the candidate hidden point S43, the random value is a non-positive number. In this case, a≤0, and the truncated exponential distribution algorithm is used to generate a random value within a given range.

5. A method for constructing a hidden area hybrid topology map according to claim 4, characterized in that: The step S43 specifically includes: Calculate the cumulative distribution values ​​F(a) and F(b) in the interval [a,b]. The expressions are as follows: F(a)=1-e -λa ; F(b)=1-e -λb ; After obtaining the cumulative distribution values ​​F(a) and F(b), a uniformly distributed random number u is generated in the interval [F(a), F(b)], and an exponentially distributed random number x is obtained. The expression of x is as follows: Among them, the typical values ​​are a=0.5, b=1, λ=1; Finally, we get the candidate hidden points 6. A method for constructing a hidden area hybrid topology map according to claim 5, characterized in that: In step S5, p w The expression is as follows: Among them, R bl and t bl are the rotation and translation matrices that transform a point in the radar coordinate system to the vehicle chassis coordinate system, R wb , and t bl They are the rotation and translation matrices that transform a point under the vehicle chassis to the map coordinate system.

Images