Hidden area autonomous recognition algorithm for patrol robots based on 3D lidar

Abstract

The present invention discloses an autonomous recognition algorithm for hidden areas of a patrol robot based on 3D laser radar, and relates to the field of artificial intelligence technology. The autonomous recognition algorithm for hidden areas of a patrol robot based on 3D laser radar provided by the present invention includes adopting a ground point cloud filtering algorithm to obtain non-ground point clouds; for the obtained non-ground point clouds, concentric circle areas are divided according to Euclidean distance, and the point clouds of the concentric circle areas are respectively subjected to Euclidean clustering to obtain clustered point cloud clusters; according to the point cloud clusters, axis-aligned bounding boxes are calculated, and the coordinates of hidden points are obtained; according to the point cloud clusters, their directional bounding boxes are calculated, and the coordinates of the blocked points are calculated by line of sight blocking, thereby automatically calculating the blocked angles. Therefore, by adopting the above method and utilizing 3D laser radar data, it is possible to reasonably select hidden areas according to angle information during the patrol process, achieve a hidden effect, and better complete the patrol task, and have high robustness and practicality.

Description

Technical Field

[0001] The present invention relates to the field of artificial intelligence technology, and in particular to an autonomous recognition algorithm for hidden areas of a patrol robot based on a 3D laser radar. Background Art

[0002] The digital age and the development of information technology are constantly changing human lifestyles. The rapid advancement of artificial intelligence, in particular, has provided new solutions for patrol work. Patrol robots are intelligent robots capable of autonomous patrols and remote monitoring. Different scenarios require different functional capabilities, such as facial recognition, behavioral recognition, license plate recognition, and stranger detection, effectively providing security for industrial parks. Compared to traditional video surveillance systems, patrol robots not only provide monitoring capabilities but also offer greater flexibility and maneuverability, a wide reconnaissance range, and minimal blind spots, making them an effective alternative to human patrols.

[0003] Patrol robots are intelligent robots that integrate environmental perception, modeling, and decision-making capabilities. They can replace most manual security patrols and improve patrol efficiency. Unlike industrial or transport robots, patrol robots must possess certain anti-reconnaissance capabilities to quickly react and protect themselves in dangerous situations. However, current patrol robots generally lack self-protection awareness and mechanisms, making them vulnerable to detection and even sabotage during their missions, leading to patrol mission failures and property damage. Therefore, patrol robots' awareness of hidden areas is crucial during patrols.

[0004] Early biologists Kiltie and Laine pointed out that camouflage could be combined with computer vision. Researchers began developing various camouflage techniques based on computer vision, leveraging the surrounding environment to blend in. Equipment camouflage primarily uses deformation camouflage. This technique uses patches of various colors and irregular shapes on the target surface to segment the target's outline, creating a smooth blend with the natural background and reducing its conspicuity. However, in most scenarios, the environment in which the camouflaged person resides is not suitable for camouflage equipment, resulting in low generalization capabilities. Some researchers have developed electronic skin that can be gently and compactly integrated into fully soft robots. However, due to limitations such as high manufacturing material costs and limited wearability, electronic skin remains in its early stages of development. Zhang Botao et al. used fuzzy mapping functions to assess color and texture similarity to select the optimal protection zone for a robot. However, this method is affected by lighting and viewing angles and requires further optimization. Furthermore, the "Chameleon" designed by researchers at Seoul National University features an electronic skin composed of a thermochromic liquid crystal layer and silver nanomaterials, enabling real-time blending into the background. This represents a major breakthrough in artificial camouflage technology.

[0005] However, existing technologies all focus on changing the camouflage's color and appearance to blend in with the background, without considering the role of the robot's active actions and recognition of the hidden environment in concealment. For example, a lion on the grassland often uses the cover of grass and trees to slowly approach its target before making a final strike. Choosing a suitable hiding environment and a light and agile posture can effectively facilitate reconnaissance and hunting. On the battlefield, scouts often wear camouflage to blend in with the environment and use obstructions to hide, effectively avoiding opponents. In different situations, patrol robots can act as both pursuers and evaders, concealing themselves during the pursuit and preventing themselves from being destroyed by criminals during the pursuit.

[0006] Therefore, it is urgent to design an autonomous identification method for hidden areas in a three-dimensional environment. The hidden areas in the environment can be identified through 3D lidar point cloud clustering, and the occlusion angle analysis of the hidden areas can be performed to obtain the target points in the hidden areas. This can provide a reference basis for selecting reasonable hidden areas during the robot patrol process, reduce the risk of being discovered by criminals, and thus better complete the patrol mission. Summary of the Invention

