An obstacle avoidance method for unmanned boat based on laser radar angle correction

Abstract

The present invention discloses an unmanned boat obstacle avoidance method based on laser radar angle correction. The unmanned boat obstacle avoidance method is as follows: 1. The laser radar equipment installed on the unmanned boat continuously collects data; 2. The clustering method is performed on the point cloud data; 3. The obstacle points in each cluster are linearly judged respectively. 4. Each cluster is scanned and corrected respectively. 5. According to the position and width of each obstacle determined by each cluster, the unmanned boat avoids the obstacle and continues to travel. The method of correcting obstacle detection points based on the laser radar attitude angle data in the present invention effectively reduces the obstacle measurement error caused by the change of the laser radar attitude, so that the processed obstacle information is closer to the real data. After correcting the error scanning point, the present invention corrects the robot's inaccessible area caused by the original erroneous scanning point into a reachable area, and selects the obstacle avoidance path according to the obstacle avoidance weight of each sector, so that the surface robot can achieve accurate obstacle avoidance.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned boat obstacle avoidance technology, and specifically relates to a method for an unmanned boat to avoid water surface obstacles based on laser radar angle data correction. Background Art

[0002] Domestic and foreign scholars have conducted some research on the obstacle avoidance problem of surface unmanned boats. Before obstacle avoidance, the obstacle detection method is crucial. The current mainstream obstacle detection technologies are mainly divided into the following four types: ultrasonic obstacle detection technology, stereoscopic vision obstacle detection technology, multi-sensor fusion obstacle detection technology and lidar obstacle detection technology. Ultrasonic obstacle detection technology mainly uses the echo characteristics of sound waves to calculate the position of obstacles. Due to the Doppler effect of sound waves, if the driving speed reaches a certain level, it will cause data anomalies. Stereoscopic vision obstacle detection technology mainly uses mathematical principles in image processing to expand the plane two-dimensional vision technology to three-dimensional space to obtain more target object information, but it will increase the cost and complexity of information processing. Multi-sensor fusion obstacle detection technology mainly processes, optimizes and merges the information of two or more similar or heterogeneous sensors, so as to obtain more environmental feature expressions, but the difficulty lies in the fusion algorithm. In fact, it is difficult to select a suitable fusion algorithm that can match multiple identical or different sensor devices. Laser radar obstacle detection technology mainly uses the time difference between the emitted laser line and the reflected laser line to calculate the angle and distance value of the obstacle point. The detection system has the advantages of high precision, strong resolution, and good real-time performance. The obstacle avoidance method for unmanned surface boats based on traditional two-dimensional laser radars is based on the premise that the obstacle information detected by the laser radar is correct for obstacle avoidance path planning. However, in actual applications, the two-dimensional laser radar will cause the measurement point to be distorted due to the change in the attitude angle of the laser radar mounted on the unmanned boat, which is inconsistent with the actual measured obstacle information. For any obstacle avoidance path planning method, only by obtaining the real data of the obstacle in the early stage can a better effect of avoiding surface obstacles be achieved. The present invention proposes an unmanned boat obstacle avoidance method based on laser radar angle data correction, which uses the angle data correction method to improve the obstacle positioning effect, and calculates the obstacle avoidance cost according to the fan-shaped area to realize the obstacle avoidance angle decision. Summary of the invention

[0003] The purpose of the present invention is to provide a method for correcting obstacle detection points and selecting effective obstacle avoidance sectors based on laser radar attitude angle data, and an unmanned boat can effectively avoid surface obstacles according to this method.

[0004] The unmanned boat obstacle avoidance method based on laser radar angle correction includes the following steps:

[0005] Step 1: The laser radar device installed on the unmanned boat continuously collects data; after the laser radar device collects non-empty point cloud data P, the attitude sensor device installed on the unmanned boat detects the current pitch angle θ and roll angle φ of the unmanned boat, and performs median filtering on the point cloud data P.

[0006] Step 2: Perform clustering on the point cloud data P to obtain multiple clusters P1, P2, ..., P k ; k is the number of clusters.

[0007] Step 3: Perform linear judgment on the obstacle points in the k clusters respectively.

