Patrol robot hidden area autonomous cognition algorithm based on 3D laser radar

Abstract

The invention discloses an autonomous cognitive algorithm of a patrol robot hidden area based on a 3D laser radar, and relates to the technical field of artificial intelligence. The invention provides an autonomous cognitive algorithm for a patrol robot hidden area based on a 3D laser radar. The autonomous cognitive algorithm comprises the following steps: acquiring a non-ground point cloud by adopting a ground point cloud filtering algorithm; for the obtained non-ground point clouds, dividing concentric circle regions according to Euclidean distances, and performing Euclidean clustering on the point clouds in the concentric circle regions to obtain clustered point cloud clusters; according to the point cloud cluster, calculating an axis alignment bounding box, and obtaining a hidden point coordinate; and according to the point cloud cluster, calculating a directional bounding box, and calculating coordinates of a shielding point in a sight line shielding mode, thereby automatically calculating and obtaining a shielding angle. Therefore, by the adoption of the method, the hidden area is reasonably selected according to the angle information in the patrol process through the 3D laser radar data, the hiding effect is achieved, the patrol task is better completed, and high robustness and practicability are achieved.

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] With the development of the digital age and information technology, human lifestyles are constantly changing. In particular, the rapid development of artificial intelligence has provided new solutions for patrol work. The patrol robot is an intelligent robot with autonomous patrol and remote monitoring functions. Different scenarios require different functions of the robot, such as face recognition, behavior recognition, license plate recognition, and stranger detection, which can effectively provide security protection for the park. Compared with traditional video surveillance, patrol robots not only have monitoring functions, but also have higher flexibility and mobility, a wide reconnaissance range, and small blind spots, which can effectively replace human patrols.

[0003] A patrol robot is an intelligent robot that integrates environmental perception, modeling and decision-making functions. It can replace most manual security patrols and improve patrol efficiency. Unlike industrial or transport robots, patrol robots need to have certain anti-reconnaissance capabilities and can respond quickly to protect their own safety in dangerous situations. However, current patrol robots generally lack self-protection awareness and protection mechanisms, and are very likely to attract the attention of criminals during the execution of their missions, or even be destroyed, resulting in patrol mission failures and property losses. Therefore, it is crucial for patrol robots to perceive hidden areas during patrols.

[0004] In the early days, biologists Kiltie and Laine pointed out that camouflage can be combined with computer vision. Researchers began to develop various camouflage technologies based on computer vision technology, using the surrounding environment to camouflage themselves into the environment. Equipment camouflage mainly uses deformed camouflage, which segments the target's outline through patches of various colors and irregular shapes on the target surface, forming a good fusion with the natural background, thereby reducing the target's salience. However, in most scenarios, the environment in which the disguiser is located is not suitable for camouflage equipment, so the generalization ability of this method is low. The electronic skin developed by some scholars can be soft and compactly carried by all-soft robots, but it is limited by the high cost of manufacturing materials and insufficient wearability, making the electronic skin still in the basic development stage. Zhang Botao and others used fuzzy mapping functions to evaluate the similarity of color and texture and select the optimal protection area of ​​the robot, but this method is affected by lighting and viewing angle and needs to be further optimized. In addition, the "Chameleon" designed by scholars from Seoul University is equipped with an electronic skin inherited from a thermochromic liquid crystal layer and silver nanomaterials. It has the camouflage ability to blend into the background in real time, which is a major breakthrough in the field of artificial camouflage technology.

[0005] However, existing technologies all start from changing the color and state of the camouflage itself to blend into the background environment, without considering the role of the robot's active behavior and the recognition of the hidden environment in hiding. For example, when hunting prey on the grassland, male lions often use the cover of grass and trees to slowly approach the target and finally hunt the prey. Reasonable selection of hidden environments and light postures can effectively conduct reconnaissance and hunting. On the battlefield, scouts often wear camouflage to blend in with the environment for camouflage and use obstructions to hide, which can achieve a better effect of avoiding opponents. That is, in different situations, patrol robots may act as pursuers or evaders, hiding themselves in the pursuit process and preventing themselves from being destroyed by criminals during the tracking process.