[0007] The purpose of this invention is to provide an autonomous recognition algorithm for hidden areas of patrol robots based on 3D lidar, which can use 3D lidar point cloud data to identify hidden areas and obtain information on obstructible angles, so that the patrol robot can better hide itself during work.

[0008] To achieve the above objectives, the present invention provides an autonomous recognition algorithm for hidden areas of a patrol robot based on 3D laser radar, comprising the following steps:

[0009] S1. Preprocess the 3D point cloud scanned by the LiDAR. Use the ground point cloud filtering algorithm to segment the preprocessed 3D point cloud. Then, perform threshold judgment based on the relationship between the height difference of adjacent point clouds, local height values, and the LiDAR installation height to obtain non-ground point clouds.

[0010] S2. Divide the acquired non-ground point cloud into concentric circle areas according to the distance, and perform Euclidean clustering on the point cloud in each concentric circle area to obtain clustered point cloud clusters;

[0011] S3. Based on the obtained clustered point cloud clusters, construct an axis-aligned bounding box, calculate the centroid coordinates and mean center coordinates of the clustered point cloud clusters, and determine the concavity and convexity of the hidden area, and then select the target point in the hidden area according to the robot size;

[0012] S4. Based on the obtained clustered point cloud clusters, an oriented bounding box is constructed to obtain the three-dimensional axis direction of the oriented bounding box. The position of the line of sight occlusion point is obtained through the line of sight occlusion method. Combined with the target point position, the occlusion point angle information of the hidden area is obtained.

[0013] Preferably, step S1 includes:

[0014] S11. Obtain the installation height of the laser radar relative to the ground, and segment the point cloud based on the laser radar coordinate system and the robot height as the threshold, and retain the point cloud data below the threshold;

[0015] S12. Differentiate the processed point cloud data horizontally according to the horizontal angular resolution of the laser radar, divide the point cloud data into sectors perpendicular to the ground in a clockwise direction, and sort the point clouds in each sector in ascending order according to the Euclidean distance of the point cloud from the origin of the radar coordinate system.

[0016] S13, performing local straight line fitting on adjacent point clouds in each sector area, and calculating the local height and global height of the current point cloud according to the local slope height threshold and the global slope height threshold;

[0017] S14, judging the ground point cloud and the non-ground point cloud by using a threshold value based on the relationship between the height difference of adjacent point clouds, the local height, and the installation height of the lidar;

[0018] S15, repeating operations S12 to S14 until all ground point cloud and non-ground point cloud data are extracted. Preferably, in step S13, the local height and global height of the current point cloud are calculated as follows:

[0019] M i =(d i -d i-1 )*tan(SL)

[0020] N i =d i *tan(SG)

[0021] Where M i 、N i are the local height and global height of the current point cloud, d i is the Euclidean distance between the current point cloud and the origin of the radar coordinate system, d i-1 is the Euclidean distance between the previous adjacent point cloud and the origin of the radar coordinate system, SL and SG are the local slope height threshold and the global slope height threshold, respectively.

[0022] Preferably, in step S14, the ground point cloud and the non-ground point cloud are judged by a threshold value, which is specifically expressed as follows:

[0023]

[0024] Where z i 、z i-1The height of the i-th and i-1-th point clouds, L is the threshold, H lidar is the installation height of the lidar relative to the ground.

[0025] Preferably, step S2 includes using thresholds of 15m and 30m to divide the non-ground point cloud into three concentric areas through a straight-through filter; and setting different clustering distance thresholds in each concentric area to find the neighborhood of the point, thereby obtaining clustered point cloud clusters, as follows:

[0026] Q={P j |d(P i ,P j )<ε}

[0027] Where, P i is the coordinate of the i-th point in the clustered point cloud cluster, Q is the clustered point cloud cluster, P j For P i The coordinates of points whose distance is within the clustering distance threshold, ε is the clustering distance threshold.

[0028] Preferably, step S3 includes:

[0029] S31. In the laser radar coordinate system, traverse all point cloud coordinates in the clustered point cloud cluster and find the maximum and minimum values ​​in the directions of the three coordinate axes X, Y, and Z respectively;

[0030] S32. Based on the obtained maximum and minimum values ​​in each coordinate axis direction, axis-aligned borders are sequentially connected and drawn, and the centroid coordinates and mean center coordinates of the clustered point cloud cluster are calculated. The specific expression is as follows:

[0031]

[0032] Where, P sc (x, y, z) is the centroid coordinate of the cluster point cloud, P qc (x, y, z) is the mean center coordinate of the cluster point cloud cluster, P i (x, y, z) is the coordinate of the i-th point, n is the number of point clouds in the cluster, x max 、y max 、z max They are the maximum values ​​of the three coordinate axes X, Y, and Z, respectively. min 、y min 、z min They are the minimum values ​​in the directions of the three coordinate axes X, Y, and Z respectively;

[0033] S33. Based on the obtained centroid coordinates and mean center coordinates, the distances from the origin of the laser radar coordinate system are calculated, and the Euclidean distance value is used to determine the concavity and convexity of the area, as follows:

[0034]

[0035] S34. Based on the identified concave and convex areas, select target points according to the size of the robot.

[0036] Preferably, step S34 includes:

[0037] The target point of the concave area is selected as:

[0038] x target_hid =x min -x r

[0039] y target_hid =(y max +y min ) / 2

[0040] The target points of the convex area are selected as:

[0041] x target_hid =p qc (x)-x r

[0042] y target_hid =(y max +y min ) / 2

[0043] Where x r is the radius of the robot's minimum circumcircle, (x target_hid ,y target_hid ) is the plane coordinate of the target point.

[0044] Preferably, step S4 includes:

[0045] S41. Perform directional bounding box extraction on the obtained clustered point cloud cluster, obtain the direction of the three-dimensional axis of the clustered point cloud cluster, and calculate the rotation amount of the main axis direction relative to the radar coordinate system to obtain the vertex coordinates in the centralized point cloud coordinate system and obtain the rotation matrix;

[0046] S42. According to the obtained rotation matrix, the vertex coordinates in the centralized point cloud coordinate system are transformed into the radar coordinate system. The transformation formula is as follows:

[0047] G i =R×L i +P qc

[0048] Where G i is the coordinate of the i-th vertex in the radar coordinate system after transformation, R is the rotation matrix, L i is the coordinate of the i-th vertex in the centralized point cloud coordinate system, and M is the translation matrix;

[0049] S43. Divide the point cloud area into two left and right areas using the positive direction of the laser radar as the dividing line. Extract the lower and right vertices of the bounding box of the clustered point cloud cluster in the left area, and the left and lower vertices of the bounding box of the clustered point cloud cluster in the right area, based on the coordinates of the mean center point of the clustered point cloud cluster in the radar coordinate system. Connect the two vertices of the bounding box of the clustered point cloud cluster in each area to form a radial line segment. Calculate the horizontal angle between the radial line segment and the robot based on the vertex coordinates and the horizontal axis of the laser radar.

[0050] S44. According to the concave-convex properties of the clustered point cloud clusters, the positions of the clustered point cloud clusters, and the calculated horizontal angle, the line of sight blocking points are extracted through the shortest distance from the point line or the point to the vertex, thereby obtaining the line of sight blocking angle information.

[0051] Preferably, step S41 includes:

[0052] First, the clustered point cloud cluster is centralized. For any clustered point cloud cluster, its mean center coordinates are calculated as the geometric center, and the clustered point cloud cluster is transformed into a coordinate system with the center coordinates as the origin to obtain the centralized point cloud cluster.

[0053] Secondly, the degree of discreteness of the obtained centralized point cloud cluster is calculated, that is, the covariance matrix H of the point cloud in three-dimensional space is calculated as follows:

[0054] Q(P i )'=Q(P i )-P qc

[0055]

[0056] Where, Q(P i )'、Q(P i ) are the coordinates of the i-th point in the clustered point cloud cluster in the centralized point cloud coordinate system and the radar coordinate system respectively;

[0057] Then use the obtained covariance matrix H to perform eigenvalue decomposition to obtain three eigenvalues ​​λ1, λ2, λ3 and their corresponding eigenvectors v1, v2, v3 to obtain the direction vectors of the three main axes of the oriented bounding box;

[0058] Then, in the obtained centralized point cloud cluster, according to the direction vectors of the three main axes, traverse the point cloud cluster to obtain the maximum distance in the three main axis directions, thereby obtaining the four vertex coordinates of the oriented bounding box in the centralized point cloud coordinate system;

[0059] Based on the obtained principal axis direction vector of the centralized point cloud, its rotation matrix relative to the radar coordinate system is calculated, and a 3×3 moment of inertia tensor matrix I is constructed in the centralized point cloud coordinate system, as follows:

[0060]

[0061] The calculation formula of the inertia moment tensor matrix elements is as follows:

[0062]

[0063] In the formula, (x′ i ,y′ i ,z′ i ) is the coordinate of the i-th point in the centralized point cloud coordinate system;

[0064] Perform eigenvalue decomposition on the obtained moment of inertia tensor matrix to obtain eigenvectors v4, v5, and v6, and the rotation matrix is ​​R = [v4, v5, v6].

[0065] Preferably, step S44 includes connecting the target point and the sight blocking point to obtain the sight blocking angle information of the current hidden area, which is specifically expressed as follows:

[0066]

[0067] Where, P target_edge (x,y)P target_hid (x,y) are the plane coordinates of the occlusion point and the target point respectively.

[0068] Therefore, the present invention adopts the above-mentioned autonomous recognition algorithm of the hidden area of ​​the patrol robot based on 3D laser radar, which has the following technical effects:

[0069] (1) Based on 3D lidar data and combined with ground filters, the interference of ground point clouds on the perception of hidden areas can be reduced, and a 3D point cloud space suitable for perceiving hidden areas can be constructed, thereby enabling patrol robots to identify hidden areas during work.

[0070] (2) Concentric circle areas are used for Euclidean clustering processing to reduce the computational complexity of data processing while retaining the spatial distribution information of the point cloud. The point cloud data is subjected to straight-through filtering according to the height threshold to ensure the accuracy of point cloud data segmentation.

[0071] (3) By automatically calculating the hidden area and the hiding angle, the continuity of the navigation process can be ensured, and the obstructible angle can be used to provide a reasonable hiding area for the patrol robot, thus realizing the autonomous hiding function of the patrol robot.

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

[0073] Figure 1This is a flowchart of the overall implementation of an embodiment of the present invention;

[0074] Figure 2 1 is a schematic diagram of regional differentiation of radar point clouds in an embodiment of the present invention;

[0075] Figure 3 1 is a schematic diagram of ground point filtering in an embodiment of the present invention, wherein (a) is a schematic diagram of a ground point, and (b) is a schematic diagram of a non-ground point;

[0076] Figure 4 is a block diagram of a ground point determination algorithm according to an embodiment of the present invention;

[0077] Figure 5 Schematic diagram of occlusion point selection in an embodiment of the present invention, where (a) is the concave area on the left, (b) is the convex area on the left, (c) is the concave area on the right, and (d) is the convex area on the right;

[0078] Figure 6 : This is the identification of hidden points and blocked points in the overhead garage in an embodiment of the present invention, where (a) is the recognition result of scene one, (b) is the recognition result of scene two, (c) is the recognition result of scene three, and (d) is the recognition result of scene four;

[0079] Figure 7 This is the identification of hidden points and blocked points in an outdoor parking lot in an embodiment of the present invention, where (a) is the recognition result of scene one, (b) is the recognition result of scene two, (c) is the recognition result of scene three, and (d) is the recognition result of scene four. DETAILED DESCRIPTION

[0080] The present invention can be explained in more detail by the following examples. The purpose of disclosing the present invention is to protect all changes and improvements within the scope of the present invention. The present invention is not limited to the following examples.

[0081] like Figure 1 As shown, the autonomous recognition algorithm of hidden areas of patrol robots based on 3D laser radar provided by the present invention includes the following steps:

[0082] S1. Preprocess the 3D point cloud scanned by the LiDAR and use the ground point cloud filtering algorithm to obtain the non-ground point cloud. The details are as follows:

[0083] S11. Obtain the installation height H of the laser radar relative to the ground lidar The laser radar coordinate system is used as the reference, and the robot height H is used robot The point cloud is segmented and the point cloud data below the threshold is retained.

[0084] S12, such as Figure 2As shown in the figure, the point cloud data is differentiated in the horizontal direction according to the horizontal angular resolution α of the laser radar, and divided into 360 / α fan-shaped areas perpendicular to the ground in the clockwise direction. For each point P in the fan-shaped area i (x i ,y i ,z i ) In the XOY plane, the Euclidean distance d from the origin i Arrange in ascending order, the Euclidean distance calculation formula is:

[0085]

[0086] S13, perform local straight line fitting on the adjacent points in the obtained fan-shaped area, and calculate the local height M of the current point according to the local slope height threshold SL and the global slope height threshold SG. i and global height N i , the expression is as follows:

[0087] M i =(d i -d i-1 )*tan(SL)

[0088] N i =d i *tan(SG)

[0089] S14, based on the relationship between the height difference of adjacent point clouds, local height, and the installation height of the lidar, the ground point cloud and the non-ground point cloud are judged by the threshold, such as Figure 3 and Figure 4 As shown, the details are as follows:

[0090]

[0091] S15. Repeat steps S12, S13, and S14 until the ground point cloud and non-ground point cloud data of all sector-shaped areas are extracted.

[0092] S2. Divide the segmented non-ground point cloud into regions based on distance. Use a straight-through filter with a distance threshold of 15m and 30m to divide the non-ground point cloud into three concentric circle regions. Based on the divided concentric circle point cloud regions, perform Euclidean clustering. The specific steps are as follows:

[0093] The non-ground point clouds in the three concentric circle areas are clustered according to the Euclidean distance, which is specifically expressed as follows:

[0094]

[0095] Where d(P1,P2) is the Euclidean distance between points P1 and P2.

[0096] Different clustering distance thresholds are set in three different concentric circle areas, that is, ε1 is between 0 and 15m, ε2 is between 15 and 30m, and ε3 is greater than 30m. In the non-ground point cloud, find each point P i The neighborhood Q of point P forms a cluster point cloud cluster. The neighborhood is defined as all points P i Point P whose distance is less than ε j , specifically expressed as follows:

[0097] Q={P j |d(P i ,P j )<ε}

[0098] S3. Based on the obtained clustered point cloud cluster, calculate its axis-aligned bounding box and obtain the coordinates of the hidden points as follows:

[0099] S31. In the laser radar coordinate system, traverse all the point cloud coordinates in the cluster point cloud cluster and find the maximum and minimum values ​​in the X, Y, and Z coordinate axis directions (x min ,x max )、(y min ,y max )、(z min ,z max ).

[0100] S32, according to the maximum and minimum values ​​of each coordinate axis direction, draw the axis-aligned border in sequence, and calculate the centroid coordinates P of the cluster point cloud cluster sc and the mean center coordinates P qc , specifically expressed as follows:

[0101]

[0102] S33. Based on the obtained centroid coordinates and mean center coordinates, calculate the Euclidean distance from the origin of the laser radar coordinate system, and use the Euclidean distance value to determine the concavity and convexity of the area, as follows:

[0103]

[0104] S34. Based on the identified concave and convex areas, select the target point according to the size of the robot, where the radius of the robot's minimum circumscribed circle is x. r .

[0105] The target points of the concave area are selected as follows:

[0106] x target_hid =x min -x r

[0107] y target_hid =(y max +y min ) / 2

[0108] The target points of the convex area are selected as follows:

[0109] x target_hid =p qc (x)-x r

[0110] y target_hid =(y max +y min ) / 2

[0111] The target points in the concave and convex areas are selected as hidden points.

[0112] S4. Calculate the oriented bounding box of the obtained clustered point cloud cluster and calculate the coordinates of the occlusion point using the line of sight occlusion method, including the following steps:

[0113] S41 , extracting a directional bounding box from the obtained clustered point cloud cluster, obtaining the three main axis directions of the clustered point cloud cluster, and calculating the rotation amount of the main axis directions relative to the coordinate system.

[0114] First, the cluster point cloud cluster is centralized. For any cluster point cloud cluster, its mean center coordinate is calculated as the geometric center, and the cluster point cloud cluster is converted to a coordinate system with the center coordinate as the origin. The specific expression is as follows:

[0115] Q(P i )'=Q(P i )-P qc

[0116] Where, Q(P i )'、Q(P i ) are the coordinates of the i-th point in the clustered point cloud cluster in the centralized point cloud coordinate system and the radar coordinate system, respectively.

[0117] Secondly, the degree of discreteness of the obtained centralized point cloud cluster is calculated, that is, the covariance matrix H of the point cloud is calculated in three-dimensional space. The formula is:

[0118]

[0119] The obtained covariance matrix H is then used to perform eigenvalue decomposition to obtain the directions of the three-dimensional axes of the oriented bounding box, resulting in three eigenvalues ​​λ1, λ2, and λ3 and the corresponding eigenvectors v1, v2, and v3. The eigenvector with the largest eigenvalue is the principal axis, and the eigenvector with the smallest eigenvalue is the minimum variance direction.