[0008] Step 4: Scan and correct each cluster separately; if the obstacle points in a cluster are linear, the rectangular obstacle scanning point correction method is executed; if the obstacle points in a cluster are nonlinear, the rectangular obstacle scanning point correction method is executed.

[0009] The specific method for correcting the scanning points of rectangular obstacles is as follows:

[0010] Extract the polar coordinates of the measured edge scanning point L in each obstacle point of the corrected cluster as P L (r L ,a L ), and the polar coordinates P of the measured front vertical point M M (r M ,a M );

[0011] According to point L and point M, the correction is obtained to obtain point J and point I when the unmanned boat is in a horizontal state. The polar coordinates P of point J and point I are J , P I The expression is as follows:

[0012]

[0013] Among them, Φ, are two transition parameters, and their expressions are as follows:

[0014]

[0015]

[0016] Point J is the edge position of one side of the obstacle in the unmanned boat's head-on posture; point I is the position of the obstacle facing the unmanned boat in the unmanned boat's head-on posture. The method of obtaining the edge position W of the other side of the obstacle in the unmanned boat's head-on posture is the same as point J; according to points I, J, and W, the position and width of the obstacle are determined.

[0017] The specific method for correcting the scanning points of cylindrical obstacles is as follows:

[0018] Extract the polar coordinates P of the measured edge scanning points D and F in each obstacle point of the corrected cluster D (r D ,a D ), P F (r F ,a F ), the polar coordinates P of the middle point E E (r E ,a E ).

[0019] Points H and C measured by the unmanned boat in a horizontal state can be corrected based on points F and D. The polar coordinates P of points H and C are H , P C The expression is as follows:

[0020]

[0021]

[0022]

[0023] Among them, Φ, are two transition parameters, and their expressions are as follows:

[0024] Φ=sin -1 (cos(|a D -a E |)·sinθ+sin(|a D -a E |)·sinφ)

[0025]

[0026] Point C is the edge position of one side of the obstacle in the unmanned boat's head-on posture; point H is the position of the center of the obstacle scanning point in the unmanned boat's head-on posture. The method of obtaining the edge position A of the other side of the obstacle in the unmanned boat's head-on posture is the same as point C; based on points A and C, the position and width of the obstacle are determined.

[0027] Step 5: According to the positions and widths of obstacles determined by each cluster, the unmanned boat avoids the obstacles and continues to move forward.

[0028] Preferably, in step 1, the point cloud data P is in polar coordinate form, and the i-th obstacle point P of the point cloud data P is i The polar coordinate point is (r i ,a i ), where r i represents the straight-line distance between the i-th obstacle point and the laser radar; a iIt represents the angle between the ray formed by the origin of the polar coordinate system where the laser radar is located and the i-th obstacle point and the 0° baseline of the laser radar scanning polar coordinate system, i = 1, 2, ..., N. N is the total number of obstacle points in the point cloud data.

[0029] Preferably, in step 3, the Pearson correlation coefficient method is used to perform linear judgment in the same cluster, and the specific process is as follows:

[0030] The Pearson correlation coefficient ρ(X,Y) is calculated as follows:

[0031]

[0032]

[0033]

[0034]

[0035] Among them, X i Y represents the distance parameter of the i-th polar coordinate of the cluster being judged; i represents the angle parameter of the i-th polar coordinate in the cluster being judged; μ X represents the mean of all polar coordinate distance parameters in the cluster being judged; μ Y represents the mean of all polar coordinate angle parameters in the cluster being judged; cov(X,Y) represents the covariance of all polar coordinate distances and angle parameters in the cluster being judged; σ X , σ Y They represent the distance parameter standard deviation and angle parameter standard deviation of all polar coordinates in the cluster being judged; ρ(X,Y) represents the cluster P j Pearson correlation coefficient values ​​for polar coordinates.

[0036] According to the value of the Pearson correlation coefficient ρ(X,Y), it is determined whether each obstacle point in the judged cluster is linear or nonlinear.

[0037] As a preferred embodiment, the specific process of step five is as follows:

[0038] 5-1. Detection range R of the laser radar sensor on the unmanned boat Lidar Partitioning, the number of partitions is n, then the resolution Therefore, the starting angle of the x-th partition is λ=α(x-1).

