Amphibious unmanned vehicle cross-domain scene loadable shoreline identification method based on laser radar

Abstract

The invention relates to an amphibious unmanned vehicle cross-domain scene loadable shoreline identification method based on a laser radar, belongs to the technical field of amphibious unmanned vehicles, and solves the problem of lack of a shoreline identification method based on a vehicle end laser radar in the prior art. The method comprises the following steps: acquiring laser radar point cloud data of the amphibious unmanned vehicle at the current moment; judging the current working condition of the amphibious unmanned vehicle based on the point cloud data; filtering the point cloud data if the current working condition is to be underwater or sail on water; recognizing boundary points in the filtered point cloud based on the normal vector of the point cloud to obtain a boundary point cloud; extracting a shoreline plane point cloud in the boundary point cloud based on point cloud plane segmentation; and filtering non-shoreline points in the shoreline plane point cloud based on point cloud clustering to obtain a shoreline point cloud. And rapid and accurate shoreline identification is realized.

Description

Technical Field

[0001] The present invention relates to the technical field of amphibious unmanned vehicles, and in particular to a laser radar-based method for identifying a landing shoreline in a cross-domain scenario for an amphibious unmanned vehicle. Background Art

[0002] With the rapid development of modern unmanned driving technology, unmanned driving is no longer limited to traditional land transportation, but has expanded its application scope to amphibious unmanned vehicles. Amphibious unmanned vehicles are highly versatile intelligent vehicles that can travel on land and water at the same time. Compared with traditional vehicles, unmanned amphibious vehicles have added cross-domain landing requirements, that is, the change of working conditions from "land to water" and "from water to land". How to accurately identify the coastline and accurately judge the time of switching between water and land working conditions and the landing area is of great significance for amphibious unmanned vehicles to achieve smooth cross-domain transition.

[0003] LiDAR can generate high-resolution three-dimensional point cloud data by emitting laser beams and measuring the time it takes for them to return. These point cloud data provide detailed information about the terrain, obstacles and their heights, allowing unmanned vehicles to accurately perceive the environment. At present, LiDAR is widely used in environment construction and target recognition or object recognition in road scenes, but there is a lack of related research in the field of amphibious cross-domain. Therefore, how to realize shoreline recognition through vehicle-side LiDAR is one of the important issues that need to be solved in the current cross-domain field of unmanned amphibious vehicles. Summary of the invention

[0004] In view of the above analysis, an embodiment of the present invention aims to provide a method for identifying landable shorelines in cross-domain scenarios for amphibious unmanned vehicles based on laser radar, so as to solve the problem of the existing lack of shoreline identification methods based on vehicle-side laser radar.

[0005] On the one hand, an embodiment of the present invention provides a method for identifying a landing shoreline in a cross-domain scenario for an amphibious unmanned vehicle based on a laser radar, comprising the following steps:

[0006] Acquire the laser radar point cloud data of the amphibious unmanned vehicle at the current moment; and determine the current working condition of the amphibious unmanned vehicle based on the point cloud data;

[0007] If the current working condition is to be launched into water or to sail on water, the point cloud data is filtered; the boundary points in the filtered point cloud are identified based on the normal vector of the point cloud to obtain a boundary point cloud;

[0008] Extract shoreline plane point cloud from boundary point cloud based on point cloud plane segmentation;

[0009] Based on point cloud clustering, non-shoreline points in the shoreline plane point cloud are filtered to obtain the shoreline point cloud.

[0010] Based on the further improvement of the above method, after obtaining the laser radar point cloud data of the amphibious unmanned vehicle at the current moment, before judging the current working condition of the amphibious unmanned vehicle based on the point cloud data, it also includes correcting each point in the obtained laser radar point cloud data in the following manner:

[0011] p' i =P i ·R

[0012] R=R roll ·R pitch

[0013]

[0014] Among them, P i represents the original coordinates of the i-th point, p' i represents the corrected coordinates of the i-th point, R represents the comprehensive rotation matrix, θ represents the pitch angle of the amphibious unmanned vehicle, and R pitch represents the pitch angle correction matrix, φ represents the roll angle of the amphibious unmanned vehicle, R roll Represents the roll angle correction matrix.

[0015] Based on the further improvement of the above method, the current working condition of the amphibious unmanned vehicle is determined based on the point cloud data in the following manner:

[0016] Calculate the number of points within the first range around the current amphibious unmanned vehicle;

[0017] If the number of points is less than the first threshold, the amphibious unmanned vehicle is currently in a water navigation condition;

[0018] If the number of points exceeds the second threshold, and the ratio of the number of points in front of the amphibious unmanned vehicle to the number of points in the rear of the amphibious unmanned vehicle is less than the third threshold, the amphibious unmanned vehicle is currently in the waiting to launch condition; otherwise, the amphibious unmanned vehicle is currently in other conditions.

[0019] Based on the further improvement of the above method, the point cloud data is filtered in the following manner:

[0020] Performing initial filtering on the point cloud data by using straight-through filtering and conditional filtering;

[0021] For each point after the initial filtering, the distance from each neighboring point to the point is calculated, and based on the distance from each neighboring point to the point, it is determined whether each neighboring point of the point is a discrete point. If so, the discrete point is filtered out.