[0120] Then, in the obtained centralized point cloud, according to the obtained three-dimensional axis direction vector, traverse the point cloud cluster to obtain the maximum distance d in the three axis directions x ,d y ,d z Based on this, in the centralized point cloud coordinate system, the eight vertex coordinates L of the bounding box are i (-d x ,d y ,-d z ), (-d x ,d y ,d z ), (-d x ,-d y ,-d z ), (-d x ,-d y ,d z ), (d x ,d y ,-d z ), (d x ,d y ,d z ), (d x ,-d y ,-d z ), (d x ,-d y ,d z ).

[0121] Based on the obtained principal axis direction of the centralized point cloud, its rotation matrix relative to the radar coordinate system is calculated, and a 3x3 moment of inertia tensor matrix I is constructed in the centralized point cloud coordinate system. The relationship between the coordinate value of each point in the centralized point cloud cluster relative to the center of mass and the inertia tensor matrix is ​​as follows:

[0122]

[0123] The calculation formula of the inertia moment tensor matrix elements is as follows:

[0124]

[0125] In the formula, (x′ i ,y′ i ,z′ i ) is the coordinate of the i-th point in the centralized point cloud coordinate system.

[0126] Finally, the obtained moment of inertia tensor matrix is ​​subjected to eigenvalue decomposition to obtain the eigenvectors v4, v5, and v6, and the rotation matrix R is:

[0127] R=[v4,v5,v6]

[0128] S42. According to the obtained rotation matrix, the eight vertex coordinates in the centralized point cloud coordinate system are transformed into the radar coordinate system. The transformation formula is as follows:

[0129] G i =R×L i +P qc

[0130] Where G i is the vertex coordinate in the transformed radar coordinate system.

[0131] S43. Divide the point cloud area into two areas on the left and right sides with the positive direction of the laser radar as the dividing line. According to the coordinates of the mean center point of the clustered point cloud cluster in the radar coordinate system, extract the lower vertex and right vertex of the border of the clustered point cloud cluster in the left area, and the left vertex and lower vertex of the border of the clustered point cloud cluster in the right area respectively. Connect the two vertices extracted on the border of the clustered point cloud cluster in each area into a radial line segment. Calculate the horizontal angle between the radial line segment and the robot based on the vertex coordinates and the horizontal axis of the laser radar.

[0132] S44. Based on the concavity and convexity of the clustered point cloud clusters, the positions of the clustered point cloud clusters, and the calculated horizontal angle, the sight-blocking points are extracted by using the shortest distance between the point line or the point to the vertex, as follows:

[0133] The calculated horizontal angle is compared with the set angle threshold. If the calculated horizontal angle is greater than the set angle threshold, the cluster point cloud clusters within the bounding box area are traversed to find the 10 points closest to the selected line, and the mean coordinates of these points are calculated to determine the position of the line of sight blocking point, such as Figure 5 As shown. Among them, the equation of the line formed by the two vertices extracted by the point cloud bounding box is Ax+By+C=0, and the distance calculation formula is:

[0134]

[0135] Among them, (x1, y1) is the clustered point cloud cluster within the bounding box area, so as to obtain the coordinates of the line of sight occlusion point P target_edge (x,y).

[0136] If the horizontal angle is less than the set threshold, the 10 points closest to the left vertex (right area) or the right vertex (left area) in the point cloud are directly traversed, and the mean coordinates of these points are also calculated to determine the position P of the line of sight blocking point. target_edge (x,y).

[0137] Hide point P target_hid Connect (x, y) to the line of sight occlusion point to obtain the occlusion angle information of the current hidden area. The specific expression is as follows:

[0138]

[0139] In summary, the robot can achieve the purpose of hiding by identifying hidden areas and angle information and reasonably selecting hidden areas according to angle information during patrolling.

[0140] Verification Example

[0141] To verify the feasibility and rationality of the algorithm in this embodiment, this embodiment uses a Hunter SE robot as a test platform and is equipped with a RoboSense brand Helios 16 laser radar to conduct tests in the campus overhead garage and outdoor parking lot. Figure 6 and Figure 7 shown.

[0142] The perceived hidden point information is downsampled to reduce the number of target points in the same hidden area and the angle information is recognized under the global map. The algorithm used in this embodiment can effectively identify the hidden area of ​​the object directly in front of the robot (the red point is the center of mass, the green point is the centroid, and the blue point is the hidden point). In the overhead garage, the accuracy of hidden point recognition reached 89.6%, and the effective occlusion rate reached 86.2%; in the outdoor parking lot, the accuracy of hidden point recognition reached 82.47%, and the effective occlusion rate reached 76.5%.