[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 of the hidden areas, providing a reference basis for selecting reasonable hidden areas during the robot patrol process, reducing the risk of being discovered by criminals, and thus better completing the patrol mission. Summary of the invention

[0007] The purpose of the present invention is to provide an autonomous recognition algorithm for hidden areas of a patrol robot based on 3D laser radar, which can use 3D laser radar 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 object, the present invention provides an autonomous recognition algorithm for hidden areas of a patrol robot based on a 3D laser radar, comprising the following steps:

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

[0010] S2, dividing the acquired non-ground point cloud into concentric circle areas according to the distance, and performing 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 cluster, construct an axis-aligned bounding box, calculate the centroid coordinates and mean center coordinates of the clustered point cloud cluster, and determine the concavity and convexity of the hidden area, and then select the target point of the hidden area according to the size of the robot;

[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, and 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 comprises:

[0014] S11, obtaining the installation height of the laser radar relative to the ground, and using the laser radar coordinate system as a reference and the robot height as a threshold to segment the point cloud, and retaining the point cloud data below the threshold;

[0015] S12. Differentiate the processed point cloud data according to the angle in the horizontal direction according to the horizontal angular resolution of the laser radar, divide it into sector areas perpendicular to the ground in the clockwise direction, and arrange the point clouds in each sector area 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 according to the relationship between the height difference of adjacent point clouds, the local height, and the installation height of the laser radar;

[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 specifically 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, respectively, 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, which is specifically expressed as follows:

[0023]

[0024] In the formula, z i 、z i-1The heights 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, in step S2, the non-ground point cloud is divided into three concentric circle areas by using a pass-through filter with a distance threshold of 15m and 30m; and in each concentric circle area, a different clustering distance threshold is set to find the neighborhood of the point, so as to obtain a clustered point cloud cluster, 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 point coordinates whose distance is within the clustering distance threshold, ε is the clustering distance threshold.

[0028] Preferably, step S3 comprises:

[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 of X, Y, and Z respectively;

[0030] S32, according to the obtained maximum and minimum values ​​in the directions of each coordinate axis, the axis-aligned borders are connected and drawn in sequence, and the centroid coordinates and mean center coordinates of the clustered point cloud cluster are calculated, which are specifically expressed as follows:

[0031]

[0032] Where P sc (x, y, z) is the centroid coordinate of the cluster point cloud cluster, 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. 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, according to the obtained centroid coordinates and mean center coordinates, respectively calculate the distance from the origin of the laser radar coordinate system, and use the Euclidean distance value to judge the concavity and convexity of the area, as follows:

[0034]

[0035] S34. Based on the identified concave-convex area, the target point is selected 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] In the formula, x r is the radius of the robot's minimum circumscribed circle, (x target_hid ,y target_hid ) is the plane coordinate of the target point.

[0044] Preferably, step S4 comprises:

[0045] S41, extracting a directional bounding box from the obtained clustered point cloud cluster, obtaining the direction of the three-dimensional axis of the clustered point cloud cluster, and calculating the rotation amount of the main axis direction relative to the radar coordinate system, obtaining the vertex coordinates in the centralized point cloud coordinate system, and obtaining 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] In the formula, 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 centered point cloud coordinate system, and M is the translation matrix;

[0049] S43, taking the positive direction of the laser radar as the dividing line, dividing the point cloud area into two areas on the left and right, and extracting the lower vertex and the right vertex of the border of the clustered point cloud cluster in the left area, and the left vertex and the lower vertex of the border of the clustered point cloud cluster in the right area respectively according to the coordinates of the mean center point of the clustered point cloud cluster in the radar coordinate system, connecting the two vertices of the border of the clustered point cloud cluster in each area into a radial line segment, and calculating the horizontal angle between the radial line segment and the robot according to 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, so as to obtain the line of sight blocking angle information.

[0051] Preferably, step S41 includes:

[0052] First, the clustered point cloud clusters are 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 discrete degree 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] In the formula, 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, the obtained covariance matrix H is used for 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, the point cloud cluster is traversed to obtain the maximum distance in the three main axis directions, thereby obtaining the coordinates of the four vertices of the oriented bounding box in the centralized point cloud coordinate system;

[0059] 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 I is constructed in the centralized point cloud coordinate system, as follows:

[0060]

[0061] Among them, the calculation formula of the inertia moment tensor matrix element 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, 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 to reduce the computational complexity of data processing while retaining the spatial distribution information of the point cloud. The point cloud data is filtered 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 1It is a flowchart of the overall implementation in an embodiment of the present invention;

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

[0075] Figure 3 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 in an embodiment of the present invention;

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

[0078] Figure 6 1 is the identification of hidden points and blocked points in the overhead garage in the embodiment of the present invention, wherein (a) is the identification result of scene 1, (b) is the identification result of scene 2, (c) is the identification result of scene 3, and (d) is the identification result of scene 4;

[0079] Figure 7 1 is the identification of hidden points and blocked points in an outdoor parking lot in an embodiment of the present invention, wherein (a) is the identification result of scene one, (b) is the identification result of scene two, (c) is the identification result of scene three, and (d) is the identification 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 the hidden area of ​​the patrol robot 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, 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 as the reference. robot is the threshold, segmenting the point cloud and retaining the point cloud data below the threshold.

[0084] S12, such as Figure 2As shown in the figure, the point cloud data is differentiated according to the horizontal angular resolution α of the laser radar in the horizontal direction, 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, according to the Euclidean distance d from the origin i Arranged 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 the 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 height difference between adjacent point clouds, local height, and the relationship between the laser radar installation height, 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 non-ground point cloud into three concentric circle areas by distance using the thresholds of 15m and 30m through the pass filter. Based on the divided concentric circle point cloud areas, perform Euclidean clustering respectively. 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 point cloud coordinates in the cluster point cloud cluster, and find the maximum and minimum values ​​(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 borders 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, according to the obtained centroid coordinates and mean center coordinates, respectively calculate the Euclidean distance from the origin of the laser radar coordinate system, and use the Euclidean distance value to judge 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 selected in the concave area and the convex area are the hidden points.

[0112] S4. Calculate the oriented bounding box of the obtained clustered point cloud cluster, and calculate the coordinates of the occlusion point by 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] In the formula, 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] Then, the obtained covariance matrix H is used for eigenvalue decomposition to obtain the direction of the three-dimensional axis of the oriented bounding box, and three eigenvalues ​​λ1, λ2, λ3 and corresponding eigenvectors v1, v2, v3 are obtained. Among them, 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, the eight vertex coordinates L of the bounding box in the centralized point cloud coordinate system are i They are (-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 main 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] Among them, the calculation formula of the inertia moment tensor matrix element 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 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] In the formula, G i is the vertex coordinate in the transformed radar coordinate system.

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