[0022] Based on the further improvement of the above method, the boundary points in the filtered point cloud are identified based on the normal vector of the point cloud to obtain the boundary point cloud, including:

[0023] Calculate the normal vector of each point in the filtered point cloud;

[0024] Traverse each point in the filtered point cloud, calculate the angle between the point and the normal vector of each of its neighboring points, and if the ratio of the number of neighborhood points with an angle greater than the fourth threshold to the total number of neighborhood points of the point exceeds the fifth threshold, then the point is a boundary point;

[0025] All boundary points constitute the boundary point cloud.

[0026] Based on the further improvement of the above method, the shoreline plane point cloud in the boundary point cloud is extracted based on the point cloud plane segmentation, including:

[0027] S31, randomly select three points in the boundary point cloud to obtain a fitting plane;

[0028] S32, determining whether the fitting plane is a valid plane, if not, returning to step S31, otherwise executing step S33;

[0029] S33, calculating the distance from each point in the boundary point cloud to the fitting plane, judging whether each point is an in-plane point based on the distance, and obtaining the number of in-plane points corresponding to the fitting plane; if the current iteration is the first time, the fitting plane is taken as the optimal plane, otherwise, if the number of in-plane points corresponding to the fitting plane is greater than the number of in-plane points corresponding to the optimal plane, the fitting plane is taken as the optimal plane; executing step S34;

[0030] S34. If the maximum number of iterations has not been reached, return to step S31; otherwise, stop the iteration, and the points within the plane corresponding to the optimal plane constitute the shoreline plane point cloud.

[0031] Based on the further improvement of the above method, the non-shoreline points in the shoreline plane point cloud are filtered based on point cloud clustering to obtain the shoreline point cloud, including:

[0032] Clustering the shoreline plane point cloud based on the distance between points in the shoreline plane point cloud;

[0033] For each type of point cloud, determine whether the point cloud is a linear structure. If not, filter out the point cloud. Otherwise, keep the point cloud to obtain the initial shoreline point cloud.

[0034] The initial shoreline point cloud is filtered based on the current working condition of the amphibious unmanned vehicle to obtain the final shoreline point cloud.

[0035] Based on the further improvement of the above method, the shoreline plane point cloud is clustered based on the distance between points in the shoreline plane point cloud, including:

[0036] S411, setting the current category serial number to zero;

[0037] S412, randomly selecting an unclassified point in the shoreline plane point cloud, setting the category number of the unclassified point as the current category number; and adding the unclassified point to the to-be-searched set;

[0038] S413, taking the first to-be-searched point in the to-be-searched set as the current to-be-searched point, calculating the distance between each unclassified neighboring point of the current to-be-searched point and the current to-be-searched point; setting the category number of the unclassified neighboring point whose distance is less than the eighth threshold as the current category number, and adding it to the to-be-searched set; deleting the current to-be-searched point from the to-be-searched set;

[0039] S413. If the set to be searched is not empty, return to step S413; otherwise, increase the current category number by one; if there are unclassified points in the shoreline plane point cloud, return to step S412; otherwise, end clustering.

[0040] Based on the further improvement of the above method, the following formula is used to determine whether this type of point cloud is a linear structure:

[0041] For each point in the class, calculate the distance from the point to its neighboring points; if the number of neighboring points whose distance is less than the sixth threshold is 1, the point is an endpoint;

[0042] If the class has two endpoints, the horizontal distance between the two endpoints is calculated. If the horizontal distance is greater than the seventh threshold, the class is a linear structure, otherwise it is not a linear structure.

[0043] Based on the further improvement of the above method, the initial shoreline point cloud is filtered based on the current working condition of the amphibious unmanned vehicle to obtain the final shoreline point cloud, including:

[0044] If the current working condition of the amphibious unmanned vehicle is the water navigation condition, then find the point with the smallest x coordinate value in the initial shoreline point cloud (x xmin ,y xmin ,z xmin ), filter out the initial shoreline point cloud with x value coordinates greater than (x xmin +5); in the remaining point clouds, search for point cloud classes with a span greater than the seventh threshold to form a shoreline point cloud;

[0045] If the current working condition of the amphibious unmanned vehicle is to be launched into the water, then find the point with the largest x coordinate value in the initial shoreline point cloud (x xmax ,y xmax ,z xmax ), filter out the initial shoreline point cloud with x value coordinates less than (x xmax -5); in the remaining point clouds, search for point cloud classes with a span greater than the seventh threshold to form the shoreline point cloud.

[0046] Compared with the prior art, the present invention facilitates accurate identification of the shoreline by first judging the current working condition; if it is in the working condition of waiting to be launched or sailing on the water, the point cloud data is filtered to remove irrelevant or interfering points, so as to facilitate accurate identification of the shoreline; the scope of the shoreline point cloud is narrowed by identifying the boundary point cloud, and the shoreline plane point cloud is further extracted from the boundary point cloud to obtain the points on the plane where the shoreline is located, but it contains non-shoreline points, so the non-shoreline points in the shoreline plane point cloud are filtered out through clustering, so as to quickly and accurately obtain the final shoreline point cloud.