[0143] Therefore, the present invention adopts the above-mentioned autonomous recognition algorithm of the hidden area of ​​the patrol robot based on 3D lidar, which can utilize 3D lidar data to reasonably select the hidden area according to the angle information during the patrol process, achieve the hiding effect, and thus better complete the patrol task.

[0144] 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. The autonomous recognition algorithm of hidden areas of patrol robots based on 3D laser radar is characterized by: The following steps are involved: S1. Preprocess the 3D point cloud scanned by the LiDAR. Use the ground point cloud filtering algorithm to segment the preprocessed 3D point cloud. Then, perform threshold judgment based on the relationship between the height difference of adjacent point clouds, local height values, and the LiDAR installation height to obtain non-ground point clouds. S2. Divide the acquired non-ground point cloud into concentric circle areas according to the distance, and perform Euclidean clustering on the point cloud in each concentric circle area to obtain clustered point cloud clusters; S3. Based on the obtained clustered point cloud clusters, construct an axis-aligned bounding box, calculate the centroid coordinates and mean center coordinates of the clustered point cloud clusters, and determine the concavity and convexity of the hidden area, and then select the target point in the hidden area according to the robot size; S31. In the laser radar coordinate system, traverse all the point cloud coordinates in the cluster point cloud cluster and find out the points in The maximum and minimum values ​​in the three coordinate axis directions; S32. Based on the obtained maximum and minimum values ​​in each coordinate axis direction, axis-aligned borders are sequentially connected and drawn, and the centroid coordinates and mean center coordinates of the clustered point cloud cluster are calculated. The specific expression is as follows: Where, is the centroid coordinate of the clustered point cloud cluster, is the mean center coordinate of the clustered point cloud cluster, For the The coordinates of the points, is the number of point clouds in the clustered point cloud cluster, 、 、 They are The maximum values ​​in the three coordinate axis directions, 、 、 They are Minimum values ​​in the three coordinate axis directions; S33. Based on the obtained centroid coordinates and mean center coordinates, the distances from the origin of the laser radar coordinate system are calculated, and the Euclidean distance value is used to determine the concavity and convexity of the area, as follows: S34, based on the identified concave and convex areas, selecting a target point according to the size of the robot; S4. Based on the obtained clustered point cloud clusters, an oriented bounding box is constructed to obtain the three-dimensional axis direction of the oriented bounding box. The position of the line of sight occlusion point is obtained by the line of sight occlusion method. Combined with the position of the target point, the occlusion point angle information of the hidden area is obtained; S41. Perform directional bounding box extraction on the obtained clustered point cloud cluster, obtain the direction of the three-dimensional axis of the clustered point cloud cluster, and calculate the rotation amount of the main axis direction relative to the radar coordinate system to obtain the vertex coordinates in the centralized point cloud coordinate system and obtain the rotation matrix; S42. According to the obtained rotation matrix, the vertex coordinates in the centralized point cloud coordinate system are transformed into the radar coordinate system. The transformation formula is as follows: Where, is the transformed radar coordinate system The coordinates of the vertices, is the rotation matrix, is the first point in the centralized point cloud coordinate system The coordinates of the vertices, is the translation matrix; S43. Divide the point cloud area into two left and right areas using the positive direction of the laser radar as the dividing line. Extract the lower and right vertices of the bounding box of the clustered point cloud cluster in the left area, and the left and lower vertices of the bounding box of the clustered point cloud cluster in the right area, based on the coordinates of the mean center point of the clustered point cloud cluster in the radar coordinate system. Connect the two vertices of the bounding box of the clustered point cloud cluster in each area to form a radial line segment. Calculate the horizontal angle between the radial line segment and the robot based on the vertex coordinates and the horizontal axis of the laser radar. S44. According to the concave-convex properties of the clustered point cloud clusters, the positions of the clustered point cloud clusters, and the calculated horizontal angle, the line of sight blocking points are extracted through the shortest distance from the point line or the point to the vertex, thereby obtaining the line of sight blocking angle information.