[0132] S44, according to the concavity and convexity of the clustered point cloud cluster, the position of the clustered point cloud cluster 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, 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 in 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 straight 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 cluster 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 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) with 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] In order 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 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 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. An autonomous recognition algorithm for hidden areas of patrol robots based on 3D laser radar, characterized in that: The following steps are involved: S1. Preprocess the 3D point cloud scanned by the laser radar, use the ground point cloud filtering algorithm to segment the preprocessed 3D point cloud, and perform threshold judgment based on the relationship between the height difference of adjacent point clouds, the local height value, and the installation height of the laser radar to obtain the non-ground point cloud; S2, dividing the acquired non-ground point cloud into concentric circle areas according to the distance, and performing 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 cluster, construct an axis-aligned bounding box, calculate the centroid coordinates and mean center coordinates of the clustered point cloud cluster, and determine the concavity and convexity of the hidden area, and then select the target point of the hidden area 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, and 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.

2. The autonomous recognition algorithm of hidden areas of patrol robots based on 3D laser radar according to claim 1 is characterized in that: Step S1 includes: S11, obtaining the installation height of the laser radar relative to the ground, and using the laser radar coordinate system as a reference and the robot height as a threshold to segment the point cloud, and retaining the point cloud data below the threshold; S12. Differentiate the processed point cloud data according to the angle in the horizontal direction according to the horizontal angular resolution of the laser radar, divide it into sector areas perpendicular to the ground in the clockwise direction, and arrange the point clouds in each sector area 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 according to the relationship between the height difference of adjacent point clouds, the local height, and the installation height of the laser radar; S15. Repeat operations S12 to S14 until all ground point cloud and non-ground point cloud data are extracted.

3. The autonomous recognition algorithm of 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: M i =(d i -d i-1 )*tan(SL) <h2 style=";text-align:left;direction:ltr">N<h2 style=";text-align:left;direction:ltr"> i <h2 style=";text-align:left;direction:ltr"> =d<h2 style=";text-align:left;direction:ltr"> i <h2 style=";text-align:left;direction:ltr"> *tan(SG) Where M i 、N i are the local height and global height of the current point cloud, respectively, 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.