[0047] In the present invention, the above-mentioned technical solutions can also be combined with each other to achieve more preferred combination solutions. Other features and advantages of the present invention will be described in the subsequent description, and some advantages can become obvious from the description, or can be understood by practicing the present invention. The purpose and other advantages of the present invention can be realized and obtained through the contents particularly pointed out in the description and the drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] The accompanying drawings are only used for the purpose of illustrating specific embodiments and are not to be considered as limiting the present invention. In the entire drawings, the same reference symbols represent the same components;

[0049] Figure 1 This is a flow chart of a method for identifying a landable shoreline in a cross-domain scenario for an amphibious unmanned vehicle based on a laser radar according to an embodiment of the present invention;

[0050] Figure 2 This is an opening and closing logic diagram of the shoreline point cloud recognition algorithm of an embodiment of the present invention. DETAILED DESCRIPTION

[0051] The preferred embodiments of the present invention are described in detail below in conjunction with the accompanying drawings, wherein the accompanying drawings constitute a part of this application and are used together with the embodiments of the present invention to illustrate the principles of the present invention, but are not used to limit the scope of the present invention.

[0052] A specific embodiment of the present invention discloses a method for identifying a landing shoreline for an amphibious unmanned vehicle in a cross-domain scenario based on a laser radar. Figure 1 As shown, the following steps are included:

[0053] S1. Obtaining laser radar point cloud data of the amphibious unmanned vehicle at the current moment; and determining the current working condition of the amphibious unmanned vehicle based on the point cloud data;

[0054] S2. If the current working condition is waiting to be launched into the water or sailing on the water, the point cloud data is filtered; and the boundary points in the filtered point cloud are identified based on the normal vector of the point cloud to obtain a boundary point cloud;

[0055] S3, extracting the shoreline plane point cloud in the boundary point cloud based on point cloud plane segmentation;

[0056] S4. Filter non-shoreline points in the shoreline plane point cloud based on point cloud clustering to obtain the shoreline point cloud.

[0057] During implementation, the amphibious unmanned vehicle is equipped with a distance sensor, namely a lidar, which obtains the point cloud data of each frame collected by the lidar, and judges the current working condition of the amphibious unmanned vehicle based on the point cloud data of the current frame. If it is waiting to be launched into the water or sailing on the water, shoreline point cloud recognition is performed, that is, image recognition is performed on the radar point cloud to identify possible shorelines.

[0058] Compared with the prior art, the shoreline recognition method for cross-domain landing scenarios of amphibious unmanned vehicles based on lidar provided in this embodiment first determines the current working conditions to facilitate accurate identification of the shoreline. If it is in the condition of waiting to be launched or sailing on the water, the point cloud data is filtered to remove irrelevant or interference points to facilitate accurate identification of the shoreline. The scope of the shoreline point cloud is narrowed by identifying the boundary point cloud, and the shoreline plane point cloud is further extracted from the boundary point cloud to obtain the points on the plane where the shoreline is located, but it contains non-shoreline points. Therefore, the non-shoreline points in the shoreline plane point cloud are filtered out through clustering, so as to quickly and accurately obtain the final shoreline point cloud.

[0059] When an amphibious unmanned vehicle is traveling on the water, it will be affected by waves, wind, etc., and the body will have yaw such as roll and pitch. On land, it will also have yaw due to uneven roads. This yaw will affect the data collected by the LiDAR. Therefore, after obtaining the LiDAR point cloud data of the amphibious unmanned vehicle at the current moment, before judging the current working condition of the amphibious unmanned vehicle based on the point cloud data, the point cloud data of the LiDAR is corrected.

[0060] Specifically, each point in the acquired LiDAR point cloud data is corrected in the following manner:

[0061] p' i =P i ·R

[0062] R=R roll ·R pitch

[0063]

[0064] Among them, P i represents the original coordinates of the i-th point, p' i represents the corrected coordinates of the i-th point, R represents the comprehensive rotation matrix, θ represents the pitch angle of the amphibious unmanned vehicle, and R pitch represents the pitch angle correction matrix, φ represents the roll angle of the amphibious unmanned vehicle, R roll Represents the roll angle correction matrix.

[0065] During implementation, the pitch correction matrix and the roll correction matrix are calculated based on the attitude data of the amphibious unmanned vehicle, including the longitudinal tilt angle and the roll angle, and the two correction matrices are combined to obtain a comprehensive rotation matrix. Then, each point of the acquired laser radar point cloud data is corrected based on the comprehensive rotation matrix, so as to facilitate accurate identification of the shoreline. The corrected point cloud coordinates are relative to the vehicle coordinate system, with the x-axis direction being the front of the vehicle, the y-axis direction being the left direction of the vehicle, and the z-axis being the top of the vehicle.

[0066] The driving conditions of an amphibious unmanned vehicle include: waiting to be launched, sailing on water, sailing on land, landing and launching. Shoreline identification is only required when the unmanned vehicle is sailing on water and waiting to be launched. It is not required in other conditions. Therefore, it is necessary to first determine the current working condition of the amphibious unmanned vehicle.

[0067] Specifically, the current working condition of the amphibious unmanned vehicle is determined based on the point cloud data in the following manner:

[0068] Calculate the number of points within the first range around the current amphibious unmanned vehicle;

[0069] If the number of points is less than the first threshold, the amphibious unmanned vehicle is currently in a water navigation condition;