2. The autonomous recognition algorithm for hidden areas of patrol robots based on 3D laser radar according to claim 1 is characterized in that: Step S1 includes: S11. Obtain the installation height of the laser radar relative to the ground, and segment the point cloud based on the laser radar coordinate system and the robot height as the threshold, and retain the point cloud data below the threshold; S12. Differentiate the processed point cloud data horizontally according to the horizontal angular resolution of the laser radar, divide the point cloud data into sectors perpendicular to the ground in a clockwise direction, and sort the point clouds in each sector in ascending order according to the Euclidean distance of the point cloud from the origin of the radar coordinate system. S13, performing local straight line fitting on adjacent point clouds in each sector area, and calculating the local height and global height of the current point cloud according to the local slope height threshold and the global slope height threshold; S14, judging the ground point cloud and the non-ground point cloud by using a threshold value based on the relationship between the height difference of adjacent point clouds, the local height, and the installation height of the lidar; S15. Repeat operations S12 to S14 until all ground point cloud and non-ground point cloud data are extracted.

3. The autonomous recognition algorithm for hidden areas of patrol robots based on 3D laser radar according to claim 2 is characterized in that: In step S13, the local height and global height of the current point cloud are calculated as follows: Where, 、 are the local height and global height of the current point cloud respectively, is the Euclidean distance between the current point cloud and the origin of the radar coordinate system, is the Euclidean distance between the previous adjacent point cloud and the origin of the radar coordinate system, 、 They are the local slope height threshold and the global slope height threshold respectively.

4. The autonomous recognition algorithm for hidden areas of patrol robots based on 3D laser radar according to claim 2 is characterized in that: In step S14, the ground point cloud and non-ground point cloud are judged by the threshold value, which is specifically expressed as follows: Where, 、 Separate 、 The height of the point cloud, is the threshold, is the installation height of the lidar relative to the ground.

5. The autonomous recognition algorithm for hidden areas of patrol robots based on 3D laser radar according to claim 1 is characterized in that: In step S2, the non-ground point cloud is divided into three concentric areas using a straight-through filter with a distance threshold of 15m and 30m. In each concentric area, a different clustering distance threshold is set to find the neighborhood of the point, thereby obtaining clustered point cloud clusters, as follows: Where, is the first point cloud cluster Point coordinates, is the clustering point cloud cluster, For The point coordinates whose distance is within the cluster distance threshold range, is the clustering distance threshold.

6. The autonomous recognition algorithm for hidden areas of patrol robots based on 3D laser radar according to claim 1 is characterized in that: Step S34 includes: The target point of the concave area is selected as: The target points of the convex area are selected as: Where, is the radius of the robot’s minimum circumcircle, is the plane coordinate of the target point.

7. The autonomous recognition algorithm for hidden areas of patrol robots based on 3D laser radar according to claim 1 is characterized in that: Step S41 includes: First, the clustered point cloud cluster is centralized. For any clustered point cloud cluster, its mean center coordinates are calculated as the geometric center, and the clustered point cloud cluster is transformed into a coordinate system with the center coordinates as the origin to obtain the centralized point cloud cluster. Secondly, calculate the degree of discreteness of the obtained centralized point cloud cluster, that is, calculate the covariance matrix of the point cloud in three-dimensional space , as follows: Where, 、 They are the first points in the clustered point cloud cluster under the centralized point cloud coordinate system and the radar coordinate system respectively. The coordinates of the points; Reusing the obtained covariance matrix Perform eigenvalue decomposition and obtain three eigenvalues 、 、 and its corresponding eigenvector 、 、 , to get the direction vectors of the three main axes of the oriented bounding box; Then, in the obtained centralized point cloud cluster, according to the direction vectors of the three main axes, traverse the point cloud cluster to obtain the maximum distance in the three main axis directions, thereby obtaining the four vertex coordinates of the oriented bounding box in the centralized point cloud coordinate system; Based on the main axis direction vector of the centralized point cloud, its rotation matrix relative to the radar coordinate system is calculated, and a 3×3 moment of inertia tensor matrix is ​​constructed in the centralized point cloud coordinate system. , as follows: The calculation formula of the inertia moment tensor matrix elements is as follows: Where, is the first point in the centralized point cloud coordinate system The coordinates of the points; Perform eigenvalue decomposition on the obtained moment of inertia tensor matrix and obtain the eigenvector 、 、 , then the rotation matrix is .

8. The autonomous recognition algorithm for hidden areas of patrol robots based on 3D laser radar according to claim 1 is characterized in that: Step S44 includes connecting the target point and the sight blocking point to obtain the blocking angle information of the current hidden area, which is specifically expressed as follows: Where, 、 are the plane coordinates of the occlusion point and the target point respectively.

Images