A method for constructing a hidden area hybrid topology map

Abstract

The present invention discloses a method for constructing a hidden region hybrid topology map, which relates to the field of robot perception and mapping technology. The method comprises the following steps: S1, inputting a laser point cloud and using a k-dimensional tree to establish an index; S2, filtering out the ground of the input laser point cloud; S3, calculating the normal of each point in the filtered laser point cloud and inputting the normal into Region Growing clustering to obtain a point cloud cluster C. i ; S4, according to the point cloud cluster C i In the point cloud, point cloud clusters with more than 100 points are selected as candidate point cloud clusters; S5. A priori map of the hidden area is established; S6. The latitude and longitude coordinates (lat0, lon0) at the time of initial mapping in step 5 are obtained, and a local Cartesian origin is established based on this origin; S7. The position data in the six local Cartesian coordinate systems are least squares estimated with the synchronized positions of the mapping to obtain a set of transformation matrices; S8. A hybrid map of the hidden area is generated. The present invention uses the above-mentioned method for constructing a hybrid topological map of the hidden area to provide the robot with a priori hidden area information and reduce the storage medium usage.

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 commonly used to perform various mobile tasks, such as inspections, patrols, and food and delivery services. These patrol and inspection mobile robots are often equipped with high-value sensors and typically operate in unstructured, semi-dynamic environments. Encountering a suspicious individual during patrols or inspections can easily result in significant asset losses and require lengthy repairs before the robot can resume operations. Furthermore, pure laser mapping is ineffective in open outdoor areas and on large maps, and requires significant storage 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 destroyed, and at the same time provide the robot with an OSM-based topological map in open areas 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 the GPS-based space, a priori map of the hidden area is provided for the robot in the GPS-based environment, which facilitates the robot to plan tracking or escape routes when encountering suspicious persons and reduces the probability of being destroyed; in areas with good GPS, an OSM-based topological map is provided to reduce the robot's energy consumption 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 the laser point cloud and use the k-dimensional tree to create an index;

[0007] 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;

[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, 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 of the laser radar and the vehicle 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 the hidden area;

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

[0012] Step S7: perform least squares estimation on the six sets of position data in the local Cartesian coordinate system and the synchronous positions of the mapping 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 scanned on each laser beam.

[0015] Preferably, in step S2, satisfies 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 the 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. 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.

[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), generate a uniformly distributed random number u in the interval [F(a), F(b)] and obtain an exponentially distributed random number x. The expression for x is as follows:

[0025]

[0026] Among them, typical values ​​are a=0.5, b=1, and λ=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 convert 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 Regional 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, which increases 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 storage medium occupancy.

[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 This is a flow chart of an embodiment of a method for constructing a hidden area hybrid topology map according to the present invention;

[0037] Figure 2 This is the hidden area mapping effect of a hidden area hybrid topology 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 This is a hidden area hybrid topology map effect of a hidden area hybrid topology map construction method of the present invention. DETAILED DESCRIPTION

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

[0041] Unless otherwise defined, the technical or scientific terms used in the present invention shall have the usual meanings understood by persons of ordinary skill in the field to which the present invention belongs. The words "first", "second" and similar terms used in the present invention do not indicate any order, quantity or importance, but are only used to distinguish different components. Words such as "include" or "comprise" mean that the elements or objects preceding the word include the elements or objects listed after the word and their equivalents, without excluding other elements or objects. Words such as "connect" or "connected" 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 object being described 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 create an index using a k-dimensional tree, where L is the laser beam and P is the number of points scanned on each laser beam.

[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. satisfies 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, 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 obtain values ​​within a preset range, typically [0.5, 1], and calculate the position of the hidden point relative 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 the 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 non-positive and a≤0, the truncated exponential distribution algorithm is used to generate random values ​​within a given range. Specifically, the following are included:

[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), generate a uniformly distributed random number u in the interval [F(a), F(b)] and obtain an exponentially distributed random number x. The expression for x is as follows:

[0055]

[0056] Among them, typical values ​​are a=0.5, b=1, and λ=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 of the laser radar and the vehicle 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 convert 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 mapping in step 5 as the origin of the initial coordinate system of the OSM map. Use this origin to establish a local Cartesian coordinate system. Then, during the robot's mapping motion interval, obtain the robot's six sets of local Cartesian coordinate system pose data at different times in RTK differential mode. , and the pose estimation aligned with this time in laser mapping Among them, i∈[1,6], k∈[1,6].

[0063] Step S7: perform least squares estimation on the six sets of position data in the local Cartesian coordinate system and the synchronous positions of the mapping 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 hidden areas. Based on laser radar, it uses a point cloud Regional Growth clustering method to find hidden areas. It can work under different lighting and weather conditions and segment the hidden areas to be screened. The clustered point cloud clusters are screened based on the number of points in the point cloud clusters. Then, a double randomization algorithm is used to determine the local relative positions of hidden points, increasing the randomness of hidden point selection. The hidden area mapping and SLAM mapping are simultaneously achieved. The hidden area point cloud map, laser SLAM point cloud map, and OSM topological map are fused into a hybrid topological map of the hidden area using a transformation matrix. This provides the robot with a priori hidden area information and reduces storage medium usage.

[0066] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention rather than to limit the same. 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 solutions of the present invention with equivalents, and these modifications or equivalent replacements cannot cause the modified technical solutions to deviate from the spirit and scope of the technical solutions 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: 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; 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, 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 of the laser radar and the vehicle 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 the hidden area; Step S6: Obtain the latitude and longitude coordinates (lat0, lon0) at the beginning of the mapping in step 5 as the origin of the initial coordinate system of the OSM map. Use this origin to establish a local Cartesian coordinate system. Then, during the robot's mapping motion interval, obtain the robot's six sets of local Cartesian coordinate system pose data at different times in RTK differential mode. And the pose estimation aligned with this time in laser mapping Among them, i∈[1,6], k∈[1,6]; Step S7: performing least squares estimation on the six sets of position data in the local Cartesian coordinate system and the synchronous positions of the mapping 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. The method for constructing a hidden area hybrid topology map according to claim 1, wherein: 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 scanned on each laser beam.

3. The method for constructing a hidden area hybrid topology map according to claim 2, wherein: 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 the 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. 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.

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), generate a uniformly distributed random number u in the interval [F(a), F(b)] and obtain an exponentially distributed random number x. The expression for x is as follows: Among them, typical values ​​are a=0.5, b=1, and λ=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 convert a point under the vehicle chassis to the map coordinate system.

Images