[0070] If the number of points exceeds the second threshold, and the ratio of the number of points in front of the amphibious unmanned vehicle to the number of points in the rear of the amphibious unmanned vehicle is less than the third threshold, the amphibious unmanned vehicle is currently in the waiting to launch condition; otherwise, the amphibious unmanned vehicle is currently in other conditions.

[0071] Since there is no echo or the echo intensity is very weak when the laser radar shines on the water surface and the air, it can be judged by identifying the number of point clouds within a certain range around the amphibious unmanned vehicle.

[0072] During implementation, the horizontal projection of the vehicle is first simplified into a rectangle using the amphibious unmanned vehicle's own size data (length L, width W) to obtain the range of the vehicle body points in the point cloud, and a 2m circular range around the vehicle body is set as the consideration range to obtain the first range.

[0073] The number of points within the first range around the amphibious unmanned vehicle is counted. If it is less than the first threshold, it is considered to be in water navigation condition. During implementation, a set of radar points within the range under land conditions can be collected through experiments, and the minimum value is set as the threshold K. The first threshold can be set to 0.3K.

[0074] If the number of points within the first range around the current amphibious unmanned vehicle is greater than the second threshold, it is considered that the amphibious unmanned vehicle is on land, and it is necessary to continue to determine whether it is in the "waiting to enter the water" condition, that is, to judge the number of point clouds in front of and behind the amphibious unmanned vehicle. If the ratio of the number of points in the front of the amphibious unmanned vehicle to the number of points in the rear of the amphibious unmanned vehicle is less than the third threshold, it is considered that the amphibious unmanned vehicle is traveling towards the water area, so the current is the waiting to enter the water condition, otherwise it is other conditions. In implementation, the second threshold can be set to 0.7K. The third threshold can be set to 0.75.

[0075] If the number of points within the first range around the current amphibious unmanned vehicle is greater than or equal to the first threshold and less than or equal to the second threshold, it is also other conditions.

[0076] For the waiting to enter the water and water navigation conditions, subsequent shoreline recognition is carried out, while other conditions do not perform shoreline recognition.

[0077] In implementation, first, the radar point cloud data is filtered to remove points that are irrelevant to the task or cause interference. Specifically, the following method is used to filter the point cloud data:

[0078] S21. Perform initial filtering on the point cloud data using direct filtering and conditional filtering;

[0079] S22. For each point after initial filtering, calculate the distance from each neighboring point of this point to this point, and judge whether each neighboring point of this point is a discrete point based on the distance from each neighboring point to this point. If so, filter out this discrete point.

[0080] In implementation, initial filtering is first performed, that is, range filtering. Range filtering mainly uses direct filtering and conditional filtering to perform a large-scale screening of the coastal scene point cloud and remove radar points that are irrelevant to the shoreline extraction task. The logic of direct filtering is: set the filtering range according to the x, y, and z coordinates; traverse each point in the point cloud and compare whether its coordinates meet this range; filter the points that meet the filtering conditions and retain the remaining points. Direct filtering can only perform large-area filtering for a certain coordinate at a time, while conditional filtering filters points that meet one or more conditions through conditional statements such as "AND" and "OR". For the shoreline extraction task, the point cloud information behind the vehicle itself does not need to be considered, so here the points with x coordinate less than 0 are filtered through direct filtering. In order to avoid the influence of the radar points reflected by the vehicle itself on the shoreline recognition, the vehicle's own point cloud is filtered through conditional filtering. Taking the point cloud data after direct filtering as the input, set the filtering condition as -L / 2 < x < L / 2 and -W / 2 < y < W / 2, and filter the points that meet the conditions to obtain the remaining point cloud.

[0081] After completing the range filtering, it is also necessary to further simplify the point cloud. Here, the discrete point filtering is used to remove outliers, noise points, etc. in the point cloud.

[0082] During implementation, for each point in the point cloud, the distance from each neighboring point to the point is calculated. The neighboring point is the point closest to the point. The number of neighboring points is N. L For example, it can be set to 60, then the 60 points closest to the point are the neighborhood points of the point.

[0083] When implementing, calculate this N L The Euclidean distance from each point to the adjacent point is calculated as follows:

[0084]

[0085] Among them, (x self ,y self , z self ) is the coordinate of the point traversed, (x i ,y i , z i ) is the coordinate of its i-th neighborhood point. After completing the distance calculation, these distances are statistically analyzed to calculate the mean and standard deviation of the distances. The calculation formulas for the mean and standard deviation are as follows:

[0086]

[0087] Among them, d i is the distance from the i-th adjacent point of the point to the point, D is the mean distance from the point's neighborhood points to the point, and σ is the standard deviation of the distance.

[0088] The distance between this point and all points in the neighborhood conforms to the Gaussian distribution. Therefore, the threshold is set by distance and standard deviation to filter the discrete points. When the distance from the neighborhood point to this point is greater than (D+2.5σ), the neighborhood point is considered to be a discrete point and is filtered. Traverse all points in the point cloud, perform the above calculations, filter out the discrete points, and obtain the filtered point cloud.

[0089] After removing discrete and interfering points, the coastline is extracted based on the filtered point cloud. For coastline extraction, the boundary is extracted first, and then non-coastline points are filtered to obtain the plane where the coastline is located and extract the coastline points.

[0090] Specifically, identifying the boundary points in the filtered point cloud based on the normal vector of the point cloud to obtain the boundary point cloud includes:

[0091] Calculate the normal vector of each point in the filtered point cloud;

[0092] Traverse each point in the filtered point cloud, calculate the angle between the point and the normal vector of each of its neighboring points, and if the ratio of the number of neighborhood points with an angle greater than the fourth threshold to the total number of neighborhood points of the point exceeds the fifth threshold, then the point is a boundary point;

[0093] All boundary points constitute the boundary point cloud.

[0094] In implementation, for each point in the filtered point cloud, a KD tree can be used to find its nearest N L Neighborhood points are used to calculate the normal vector. First calculate this N L The center point of each field point:

[0095]

[0096] Among them, μ is the coordinate of the center point, N L is the number of neighborhood points, P i is the coordinate of the i-th field point.

[0097] Then each neighborhood point is centralized. The specific formula is as follows:

[0098] A i =P i -μ

[0099] Among them, A i is the coordinate of the ith neighborhood point after centralization, μ is the coordinate of the center point, P i is the coordinate of the i-th field point.

[0100] For all the centralized neighborhood points, construct them into a matrix A, where each row represents the centralized coordinates of a neighborhood point, and the dimension of the matrix is ​​N L ×3. Then, a covariance matrix C is constructed from matrix A. The principal component analysis is performed using the PCA algorithm to calculate the eigenvalues ​​and eigenvectors of the covariance matrix C, and the eigenvector direction corresponding to the smallest eigenvalue is selected as the direction of the normal vector of the point.

[0101] Since the direction of the normal vector of the boundary point fluctuates greatly with that of its neighboring points, for each point and its neighboring points, the direction relationship between its normal vector and the normal vector of its neighboring points is compared to determine the boundary point.

[0102] Traverse each point in the filtered point cloud, and for the current traversal point, calculate the angle between the normal vector of the point and each of its neighboring points (the number of neighboring points here may be different from the previous one, for example, it may be 45). If the angle is greater than the fourth threshold, it is considered that the point and the neighboring point are not on the same surface. It is known that non-boundary points are often consistent with the points around them, so a neighborhood consistency analysis is performed on the point to count the distribution of the angle between its normal vector and the normal vector of each point in the neighborhood. If the distribution is not concentrated, that is, the ratio of the number of domain points with an angle greater than the fourth threshold to the normal vector of the point to the total number of domain points of the point exceeds the fifth threshold, then the point is considered to be a boundary point, otherwise it is considered not to be a boundary point. During implementation, the fifth threshold can be set according to the calculation accuracy requirements and efficiency requirements.

[0103] The boundary points extracted through the above steps include the outline of the shoreline and the outlines of various objects including shore buildings, people, vehicles, etc. Therefore, in order to extract the shoreline, it is necessary to filter the non-shoreline part of the identified boundary to obtain the shoreline points. During implementation, the shoreline plane point cloud is first extracted, and then the non-shoreline points are filtered out to obtain the shoreline point cloud.

[0104] Considering the fact that the point cloud collected by the actual LiDAR may have uneven density due to the influence of weather and equipment, the contour boundary extracted by the above steps may generate noise. In order to prevent the influence of noise on subsequent steps, all noise points of the boundary points can be filtered here. The same process as S22 can be used to filter noise points, but the number of neighborhood points needs to be reset to achieve the best filtering effect.

[0105] Since the extracted boundary point cloud contour contains the contours of all objects in the environment, it is difficult to directly extract the shoreline points. Therefore, the horizontal plane where the shoreline is located is extracted here to remove the influence of the point cloud of objects in the environment that are higher than the shoreline, which facilitates further extraction.

[0106] Specifically, the shoreline plane point cloud in the boundary point cloud is extracted based on the point cloud plane segmentation, including:

[0107] S31, randomly select three points in the boundary point cloud to obtain a fitting plane;

[0108] S32, determining whether the fitting plane is a valid plane, if not, returning to step S31, otherwise executing step S33;

[0109] S33, calculating the distance between each point in the boundary point cloud and the fitting plane, judging whether each point is an in-plane point based on the distance, and obtaining the number of in-plane points corresponding to the fitting plane; if the number of in-plane points corresponding to the fitting plane is greater than the number of in-plane points corresponding to the optimal plane, the fitting plane is used as the optimal plane; executing step S34;

[0110] S34. If the maximum number of iterations has not been reached, return to step S31; otherwise, stop the iteration, and the points within the plane corresponding to the optimal plane constitute the shoreline plane point cloud.

[0111] Initially, the number of points in the optimal plane is set to zero. Randomly select three points in the boundary point cloud (because the plane is extracted, three points can determine a plane); use these three points to fit the plane, and the fitting adopts the least square method, that is, set the plane equation to Ax+By+Cz=0, for these three points P1(x1,y1), P2(x2,y2) and P3(x3,y3), calculate the normal vector N through the cross product N=V1×V2 of the vector V1=P1-P2 and the vector V2=P1-P3.

[0112] Based on the normal vector of the fitted plane, it is determined whether the fitted plane is a valid plane. If the angle between the normal vector and the z-axis is greater than a preset angle threshold, it is considered to be an invalid plane, and the process returns to step S31 for the next iteration to find a new plane.