[0039] 5-2. Sector cost evaluation. The n partitions are grouped into a set T{s1,s2,...,s n}, each element s xRepresents the value corresponding to the xth sector. First, initialize all elements in the set T to 0; then, scan point P according to the obstacle i (r i ,a i ) polar coordinate data, determine that the obstacle point is located in the xth partition, and then set the xth element s in the set T x The value is set to 1, and the value judgment is as follows:

[0040]

[0041] 5-3. Candidate movement direction selection. According to the currently updated set T, determine the next movement direction of the unmanned boat. In the set T, the element s x The value of 1 indicates an infeasible partition, and the value of 0 indicates a feasible partition. Continuous partitions with element values ​​of 0 will constitute a feasible motion area, and the unmanned boat will preferentially choose the middle direction of the feasible motion area closest to the current unmanned boat's heading angle as the direction of travel.

[0042] The beneficial effects of the present invention are as follows:

[0043] 1. The method of correcting obstacle detection points based on the laser radar attitude angle data in the present invention effectively reduces the obstacle measurement error caused by the change of the laser radar attitude, making the processed obstacle information closer to the real data.

[0044] 2. After correcting the erroneous scanning points, the present invention corrects the robot's inaccessible area caused by the original erroneous scanning points into a reachable area, and selects an obstacle avoidance path according to the obstacle avoidance weight of each sector, so that the surface robot can achieve accurate obstacle avoidance and reach the target point faster and more efficiently. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 It is a flow chart of the obstacle avoidance method based on laser radar angle correction of the present invention;

[0046] Figure 2 It is a schematic diagram of horizontal scanning of the laser radar of the present invention;

[0047] Figure 3 is a schematic diagram of a laser radar deviation scan of the present invention;

[0048] Figure 4 It is a schematic diagram of a rectangular parallelepiped obstacle detection error correction model of the present invention;

[0049] Figure 5 It is a schematic diagram of the cylindrical obstacle detection error correction model of the present invention;

[0050] Figure 6 It is a schematic diagram of the laser radar scanning range partitioning of the present invention;

[0051] Figure 7 It is a schematic diagram of the unmanned boat navigation scene of the present invention;

[0052] Figure 8 is a schematic diagram of obstacle avoidance decision-making of an unmanned boat of the present invention;

[0053] Fig. 9 This is a comparison diagram of the measurement errors before and after the angle correction of the rectangular obstacle of the present invention;

[0054] Fig.10 It is a comparison diagram of the measurement errors before and after the angle correction of the cylindrical obstacle of the present invention. DETAILED DESCRIPTION

[0055] The present invention is further described below with reference to the accompanying drawings.

[0056] like Figure 1 As shown in FIG. 1 , the obstacle avoidance method of the unmanned boat based on the correction of the laser radar angle data includes the following main steps:

[0057] Step 0: Get the point cloud data returned by the laser radar device fixed on the unmanned boat at the current moment; if no point cloud data is returned, the unmanned boat continues to move forward in the current navigation direction without the need for subsequent steps. i (r i ,a i ),i=1,2,3,...,N; where P i (r i ,a i ) represents the polar coordinate point of the ith obstacle point; ri represents the straight-line distance between the ith obstacle point and the laser radar; a i represents the angle between the ray formed by the origin of the polar coordinate system where the laser radar is located and the ith obstacle point and the 0° baseline of the laser radar scanning polar coordinate system; N is the total number of obstacle points returned by the laser radar device; then, the attitude angle data returned by the attitude sensor device fixed on the unmanned boat at the current moment is obtained, including the pitch angle θ and the roll angle φ, such as Figure 2 , Figure 3 shown.

[0058] Step 1: Perform median filtering on the LiDAR point cloud data obtained in step 0. Median filtering is a fast and efficient nonlinear smoothing method that has a good effect on suppressing pulse interference level noise. The filtered data retains the original change trend while removing the impact of spike pulses on the analysis. The main operation method is to sort the values ​​in a sliding window and replace the value of the original window center element with the sorted median, as shown in the following formula:

[0059] P mid =med{Pi-m ,...,P i-1 ,P i ,P i+1 ,...,P i+m}