4. The autonomous recognition algorithm of 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 the non-ground point cloud are judged by the threshold, which is specifically expressed as follows: In the formula, z i 、z i-1 The 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.

5. The autonomous recognition algorithm of 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 circle areas by using a pass-through filter with a distance threshold of 15m and 30m; and in each concentric circle area, different clustering distance thresholds are set to find the neighborhood of the point, so as to obtain a clustered point cloud cluster, as follows: Q={P j |d(P i ,P j )<ε} 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 point coordinates whose distance is within the clustering distance threshold, ε is the clustering distance threshold.

6. The autonomous recognition algorithm of hidden areas of patrol robots based on 3D laser radar according to claim 3 is characterized in that: Step S3 includes: 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 of X, Y, and Z respectively; S32, according to the obtained maximum and minimum values ​​in the directions of each coordinate axis, the axis-aligned borders are connected in sequence and the centroid coordinates and mean center coordinates of the clustered point cloud cluster are calculated, which are specifically expressed as follows: Where P sc (x, y, z) is the centroid coordinate of the cluster point cloud cluster, 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. min ,y min 、z min They are the minimum values ​​in the directions of the three coordinate axes X, Y, and Z respectively; S33, according to the obtained centroid coordinates and mean center coordinates, respectively calculate the distance from the origin of the laser radar coordinate system, and use the Euclidean distance value to judge the concavity and convexity of the area, as follows: S34. Based on the identified concave-convex area, the target point is selected according to the size of the robot.

7. The autonomous recognition algorithm of hidden areas of patrol robots based on 3D laser radar according to claim 6 is characterized in that: Step S34 includes: The target point of the concave area is selected as: x target_hid =x min -x r and target_hid (and max +y min ) / 2 The target points of the convex area are selected as: x target_hid =p qc (x)-x r and target_hid (and max +y min ) / 2 In the formula, x r is the radius of the robot's minimum circumscribed circle, (x target_hid ,y target_hid ) is the plane coordinate of the target point.

8. The autonomous recognition algorithm of hidden areas of patrol robots based on 3D laser radar according to claim 1 is characterized in that: Step S4 includes: S41, extracting a directional bounding box from the obtained clustered point cloud cluster, obtaining the direction of the three-dimensional axis of the clustered point cloud cluster, and calculating the rotation amount of the main axis direction relative to the radar coordinate system, obtaining the vertex coordinates in the centralized point cloud coordinate system, and obtaining 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: G i =R×L i +P qc In the formula, 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 centered point cloud coordinate system, and M is the translation matrix; S43, taking the positive direction of the laser radar as the dividing line, dividing the point cloud area into two areas on the left and right, and extracting the lower vertex and the right vertex of the border of the clustered point cloud cluster in the left area, and the left vertex and the lower vertex of the border of the clustered point cloud cluster in the right area respectively according to the coordinates of the mean center point of the clustered point cloud cluster in the radar coordinate system, connecting the two vertices of the border of the clustered point cloud cluster in each area into a radial line segment, and calculating the horizontal angle between the radial line segment and the robot according to 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, so as to obtain the line of sight blocking angle information.

9. The autonomous recognition algorithm of hidden areas of patrol robots based on 3D laser radar according to claim 8 is characterized in that: Step S41 includes: First, the clustered point cloud clusters are 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, the discrete degree 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: Q(P i )'=Q(P i )-P qc In the formula, 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; Then, the obtained covariance matrix H is used for 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; Then, in the obtained centralized point cloud cluster, according to the direction vectors of the three main axes, the point cloud cluster is traversed to obtain the maximum distance in the three main axis directions, thereby obtaining the coordinates of the four vertices 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 I is constructed in the centralized point cloud coordinate system, as follows: Among them, the calculation formula of the inertia moment tensor matrix element is as follows: In the formula, (x' i ,y' i ,z' i ) is the coordinate of the i-th point in the centralized point cloud coordinate system; Perform eigenvalue decomposition on the obtained moment of inertia tensor matrix to obtain eigenvectors v4, v5, v6, and the rotation matrix is ​​R = [v4, v5, v6].

10. The autonomous recognition algorithm of hidden areas of patrol robots based on 3D laser radar according to claim 8 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 P target_edge (x,y),P target_hid (x, y) are the plane coordinates of the occlusion point and the target point respectively.

Images