[0113] If it is not greater than the preset angle threshold, it is a valid plane. Calculate the distance of each point from the boundary point cloud to the valid plane. If the distance is greater than the preset distance threshold, the point is an out-of-plane point of the valid plane, otherwise it is a surface point of the valid plane, and record the number of in-plane points of the valid plane. Determine whether the number of in-plane points of the valid plane is greater than the number of in-plane points of the current optimal plane. If so, the valid plane is used as the optimal plane, that is, the plane with more in-plane points is set as the optimal plane. If the number of iterations is not reached, return to step S31 for the next iteration, otherwise stop the iteration, and the optimal plane obtained is the shoreline plane point cloud containing the shoreline.

[0114] Then, based on point cloud clustering, the non-shoreline points in the shoreline plane point cloud are filtered to obtain the shoreline point cloud, which includes:

[0115] S41, clustering the shoreline plane point cloud based on the distance between points in the shoreline plane point cloud;

[0116] During implementation, the point cloud is divided according to the connectivity between points. Specifically, it includes:

[0117] S411, setting the current category serial number to zero;

[0118] S412, randomly selecting an unclassified point in the shoreline plane point cloud, setting the category number of the unclassified point as the current category number; and adding the unclassified point to the to-be-searched set;

[0119] S413, taking the first to-be-searched point in the to-be-searched set as the current to-be-searched point, calculating the distance between each unclassified neighboring point of the current to-be-searched point and the current to-be-searched point; setting the category number of the unclassified neighboring point whose distance is less than the eighth threshold as the current category number, and adding it to the to-be-searched set; deleting the current to-be-searched point from the to-be-searched set;

[0120] S413. If the set to be searched is not empty, return to step S413; otherwise, increase the current category number by one; if there are unclassified points in the shoreline plane point cloud, return to step S412; otherwise, end clustering.

[0121] Initially, the current category serial number index is 0. First, starting from a certain point in the point cloud, its category needs to be marked as index, and then it is necessary to find its similar points. Therefore, it is added to the set to be found. At this time, there is a point in the set to be found. Take this point in the set to be found as the current point to be found, and calculate the distance from its neighboring points to the current point to be found. The neighboring points can be obtained through KD tree search, and the number of neighboring points can be set to 25. If the distance from the neighboring point to the point is less than the eighth threshold, it is considered that the domain point and the point to be found belong to the same type of point, and the category serial number of the domain point is marked as index, and the neighborhood point is added to the set to be found. After traversing each neighboring point of the current point to be found, the current point to be found is deleted from the set to be found. If the set to be searched is not empty at this time, continue to execute S413 until the set of points to be searched is empty, indicating that the same type of points in the current category have been found. Therefore, the current category number is increased by one. If there are unclassified points in the shoreline plane point cloud, return to step S412 to start searching for a new category. Otherwise, all points have been classified and clustering is ended.

[0122] S42, for each type of point cloud, determine whether the point cloud is a linear structure, if not, filter out the point cloud, otherwise retain the point cloud to obtain an initial shoreline point cloud;

[0123] Specifically, the following formula is used to determine whether this type of point cloud is a linear structure:

[0124] S421, for each point in the class, calculating the distance between the point and its neighboring points; if the number of neighboring points whose distance is less than the sixth threshold is 1, the point is an endpoint;

[0125] When implemented, the distance can be Euclidean distance. The neighboring points of the point are the neighboring points in the class.

[0126] S422. If there are two endpoints of the class, calculate the horizontal distance between the two endpoints. If the horizontal distance is greater than the seventh threshold, the class is a linear structure, otherwise it is not a linear structure.

[0127] Count the number of endpoints of each class. Linear structures such as coastlines will have two endpoints, so classes with more than two endpoints can be filtered out.

[0128] The shape of the point cloud of other objects left on the extracted plane after processing may be a "C"-like shape, and there will also be two endpoints. Therefore, the horizontal distance between the two endpoints, that is, the distance on the xy plane, is calculated. According to expert experience, if the shoreline is landable, its span must be greater than three times the width of the vehicle. Therefore, the horizontal distance is compared with the seventh threshold (3W). If the horizontal distance is less than the seventh threshold, such point clouds are filtered; otherwise, they are retained.

[0129] The retained point cloud constitutes the initial shoreline point cloud, and its class attributes are still retained. Then, the initial shoreline point cloud is filtered based on the current working condition of the amphibious unmanned vehicle to obtain the final shoreline point cloud.

[0130] Specifically include:

[0131] If the current working condition of the amphibious unmanned vehicle is the water navigation condition, then find the point with the smallest x coordinate value in the initial shoreline point cloud (x xmin ,y xmin ,z xmin ), filter out the initial shoreline point cloud with x value coordinates greater than (x xmin +5); in the remaining point clouds, search for point cloud classes with a span greater than the seventh threshold to form a shoreline point cloud;

[0132] If the current working condition of the amphibious unmanned vehicle is to be launched into the water, then find the point with the largest x coordinate value in the initial shoreline point cloud (x xmax ,y xmax ,z xmax ), filter out the initial shoreline point cloud with x value coordinates less than (x xmax -5); in the remaining point clouds, search for point cloud classes whose span is greater than the seventh threshold to form a shoreline point cloud.

[0133] It should be noted that the span is the span in the y direction.