[0060] Among them, P mid is the data after median filtering, med{} represents the function of taking the middle value of the data sequence, P i is the single LiDAR obstacle scanning point data, and m is the size of the preset sliding window area, which is set to 5.

[0061] Step 2: Use K-means clustering to classify obstacle scanning points on the laser radar point cloud data filtered in step 1. The K-means clustering method is simple and fast, and can meet the real-time requirements of unmanned boat environment detection.

[0062] The implementation steps of the K-means clustering method are divided into the following five steps:

[0063] 2-1. In order to effectively eliminate the influence of noise on obstacle detection, the present invention sets the number of clusters k to 3, so that the algorithm is finally divided into k groups.

[0064] 2-2. Randomly select k points from the lidar point cloud data processed in step 1 as the initial center of each cluster.

[0065] 2-3. The distance (Euclidean distance) between the selected data object and the k cluster centers is used as the clustering eigenvalue. By calculating the eigenvalue of each data object, each discrete point in the lidar point cloud data is classified into the cluster corresponding to the cluster center closest to it.

[0066] 2-4. When all discrete points are clustered, calculate the new cluster center for each cluster.

[0067] 2-5. Repeat steps 2-3 and 2-4 until the distance between the cluster center updated in step 2-4 and the original cluster center is less than the preset threshold (i.e., the cluster center basically no longer changes), and clustering is completed and stopped. The lidar point cloud data is finally divided into k groups, i.e., the lidar point cloud data P{P1,P2,...,P k}.

[0068] Step 3: After K-means clustering, the Pearson correlation coefficient method is used to perform linear judgment on the obstacle points in each cluster, that is, to determine whether the data points belonging to the same class are linear or nonlinear.

[0069] The Pearson correlation coefficient ρ(X,Y) is calculated by using the jth cluster P in the current k group. jThe covariance cov(X,Y) between the polar coordinate points is obtained by dividing its standard deviation, as shown in the formula:

[0070]

[0071]

[0072]

[0073]

[0074] Among them, X i represents the jth cluster P j The distance parameter of the i-th polar coordinate in Y i represents the jth cluster P j The angle parameter of the i-th polar coordinate in; μ X represents the jth cluster P j The mean of all polar distance parameters in ; μ Y represents the jth cluster P j The mean of all polar coordinate angle parameters in; cov(X,Y) represents the jth cluster P j The covariance of all polar distance and angle parameters in ; σ X , σ Y Respectively represent the j-th cluster P j The distance parameter standard deviation and angle parameter standard deviation of all polar coordinates in; ρ(X,Y) represents the jth cluster P j Pearson correlation coefficient values ​​for polar coordinates.

[0075] The Pearson correlation coefficient ρ(X,Y) can normalize the output range to [-1,+1]. 0 represents no correlation in the data, negative values ​​represent negative correlation, and positive values ​​represent positive correlation. When the linear relationship between two variables increases, the correlation coefficient tends to 1 or -1; when one variable increases and the other variable also increases, it indicates that they are positively correlated, and the correlation coefficient is greater than 0; if one variable increases and the other variable decreases, it indicates that they are negatively correlated, and the correlation coefficient is less than 0; if the correlation coefficient is equal to 0, it indicates that there is no linear correlation between them. When calculating the Pearson correlation coefficient of the same type of data points, the absolute value of the coefficient exceeding the threshold Th is judged as linearly correlated data, otherwise it is classified as nonlinearly correlated data. This article sets the threshold Th to 0.9.

[0076] Step 4. Determine whether the data in each cluster is linear based on the Pearson correlation coefficient corresponding to each cluster obtained in step 3; based on the linear judgment result, take different correction methods for obstacles with linear or nonlinear laser radar scanning surfaces. Since the unmanned boat is equipped with inertial navigation and other attitude sensors, it is possible to accurately measure the current attitude angle of the unmanned boat, so the laser radar data can be calibrated according to the current attitude angle. Perform step 4-1 for clusters with linear correlation and step 4-2 for clusters with nonlinear correlation. The models of rectangular obstacles and cylindrical obstacles are as follows: Figure 4 and Figure 5 shown.

[0077] 4-1. Scanning point correction for rectangular obstacles

[0078] Correction of the scanning points of a rectangular obstacle requires two prerequisites: 1. When looking down at the obstacle, there are scanning points where the laser radar scanning line is perpendicular to the front of the obstacle; 2. When looking straight at the obstacle, the leftmost and rightmost scanning points fall on the edges of the two sides of the rectangular obstacle.

[0079] Extract the polar coordinates P of the measured edge scanning point L of any clustered rectangular obstacle L (r L ,a L ), the polar coordinates P of the measured front vertical point M (i.e., the obstacle scanning point detected by the laser radar scanning line perpendicular to the front of the obstacle in a bird's-eye view) M (r M ,a M );

[0080] Due to the change of attitude angle when the unmanned boat is sailing in the water, the polar coordinate P L (r L ,a L ) and P D (r D ,a D ) may be measured when the head of the unmanned boat is tilted upward or downward, so it is necessary to correct the points J and I measured by the unmanned boat in a horizontal state based on points L and M. The polar coordinates P of points J and I are J , P I The expression is as follows:

[0081]

[0082] Among them, Φ, are two transition parameters, and their values ​​are obtained through ΔIOM, ΔJOL, and ΔVOM. The expressions are as follows:

[0083]

[0084]

[0085] Point J is the edge position of one side of the obstacle in the unmanned boat's head-on posture; point I is the position of the obstacle facing the unmanned boat in the unmanned boat's head-on posture. The method of obtaining the edge position W of the other side of the obstacle in the unmanned boat's head-on posture is the same as point J; according to points I, J, and W, the position and width of the obstacle are determined so that the unmanned boat can avoid the obstacle.

[0086] 4-2. Scanning point correction for cylindrical obstacles

[0087] To correct the scanning points of a cylindrical obstacle, it is necessary to ensure that the leftmost and rightmost scanning points fall on the edges of the rectangular obstacle when looking directly at the obstacle.

[0088] Extract the polar coordinates P of the measured edge scanning points D and F of any clustered cylindrical obstacle D (r D ,a D ), P F (r F ,a F ), the polar coordinates P of the middle point E E (r E ,a E ), is the midpoint of the line connecting scanning points D and F;

[0089] Due to the change of attitude angle when the unmanned boat is sailing in the water, the polar coordinate P G (r G ,a G ) and P D (r D ,a D ) may be measured when the head of the unmanned boat is tilted upward or downward, so it is necessary to correct the points H and C measured by the unmanned boat in a horizontal state based on points G and D. The polar coordinates P of points H and C are H , P C The expression is as follows:

[0090]

[0091]

[0092]

[0093] Among them, Φ, are two transition parameters, and their values ​​can be obtained through ΔEOD, ΔBOE, and ΔCOD as follows:

[0094] Φ=sin -1 (cos(|a D -aE |)·sinθ+sin(|a D -a E |)·sinφ)

[0095]

[0096] Point C is the edge position of one side of the obstacle in the unmanned boat's head-on posture; point H is the position of the center of the obstacle scanning point in the unmanned boat's head-on posture. The method of obtaining the edge position A of the other side of the obstacle in the unmanned boat's head-on posture is the same as point C; based on points A and C, the position and width of the obstacle are determined so that the unmanned boat can avoid the obstacle.

[0097] Step 5: After obtaining the more accurate LiDAR scanning point cloud data output in step 4, use this environmental perception data to enter the obstacle avoidance decision-making stage.

[0098] The following are the implementation steps of the obstacle avoidance decision method, which is divided into 3 steps:

[0099] 5-1. Detection range R of the unmanned boat’s onboard laser radar sensor Lidar Partitioning, the number of partitions is n, then the resolution Therefore, the starting angle of the x-th partition is λ=α(x-1), as Figure 6 shown.

[0100] 5-2. Sector cost evaluation. The n partitions are grouped into a set T{s1,s2,...,s n}, each element s x Represents the value corresponding to the xth sector. First, initialize all elements in the set T to 0; then, scan point P according to the obstacle i (r i ,a i ) polar coordinate data, determine that the obstacle point is located in the xth partition, and then set the xth element s in the set T x The value of is set to 1, such as Figure 7 As shown, the value judgment is as follows:

[0101]

[0102] 5-3. Candidate movement direction selection. According to the currently updated set T, determine the next movement direction of the unmanned boat. In the set T, the element value of 1 indicates an infeasible partition, and the value of 0 indicates a feasible partition. Continuous partitions with element values ​​of 0 will form a feasible movement area with a certain width, such as Figure 8 As shown, the unmanned boat preferentially selects the feasible motion area that is closest to the current unmanned boat heading angle, and selects the angle direction corresponding to the middle partition of the motion area as the next motion direction.

[0103] Step 6: According to the next navigation direction output in step 5, navigate with a fixed step length step distance, and the step distance is set to 3m. If the destination is reached, the whole process ends; if the destination is not reached, return to step 0.

[0104] In order to verify the effect of the angle correction of the laser radar point cloud data proposed in the present invention, laser radar scanning is performed on a rectangular obstacle with a width of 1m and a cylindrical obstacle with a radius of 0.22m, and the angle correction processing of the laser radar obstacle scanning point cloud data is performed according to the pitch angle and roll angle values ​​of the unmanned boat (5°, 10°, 15°, 20°, 25°, 30°) to verify the specific effect; the results are compared with Fig. 9 and Fig.10 As shown; Fig. 9 (a) to (c) respectively represent the data comparison before and after angle correction of the LiDAR point cloud data when the straight-line distance between the LiDAR and the rectangular obstacle is 4m, 6m, and 8m when the rolling angle of the unmanned boat changes. Fig. 9 (d) to (f) respectively show the comparison of the laser radar point cloud data before and after angle correction when the straight-line distance between the laser radar and the rectangular obstacle is 4m, 6m, and 8m when the pitch angle of the unmanned boat changes. It can be seen that the data after correction makes the error between the measured data and the true width of the obstacle smaller than that before correction. Fig.10 (a) and (b) respectively show the comparison of the angle correction of the laser radar point cloud data before and after the angle correction when the roll angle and pitch angle of the unmanned boat change, under the premise that the straight-line distance of the cylindrical obstacle is 4m; it can be seen that the corrected data makes the error between the measured data and the actual radius of the obstacle smaller than that before correction. The above example verifies that the effect of the angle correction of the laser radar point cloud data proposed by the present invention can make the scanning results closer to the actual environment.

[0105] Due to the effects of water flow and waves, the unmanned boat inevitably has attitude fluctuations such as roll and pitch when sailing, and the scanning results of obstacles on the water surface by the ship-borne two-dimensional laser radar are significantly different from the actual situation. In order to improve the accuracy of the unmanned boat's environmental perception when sailing on the sea, the point cloud data returned by the laser radar for scanning rectangular and cylindrical obstacles is corrected by a polar coordinate angle processing method corresponding to the shape of the obstacle based on the real-time inclination data of the unmanned boat measured by attitude equipment such as inertial navigation. This can improve the accuracy of the unmanned boat's perception of the water surface obstacle environment and the safety of navigation at sea. Finally, the present invention uses the obstacle avoidance weight iteration of the scanning sector to make the unmanned boat's obstacle avoidance direction decision, realizing distributed surface obstacle detection and efficient obstacle avoidance path planning.

Claims

1. An unmanned boat obstacle avoidance method based on laser radar angle correction, characterized in that: The following steps are involved: Step 1: The laser radar device installed on the unmanned boat continuously collects data; after the laser radar device collects non-empty point cloud data P, the attitude sensor device installed on the unmanned boat detects the current pitch angle θ and roll angle φ of the unmanned boat, and performs median filtering on the point cloud data P; Step 2: Perform clustering on the point cloud data P to obtain multiple clusters P1, P2, ..., P k ; k is the number of clusters; Step 3: Perform linear judgment on the obstacle points in the k clusters respectively; the Pearson correlation coefficient method is used for linear judgment in the same cluster, and the specific process is as follows: The Pearson correlation coefficient ρ(X,Y) is calculated as follows: Among them, X i Y represents the distance parameter of the i-th polar coordinate of the cluster being judged; i represents the angle parameter of the i-th polar coordinate in the cluster being judged; μ X represents the mean of all polar coordinate distance parameters in the cluster being judged; μ Y represents the mean of all polar coordinate angle parameters in the cluster being judged; cov(X,Y) represents the covariance of all polar coordinate distances and angle parameters in the cluster being judged; σ X , σ Y They represent the distance parameter standard deviation and angle parameter standard deviation of all polar coordinates in the cluster being judged; ρ(X,Y) represents the cluster P j Pearson correlation coefficient values ​​for polar coordinates; According to the value of Pearson correlation coefficient ρ(X,Y), it is determined whether each obstacle point in the cluster is linear or nonlinear; Step 4: Scan and correct each cluster separately; if the obstacle points in a cluster are linear, the rectangular obstacle scanning point correction method is executed; if the obstacle points in a cluster are nonlinear, the cylindrical obstacle scanning point correction method is executed; The specific method for correcting the scanning points of rectangular obstacles is as follows: The polar coordinates of the measured edge scanning point L in each obstacle point of the corrected cluster are extracted as P L (r L ,a L ), and the polar coordinates P of the measured front vertical point M M (r M ,a M ); According to point L and point M, the correction is obtained to obtain point J and point I of the unmanned boat in the horizontal state; the polar coordinates P of point J and point I J , P I The expression is as follows: Among them, Φ, are two transition parameters, and their expressions are as follows: Point J is the edge position of one side of the obstacle in the unmanned boat's head-on posture; point I is the position of the obstacle facing the unmanned boat in the unmanned boat's head-on posture; the method of obtaining the edge position W of the other side of the obstacle in the unmanned boat's head-on posture is the same as point J; according to points I, J, and W, the position and width of the obstacle are determined; The specific method for correcting the scanning points of cylindrical obstacles is as follows: Extract the polar coordinates P of the measured edge scanning points D and F in each obstacle point of the corrected cluster D (r D ,a D ), P F (r F ,a F ), the polar coordinates P of the middle point E E (r E ,a E ); According to point F and point D, the points H and C measured by the unmanned boat in the horizontal state are corrected; the polar coordinates P of point H and point C are H , P C The expression is as follows: Among them, Φ, are two transition parameters, and their expressions are as follows: Φ=sin-1(cos(a D -a E |)·sinθ+sin(a D -a E |)·sinφ) Point C is the edge position of one side of the obstacle in the unmanned boat's head-on posture; Point H is the position of the center of the obstacle scanning point in the unmanned boat's head-on posture; The method of obtaining the edge position A of the other side of the obstacle in the unmanned boat's head-on posture is the same as that of point C; Based on points A and C, the position and width of the obstacle are determined; Step 5: According to the positions and widths of obstacles determined by each cluster, the unmanned boat avoids the obstacles and continues to travel. The specific process of step 5 is as follows: 5-1. Detection range R of the laser radar sensor on the unmanned boat Lidar Partitioning, the number of partitions is n, then the resolution Therefore, the starting angle of the xth partition is λ = α(x-1); 5-2. Sector cost evaluation: n partitions are grouped into a set T{s1,s2,...,s n }, each element sx represents the value corresponding to the xth sector; first initialize all elements in the set T to 0; then, according to the obstacle scanning point P i (r i ,a i ) polar coordinate data, determine that the obstacle point is located in the xth partition, and then set the xth element s in the set T x The value is set to 1, and the value judgment is as follows: 5-3. Candidate movement direction selection: According to the currently updated set T, determine the next movement direction of the unmanned boat. In the set T, element s x The value of 1 indicates an infeasible partition, and the value of 0 indicates a feasible partition; continuous partitions with element values ​​of 0 will constitute a feasible motion area, and the unmanned boat will preferentially choose the middle direction of the feasible motion area that is closest to the current unmanned boat's heading angle as the direction of travel.

2. The unmanned boat obstacle avoidance method based on laser radar angle correction according to claim 1 is characterized in that: In step 1, the point cloud data P is in polar coordinate form, and the i-th obstacle point P of the point cloud data P is i The polar coordinate point is (ri,ai), where r i represents the straight-line distance between the i-th obstacle point and the laser radar; a i Represents the ray formed by the origin of the polar coordinate system where the laser radar is located and the i-th obstacle point and the laser radar scanning polar coordinate system 0 ° The angle formed by the baseline, i = 1, 2, ..., N; N is the total number of obstacle points in the point cloud data.

Images