[0134] The points in the "shoreline point cloud" are the identified landable shoreline point clouds. If the "shoreline point cloud" is empty, that is, there is no class with a span greater than the seventh threshold, the shoreline is considered to be unlandable, and "the shoreline is unlandable" is returned. The recognition algorithm of the present invention is used for computer vision detection to quickly and accurately identify the shoreline, thereby helping the amphibious unmanned vehicle to achieve smooth and accurate working condition conversion.

[0135] After identifying the shoreline, the amphibious unmanned vehicle will perform cross-domain tasks and switch working conditions. In order to avoid false triggering of working condition switching during the cross-domain process, the shoreline point cloud recognition algorithm of steps S1-S4 needs to be turned off and turned on again after the cross-domain task is completed. That is, after step S4, the shoreline recognition method for cross-domain landing scenes of amphibious unmanned vehicles based on lidar also includes:

[0136] Calculate and project the coordinate origin of the radar coordinate system at the current moment onto the shoreline plane;

[0137] Calculate the minimum distance from the projection point to the shoreline point cloud. If the minimum distance is less than the ninth threshold, set the algorithm shutdown flag to true.

[0138] At each moment after the algorithm shutdown flag is set to true, determine whether the algorithm shutdown flag is true. If so, calculate the distance traveled by the amphibious unmanned vehicle from the algorithm shutdown moment to the current moment. If it exceeds the tenth threshold, the algorithm shutdown flag is set to false and step S1 is executed.

[0139] The logic of turning on and off the shoreline point cloud recognition algorithm is as follows: Figure 2 As shown. During implementation, the algorithm shutdown flag is initially set to false. Before executing step S1 at each moment, first determine whether the algorithm shutdown flag is true. If not, execute step S1. If so, proceed to the next frame. Before cross-domain, the algorithm shutdown flag is false. After identifying the shoreline (the shoreline point cloud is not empty), the distance between the current amphibious unmanned vehicle and the shoreline is determined. When the distance is less than the ninth threshold, it is considered that the amphibious unmanned vehicle is to perform a cross-domain task. At this time, the algorithm shutdown flag is set to true, and steps S1-S4 are no longer executed at each subsequent moment. Instead, the distance traveled by the amphibious unmanned vehicle from the algorithm shutdown moment to the current moment is calculated. If it exceeds the tenth distance, it is considered that the amphibious unmanned vehicle has completed the cross-domain task, then the algorithm shutdown flag is set to false, the shoreline point cloud is set to empty, and the algorithm is turned on, that is, step S1 is executed. Each time the shoreline is identified, the algorithm needs to be turned off when crossing the domain to avoid false triggering of the working condition switch.

[0140] Those skilled in the art will appreciate that all or part of the processes of the above-mentioned embodiments can be implemented by instructing related hardware through a computer program, and the program can be stored in a computer-readable storage medium, wherein the computer-readable storage medium is a disk, an optical disk, a read-only storage memory, or a random access memory, etc.

[0141] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.

Claims

1. A method for identifying landable shorelines for amphibious unmanned vehicles in cross-domain scenarios based on laser radar, characterized in that: The following steps are involved: Acquire the laser radar point cloud data of the amphibious unmanned vehicle at the current moment; and determine the current working condition of the amphibious unmanned vehicle based on the point cloud data; If the current working condition is to be launched into water or to sail on water, the point cloud data is filtered; the boundary points in the filtered point cloud are identified based on the normal vector of the point cloud to obtain a boundary point cloud; Extract shoreline plane point cloud from boundary point cloud based on point cloud plane segmentation; Based on point cloud clustering, non-shoreline points in the shoreline plane point cloud are filtered to obtain the shoreline point cloud.

2. The laser radar-based method for identifying landable shorelines for amphibious unmanned vehicles in cross-domain scenarios according to claim 1 is characterized in that: After obtaining the laser radar point cloud data of the amphibious unmanned vehicle at the current moment, before judging the current working condition of the amphibious unmanned vehicle based on the point cloud data, it also includes correcting each point in the obtained laser radar point cloud data in the following manner: p' i =P i ·R R=R roll ·R pitch Among them, P i represents the original coordinates of the i-th point, p' i represents the corrected coordinates of the i-th point, R represents the comprehensive rotation matrix, θ represents the pitch angle of the amphibious unmanned vehicle, and R pitch represents the pitch angle correction matrix, φ represents the roll angle of the amphibious unmanned vehicle, R roll Represents the roll angle correction matrix.

3. According to the laser radar-based method for identifying landable shorelines in cross-domain scenarios for amphibious unmanned vehicles according to claim 1, it is characterized in that: The current working condition of the amphibious unmanned vehicle is determined based on the point cloud data in the following manner: Calculate the number of points within the first range around the current amphibious unmanned vehicle; If the number of points is less than the first threshold, the amphibious unmanned vehicle is currently in a water navigation condition; If the number of points exceeds the second threshold, and the ratio of the number of points in front of the amphibious unmanned vehicle to the number of points in the rear of the amphibious unmanned vehicle is less than the third threshold, the amphibious unmanned vehicle is currently in the waiting to launch condition; otherwise, the amphibious unmanned vehicle is currently in other conditions.

4. According to the laser radar-based method for identifying landable shorelines in cross-domain scenarios for amphibious unmanned vehicles according to claim 1, it is characterized in that: The point cloud data is filtered in the following manner: Performing initial filtering on the point cloud data by using straight-through filtering and conditional filtering; For each point after the initial filtering, the distance from each neighboring point to the point is calculated, and based on the distance from each neighboring point to the point, it is determined whether each neighboring point of the point is a discrete point. If so, the discrete point is filtered out.

5. According to the laser radar-based method for identifying landable shorelines in cross-domain scenarios for amphibious unmanned vehicles according to claim 1, it is characterized in that: Based on the normal vector of the point cloud, the boundary points in the filtered point cloud are identified to obtain the boundary point cloud, including: Calculate the normal vector of each point in the filtered point cloud; Traverse each point in the filtered point cloud, calculate the angle between the point and the normal vector of each of its neighboring points, and if the ratio of the number of neighborhood points with an angle greater than the fourth threshold to the total number of neighborhood points of the point exceeds the fifth threshold, then the point is a boundary point; All boundary points constitute the boundary point cloud.

6. According to the laser radar-based method for identifying landable shorelines in cross-domain scenarios for amphibious unmanned vehicles according to claim 1, it is characterized in that: Extract the shoreline plane point cloud from the boundary point cloud based on point cloud plane segmentation, including: S31, randomly select three points in the boundary point cloud to obtain a fitting plane; S32, determining whether the fitting plane is a valid plane, if not, returning to step S31, otherwise executing step S33; S33, calculating the distance from each point in the boundary point cloud to the fitting plane, judging whether each point is an in-plane point based on the distance, and obtaining the number of in-plane points corresponding to the fitting plane; if the current iteration is the first time, the fitting plane is taken as the optimal plane, otherwise, if the number of in-plane points corresponding to the fitting plane is greater than the number of in-plane points corresponding to the optimal plane, the fitting plane is taken as the optimal plane; executing step S34; S34. If the maximum number of iterations has not been reached, return to step S31; otherwise, stop the iteration, and the points within the plane corresponding to the optimal plane constitute the shoreline plane point cloud.

7. According to the laser radar-based method for identifying landable shorelines in cross-domain scenarios for amphibious unmanned vehicles according to claim 1, it is characterized in that: Based on point cloud clustering, non-shoreline points in the shoreline plane point cloud are filtered to obtain the shoreline point cloud, including: Clustering the shoreline plane point cloud based on the distance between points in the shoreline plane point cloud; For each type of point cloud, determine whether the point cloud is a linear structure. If not, filter out the point cloud. Otherwise, keep the point cloud to obtain the initial shoreline point cloud. The initial shoreline point cloud is filtered based on the current working condition of the amphibious unmanned vehicle to obtain the final shoreline point cloud.

8. According to the laser radar-based method for identifying landable shorelines in cross-domain scenarios for amphibious unmanned vehicles according to claim 7, it is characterized in that: Clustering of the shoreline plane point cloud based on the distance between points in the shoreline plane point cloud includes: S411, setting the current category serial number to zero; S412, randomly selecting an unclassified point in the shoreline plane point cloud, setting the category number of the unclassified point to the current category number; and adding the unclassified point to the to-be-searched set; S413, taking the first to-be-searched point in the to-be-searched set as the current to-be-searched point, calculating the distance between each unclassified neighboring point of the current to-be-searched point and the current to-be-searched point; setting the category number of the unclassified neighboring point whose distance is less than the eighth threshold as the current category number, and adding it to the to-be-searched set; deleting the current to-be-searched point from the to-be-searched set; S413. If the set to be searched is not empty, return to step S413; otherwise, increase the current category number by one; if there are unclassified points in the shoreline plane point cloud, return to step S412; otherwise, end clustering.

9. According to the laser radar-based method for identifying landable shorelines in cross-domain scenarios for amphibious unmanned vehicles according to claim 7, it is characterized in that: The following formula is used to determine whether this type of point cloud is a linear structure: For each point in the class, calculate the distance from the point to its neighboring points; if the number of neighboring points whose distance is less than the sixth threshold is 1, the point is an endpoint; If the class has two endpoints, the horizontal distance between the two endpoints is calculated. If the horizontal distance is greater than the seventh threshold, the class is a linear structure, otherwise it is not a linear structure.

10. According to the method for identifying shoreline in cross-domain landing scenarios of amphibious unmanned vehicles based on laser radar according to claim 7, it is characterized in that: Based on the current working condition of the amphibious unmanned vehicle, the initial shoreline point cloud is filtered to obtain the final shoreline point cloud, including: If the current working condition of the amphibious unmanned vehicle is the water navigation condition, then find the point with the smallest x coordinate value in the initial shoreline point cloud (x xmin ,y xmin ,z xmin ), filter out the initial shoreline point cloud with x value coordinates greater than (x xmin +5); in the remaining point clouds, search for point cloud classes with a span greater than the seventh threshold to form a shoreline point cloud; If the current working condition of the amphibious unmanned vehicle is to be launched into the water, then find the point with the largest x coordinate value in the initial shoreline point cloud (x xmax ,y xmax ,z xmax ), filter out the initial shoreline point cloud with x value coordinates less than (x xmax -5); in the remaining point clouds, find the point cloud classes whose span is greater than the seventh threshold to form the shoreline point cloud.

Images