A grid-based dynamic occupancy-based proactive cone bucket obstacle avoidance method

Abstract

The present application relates to a kind of based on grid dynamic occupancy look-ahead conical bucket obstacle avoidance method, belong to the field of intelligent car automatic control. Including the position information of 2880 points around obtained by laser radar;The look-ahead angle is obtained by look-ahead algorithm to the position information obtained;The grid dynamic occupancy angle is calculated by grid dynamic occupancy algorithm to the position information obtained;The position information of laser radar is extracted by clustering algorithm, and the clustered angle is obtained;After the look-ahead angle obtained in step S2 is multiplied by alpha 1, the grid dynamic occupancy angle obtained in step S3 is multiplied by alpha 2, the clustered angle obtained in step S4 is multiplied by alpha 3, superposition is carried out, and the final angle is obtained.The present application can efficiently utilize the data transmitted back by laser radar sensor to complete the corresponding best angle output, and high-speed obstacle avoidance can be completed by PID control.

Description

Technical Field

[0001] The present invention relates to a forward-looking cone barrel obstacle avoidance method based on grid dynamic occupancy, which belongs to the field of intelligent vehicle automatic control, and in particular to a method for intelligent vehicles to avoid obstacles on a cone barrel track. Background Art

[0002] With the rapid development and advancement of artificial intelligence technology, the intelligentization of robots has become the ultimate goal of modern robotics. The pace of robotics development is constantly accelerating, and the scope of applications is also expanding. Examples include autonomous driving, robot vacuums, and express delivery robots. While various intelligent robot products have been largely implemented, related challenges remain.

[0003] Intelligent control of robots is the foundation and core of their specific functions. However, when it comes to obstacle avoidance, all robots currently on the market are limited to low-speed control. In areas with numerous obstacles, control using visual or radar sensors is limited to low-speed travel. Obstacle avoidance at high speeds is impossible, requiring only slow control of the servo.

[0004] The existing algorithms used in ROS smart car products have deficiencies when avoiding S-curves with cones. They cannot navigate winding roads like S-curves at high speeds. The movement of smart cars at high speeds can lead to problems such as hitting cones and running out of the target road section. This is because the angles obtained by the current common algorithms through simple vector addition are not suitable for the movement of smart cars at high speeds. Summary of the Invention

[0005] In order to solve the current problem of completing obstacle avoidance at high speeds, carry out automation work normally, improve industrial work efficiency, and promote the progress of the national automation industry, the present invention proposes a new algorithm: a forward-looking cone bucket obstacle avoidance method based on grid dynamic occupancy, which can efficiently use the data returned by the lidar sensor to complete the corresponding optimal angle output, and can complete high-speed obstacle avoidance through PID control.

[0006] The present invention adopts the following technical solutions:

[0007] A forward-looking cone-barrel obstacle avoidance method based on grid dynamic occupancy includes the following steps:

[0008] S1: The laser radar used obtains the location information of 2880 points around it;

[0009] S2: Obtain the forward-looking angle by using the forward-looking algorithm on the obtained position information;

[0010] S3: Calculate the grid dynamic occupation angle using the grid dynamic occupation algorithm based on the obtained position information;

[0011] S4: Extract the position information of the lidar through the clustering algorithm to obtain the clustering angle;

[0012] S5: multiply the forward-looking angle obtained in step S2 by α1, the grid dynamic occupation angle obtained in step S3 by α2, and the cluster angle obtained in step S4 by α3, and then superimpose them to obtain the final angle.

[0013] The present invention performs corresponding cone bucket obstacle avoidance based on the forward-looking algorithm of grid dynamic position, which includes two efficient algorithms: the grid dynamic position algorithm and the forward-looking algorithm, and combines the clustering algorithm, which is used to obtain the angle that the servo should output at this moment according to the position of the cone bucket extracted by the laser radar on the track.

[0014] Preferably, step S5 includes:

[0015] S6: Perform fuzzy PID control on the final angle to output the servo angle;

[0016] S7: The PWM level value of the servo steering is finally output through the conversion of the output servo angle and the PWM level signal.

[0017] Preferably, in step S1, the laser radar adopts a TOF ranging series, supports 360° omnidirectional scanning, and the radar angular resolution is 0.125°, so the position information of 2880 points around the laser radar sensor can be extracted. The laser radar return data includes the echo intensity, return number (recording the number of times the laser beam echoes, etc.), and each point contains x, y, and z coordinate values, that is, the real geographic coordinates centered on the smart car. The coordinates are established in a way that the center of the car body is the origin, the direction of the front of the car is the positive half axis of the y axis, and the x axis is perpendicular to the y axis at the origin;

[0018] When avoiding obstacles, the coordinate information of the points behind the smart car is not needed, so the points between -180° and 0° can be removed, simplifying the number of points to 1440.

[0019] And convert the rectangular coordinates of each coordinate point into polar coordinates

[0020] Preferably, in step S2, an angle is calculated for all the position information of the laser radar through a forward-looking algorithm, and the points near the nearest point are deleted to meet the gyroscope deflection angle to determine whether the specific position of the cone barrel will affect the passage of the smart car. If it does not affect the point detected near the cone barrel, the point detected near the cone barrel can be deleted. Since the algorithm will obtain the points that will affect the passage of the smart car later, it is necessary to make an angle in advance to perform a pre-turn operation, so that when the front of the car is about to reach the cone barrel, the direction of the front is not much deviated, and it can simply make an angle to pass, which is very forward-looking. This is also a key step in passing the cone barrel at high speed. Otherwise, if you think about how to avoid obstacles when you are about to reach the cone barrel, you will make a large turn and there is a high probability that you will hit the barrel.

[0021] The look-ahead algorithm is mathematically abstracted as follows:

[0022] dot(x,y)=argmin i∈D [(x i -x c ) 2 +(y i -y c ) 2 ]

[0023] D del ={d∈D|(x d -x dot ) 2 +(y d -y dot ) 2 <1}

[0024]

[0025] Where D represents all point sets, i.e. 1440 points; x c ,y c Indicates the vehicle coordinate position; x i ,y i Indicates the coordinate position of the i-th point in all points, i = 0 to 1440; dot(x, y) represents the point closest to the current vehicle body;

[0026] D del Indicates the set of points to be deleted; x dot ,y dot Indicates the x and y coordinates of the searched dot(x,y); x d ,y d surface

[0027] Shows the coordinates of the points around the searched dot(x,y);

[0028] θ threshold It is the threshold value that needs to be adjusted through experiments, usually 10°;

[0029] θ deflection Refers to the steering angle calculated by IMU;

[0030] *Delete D del Click until satisfied: End when , otherwise iterate;

[0031] The specific implementation steps of step S2 are:

[0032] S21: Find the nearest point in the coordinates where y>0

[0033] In the point set of all position information, calculate each point (x i -x c ) 2 +(y i -y c ) 2 value, find the minimum value, which is the nearest point, where x i ,y i Indicates the coordinate position of the i-th point in all points, x c ,y c To represent the vehicle's coordinate position, find the closest point in the first and third quadrants, and remove all points within 1m of these two points from the point set.

[0034] S22: Using the IMU inertial measurement unit (IMU inertial measurement unit can obtain x, y, z three-axis acceleration and three-axis angular velocity), the acceleration is quadratically integrated to obtain the vehicle's deflection angle. If the angle between the line connecting the nearest point and the vehicle and the extension line of the deflection angle obtained by the IMU is greater than the threshold θ threshold , then go to step S23; if not, then re-enter step S21 to delete points, that is, recalculate the remaining points in the point set, find the nearest point among the remaining points, and delete all points within 1m of the nearest point until the angle between the line connecting the nearest point and the vehicle body and the deflection angle obtained by the IMU is greater than the threshold θ threshold ;

[0035] S23: After step S22, the two nearest points of all points are the positions of the two barrels that the smart car will collide with significantly after a period of time under the current vehicle body deflection angle. It is judged in advance and small angle adjustments are made in advance to ensure that the S-turn is not too extreme and can pass very smoothly.

[0036] The solution for small angle adjustment is to calculate the vector sum of the line connecting the two nearest points and the vehicle body. The angle of the straight line generated by the vector sum is the look-ahead angle.

[0037] Preferably, θ thresholdIt cannot be too large, otherwise the intelligent look-ahead position will become very far away and meaningless; nor can it be too small, otherwise it will end at the beginning and there will be no look-ahead effect. threshold Preferably 10°, usually θ threshold Good foresight, θ threshold The small forward-looking effect is weak, and θ can be fine-tuned according to the map size. threshold .

[0038] Preferably, in step S3, all the position information of the LiDAR is used to calculate an angle using a grid dynamic occupancy algorithm. This calculation method is the best way to determine the steering angle for passing through surrounding points, ensuring that the vehicle does not collide with nearby cones. Because the grid is divided into multiple parts, the grids with points are blacked out, leaving only the empty grids, and then the remaining empty grids are merged, and the grid with the largest area is used for angle calculation. The entire process is dynamic and real-time, and has been experimentally verified to have extremely high stability. It is very smooth for passing nearby cones at high speeds, making it an innovative high-speed obstacle avoidance solution.

[0039] The grid dynamic occupation algorithm is mathematically abstracted as follows:

[0040]

[0041]

[0042]

[0043]

[0044] mergeθ i and find max(θ)'s middle line*

[0045] Among them, dot left Refers to the leftmost point, that is, the position of the point where θ is the smallest;

[0046] dot right Refers to the rightmost point, that is, the position of the point with the largest θ;

[0047] x right ,y right dot right The x and y coordinates of

[0048] x left ,y left dot left The x and y coordinates of

[0049] θ i is the angle generated by iteration, which refers to the angular position of each grid;

[0050] *Merge θ i : The adjacent θ without points i Regions merged together;

[0051] *Find the largest θ and get the midline position of this θ;

[0052] The specific implementation steps of step S3 are:

[0053] S31: In the rectangular coordinate system, retain the points where y > 0, that is, the points in front of the vehicle. Find the two points with the smallest angles in the polar coordinate system, namely, the points with the smallest and largest θ in the polar coordinate system. Use the points with the smallest angles on the left and right sides in front of the smart vehicle as reference points, and divide all points within this range into multiple grids.

[0054] S32: Calculate the number of points occupied by each of the multiple grids, remove the grids with points, find the empty grids in the multiple grids, merge the adjacent empty grids, search for the largest empty grid, find the largest grid after the synthesis to occupy the grid, take the center line of the largest grid, and the angle of the center line is the grid dynamic occupation angle calculated by the grid dynamic occupation algorithm.

[0055] Preferably, in step S31, the number of grid shares needs to be adjusted accordingly for different track conditions. Generally speaking, the more grid shares there are, the greater the vibration of the vehicle body, but the stability is good and it will not hit the barrel. The fewer grid shares there are, the less vibration the vehicle body is, and it will be smoother to pass obstacles. In the present invention, the grid score is preferably 10.

[0056] Preferably, step S4 is specifically implemented as follows:

[0057] S41: Only the points with y>0 are retained in the rectangular coordinate system, and these points are clustered using the k-means or DBSCAN clustering algorithm, and the cluster centers are output;

[0058] S42: Take the two closest points after clustering and the connecting line of the smart car center point (ie, the origin) and perform vector superposition to obtain the clustering angle.

[0059] Preferably, within the range of the laser radar, approximately 2-6 cones can be detected. If only two clusters are clustered, then once the radar captures 6 cones, the motion effect will be very poor. Therefore, generally, four or more clusters are used. In the host computer, we found that there is little difference between 4 and 6 categories, so 4 categories can be used in the present invention.

[0060] Preferably, in step S41, the K-means++ clustering algorithm is selected for clustering, and the clustering process is:

[0061] S411: Automatically determine the number of point cloud clusters k by the elbow method

[0062] Define the classification error function as E, that is, the error square sum function:

[0063]

[0064] Among them, z is the input sample set, z={z1,z2,z3,...z m}; k is the number of clusters; C j is the output point cloud cluster, j is an integer from 1 to k, μ j is the initial cluster center, where j is an integer from 1 to k, that is, {μ1,μ2,μ3,...μ k};

[0065] As the number of cluster categories increases during machine learning, the decrease in E will decrease sharply, and then tend to be flat as the number of clusters k continues to increase. The elbow method requires selecting the inflection point, which corresponds to the optimal k value.

[0066] Assuming that the number of real point cloud clusters is J, when k is less than the sample J, each unit increase in k will greatly increase the degree of aggregation of each cluster, and the decrease in E will be very large; when k is close to J, the return of the degree of aggregation obtained by increasing k will quickly decrease, and the decrease in E will also decrease; as k continues to increase, the change in E will tend to be gentle, so the inflection point corresponds to the optimal k value.

[0067] The process of obtaining the optimal k value is:

[0068] After calculating the E corresponding to different k values, the E values ​​of adjacent k values ​​are connected with straight lines and plotted. The point where the slope suddenly changes is considered to be the inflection point, which corresponds to the optimal k value.

[0069] Initialize the k value from 4 to start E calculation, set the upper limit to 10, find the inflection point within this interval, and determine the optimal number of clusters by the k value corresponding to the inflection point.

[0070] S412: Initialize the cluster centers using K-means++. The initialization of the cluster centers of the unsupervised clustering algorithm will directly affect the effect of subsequent clustering. Therefore, it is necessary to initialize reasonable cluster points and make the cluster points as far apart as possible to get as close to the accurate situation as possible. Use K-means++initial to initialize. This method has little response to noise data and will not select mutation points or abnormal points. Specifically:

[0071] Randomly determine the first cluster center, then traverse each point that has not become a cluster center and calculate the closest distance to all the current cluster centers as a distance feature of the point; after traversal, compare the distance features of all points that have not become cluster centers, and select the point with the largest distance feature as the next cluster center. In this way, the uniformity of the distribution of cluster centers is maximized.

[0072] Determine whether the distance between cluster centers is greater than the nearest distance threshold q. If it is greater than q, the initialization is valid, otherwise it will be reinitialized.

[0073] The threshold q is generally one-third of the maximum distance between any two points in the point set obtained by the laser radar, and can also be flexibly selected according to actual conditions.

[0074] S413: Initialize different labels for point cloud data

[0075] The point cloud cluster C j Initialize to an empty set and calculate μ of sample x1 and each cluster center j Distance between: d ij =||x i -μ j || 2 , determine x based on the calculated minimum distance i The corresponding point cloud cluster, that is, the sample x i Classified as d ij The smallest point cloud cluster is labeled;

[0076] S414: Update new cluster center

[0077] Update the cluster center formula: After marking all sample points, recalculate the cluster center for j=1,2,...k:

[0078]

[0079] S415: Relabel point cloud data

[0080] Based on the new cluster center obtained in step S414, step S413 is repeated to relabel the point cloud data;

[0081] Steps S414 and S415 are iteratively performed, i.e., the process of updating the new cluster center and labeling is performed alternately, continuously approaching the true value;

[0082] In order to save hardware resources, a maximum number of iterations β is set, which is 200 times.

[0083] When either the update stop condition or the maximum number of iterations β is met, clustering stops. At this time, the output point cloud cluster center is the center of the cone obstacle.

[0084] Preferably, in step S5, α1=0.5, α2=0.7, and α3=0.3. Adaptive adjustments can be made on different tracks. Generally speaking, the look-ahead angle is used for advance prediction, and the angle adjustment has little effect on nearby points, so it should account for a larger proportion. The grid dynamic position angle is the optimal angle for passing through nearby points, so it should account for the largest proportion, and the cluster point is only a supplement for passing through the nearest point at the limit, so it can account for a smaller proportion. Under this distribution method, it is essentially a neutralization algorithm for far points and near points. The smart car can pass through nearby points at high speed, and can also use the steering gear in advance to straighten the car body towards the farther point. It can also make an emergency obstacle avoidance when it is about to hit the nearest point.

[0085] Preferably, in step S6, fuzzy PID controls the servo angle output, which is more effective than ordinary PID and has a faster convergence speed. It can make the calculated angle converge optimally, reduce the requirements for parameter adjustment, stabilize the angle output, and complete the corresponding control. In the control system, fuzzy control can represent non-quantitative values ​​with fuzzy sets, put these fuzzy rules and related information as knowledge into the computer knowledge base, and achieve the best adjustment of parameters through fuzzy reasoning. In fuzzy PID control, the input is the deviation e and the deviation change rate e. c , the output is ΔK p , ΔK i , ΔK d There is a fuzzy relationship between the input and output. The deviation and the rate of change of the deviation will continuously change during operation, and the fuzzy relationship is used to adjust the three parameters in real time. Fuzzy PID control is a mature existing technology and is easily applied in this invention.

[0086] Preferably, in step S7, the steering value of the servo is finally output by converting the angle and the PWM level signal. Generally, a linear conversion is adopted, that is, the servo angle of 0° corresponds to a PWM level value of 750, the maximum angle of 60° on the right side of the servo corresponds to a PWM level value of 650, and the maximum angle of -60° on the left side corresponds to a PWM level value of 850. In this way, the division can be performed, that is, Can be converted accordingly.

[0087] Where the present invention is not exhaustive, please refer to the prior art.

[0088] The beneficial effects of the present invention are:

[0089] 1. The look-ahead algorithm ensures that when passing through the current cone, it can predict the cone behind it, allowing for a small turn. This not only allows the vehicle to pass through the current cone, but also ensures that the vehicle's nose remains straight and facing the centerline of the two cones when it reaches the next cone, facilitating subsequent motion control. Otherwise, without look-ahead, even if the vehicle passes through the current cone, the vehicle's deviation angle will be significantly off. When facing the next cone, the vehicle's nose will deviate significantly, making it difficult to pass even with extreme servo angle adjustment.

[0090] 2. The grid dynamic positioning algorithm can appropriately pass through the current nearby cones. Compared with the general method of simply finding a point to activate the servo, the grid dynamic positioning algorithm has been proven to be more stable and will not cause extreme vibrations. When the vehicle body is not significantly deviated, it can smoothly pass through the nearby cones.

[0091] 3. The clustering algorithm can be used to supplement the vehicle so that it will not hit the nearest point under extreme conditions, which is a guarantee of safety. In this invention, the forward-looking algorithm and the grid dynamic occupancy algorithm are the main ones, and the clustering angle plays a supplementary role.

[0092] The method of the present invention is concise and efficient, can adapt to the use of cone information in different track scenes, outputs the most suitable steering gear angle at the moment, and has strong stability. BRIEF DESCRIPTION OF THE DRAWINGS

[0093] The drawings in the specification, which constitute a part of this application, are used to provide further understanding of this application. The illustrative embodiments of this application and their descriptions are used to explain this application and do not constitute improper limitations on this application.

[0094] Figure 1 This is a flow chart of the forward-looking cone bucket obstacle avoidance method based on grid dynamic occupancy of the present invention;

[0095] Figure 2 This is an example of a situation in which a smart car passes through S in a certain embodiment;

[0096] Figure 3 The effect of generating four types of cluster points by the clustering method in a certain embodiment;

[0097] Figure 4 is the angle obtained by clustering in a certain embodiment;

[0098] Figure 5 Schematic diagram of the effect of the look-ahead algorithm in a certain embodiment, where F is the point after the look-ahead superposition;

[0099] Figure 6 A grid schematic diagram is introduced for a grid dynamic occupation algorithm in a certain embodiment;

[0100] Figure 7The intention of removing grid placeholders in a grid dynamic placeholder algorithm in a certain embodiment;

[0101] Figure 8 A schematic diagram of the output result of the angle obtained after the grid dynamic occupation algorithm is occupied in a certain embodiment;

[0102] Figure 9 This is a schematic diagram of the final effect after the three vectors are superimposed;

[0103] Figure 10 Schematic diagram of cone barrel specifications, where (a) is specification one and (b) is specification two;

[0104] Figure 11 A schematic diagram of a bend. DETAILED DESCRIPTION

[0105] In order to enable people in this technical field to better understand the technical solutions in this specification, the technical solutions in the embodiments of the present invention are clearly and completely described below in conjunction with the drawings in the implementation of this specification, but are not limited to this. Anything not fully described in the present invention shall be based on the conventional technology in this field.

[0106] Example 1

[0107] A forward-looking cone-barrel obstacle avoidance method based on grid dynamic occupancy, such as Figure 1 As shown, the following steps are included:

[0108] S1: The laser radar used obtains the location information of 2880 points around it;

[0109] S2: Obtain the forward-looking angle by using the forward-looking algorithm on the obtained position information;

[0110] S3: Calculate the grid dynamic occupation angle using the grid dynamic occupation algorithm based on the obtained position information;

[0111] S4: Extract the position information of the lidar through the clustering algorithm to obtain the clustering angle;

[0112] S5: multiply the forward-looking angle obtained in step S2 by α1, the grid dynamic occupation angle obtained in step S3 by α2, and the cluster angle obtained in step S4 by α3, and then superimpose them to obtain the final angle. In this embodiment, α1 = 0.5, α2 = 0.7, and α3 = 0.3;

[0113] S6: Perform fuzzy PID control on the final angle to output the servo angle;

[0114] S7: The PWM level value of the servo steering is finally output through the conversion of the output servo angle and the PWM level signal.

[0115] The present invention performs corresponding cone bucket obstacle avoidance based on the forward-looking algorithm of grid dynamic position, which includes two efficient algorithms: the grid dynamic position algorithm and the forward-looking algorithm, and combines the clustering algorithm, which is used to obtain the angle that the servo should output at this moment according to the position of the cone bucket extracted by the laser radar on the track.

[0116] Example 2

[0117] A forward-looking cone-barrel obstacle avoidance method based on grid dynamic occupancy is as described in Example 1, except that in step S1, the laser radar adopts a TOF ranging series, supports 360° omnidirectional scanning, and the radar angular resolution is 0.125°. Therefore, the position information of 2880 points around the laser radar sensor can be extracted. The laser radar return data includes echo intensity, return number (recording information such as the number of times the laser beam echoes), and each point contains x, y, and z coordinate values, that is, the real geographic coordinates centered on the smart car. The coordinates are established with the center of the vehicle as the origin, the direction of the vehicle's front as the positive half axis of the y-axis, and the x-axis perpendicular to the y-axis at the origin;

[0118] When avoiding obstacles, the coordinate information of the points behind the smart car is not needed, so the points between -180° and 0° can be removed, simplifying the number of points to 1440.

[0119] And convert the rectangular coordinates of each coordinate point into polar coordinates

[0120] Example 3

[0121] A forward-looking cone barrel obstacle avoidance method based on grid dynamic occupancy is as described in Example 2, except that in step S2, an angle is calculated for all the position information of the laser radar through a forward-looking algorithm, and the nearby points of the nearest point are deleted to satisfy the gyroscope deflection angle to determine whether the specific position of the cone barrel will affect the passage of the smart car. If it does not affect it, the points detected near the cone barrel can be deleted. Since the algorithm will obtain points that will affect the passage of the smart car later, it is necessary to make an angle in advance to perform a pre-turn operation, so that when the front of the car is about to reach the cone barrel, the direction of the car head is not much deviated, and it can simply make an angle to pass through, which is very forward-looking. This is also a key step in passing the cone barrel at high speed. Otherwise, if you think about how to avoid obstacles when you are about to reach the cone barrel, there will be a large angle, and there is a high probability of hitting the barrel.

[0122] The look-ahead algorithm is mathematically abstracted as follows:

[0123] dot(x,y)=argmin i∈D [(x i -x c ) 2 +(y i -yc ) 2 ]

[0124] D del ={d∈D|(x d -x dot ) 2 +(y d -y dot ) 2 <1}

[0125]

[0126] Where D represents all point sets, i.e. 1440 points; x c ,y c Indicates the vehicle coordinate position; x i ,y i Indicates the coordinate position of the i-th point in all points, i = 0 to 1440; dot(x, y) represents the point closest to the current vehicle body;

[0127] D del Indicates the set of points to be deleted; x dot ,y dot Indicates the x and y coordinates of the searched dot(x,y); x d ,y d surface

[0128] Shows the coordinates of the points around the searched dot(x,y);

[0129] θ threshold It is the threshold value that needs to be adjusted through experiments, usually 10°;

[0130] θ deflection Refers to the steering angle calculated by IMU;

[0131] *Delete D del Click until satisfied: End when , otherwise iterate;

[0132] The specific implementation steps of step S2 are:

[0133] S21: Find the nearest point in the coordinates where y>0

[0134] In the point set of all position information, calculate each point (x i -x c ) 2 +(y i -y c ) 2 value, find the minimum value, which is the nearest point, where x i ,y iIndicates the coordinate position of the i-th point in all points, x c ,y c To represent the vehicle's coordinate position, find the closest point in the first and third quadrants, and remove all points within 1m of these two points from the point set.

[0135] S22: Using the IMU inertial measurement unit (IMU inertial measurement unit can obtain x, y, z three-axis acceleration and three-axis angular velocity), the acceleration is quadratically integrated to obtain the vehicle's deflection angle. If the angle between the line connecting the nearest point and the vehicle and the extension line of the deflection angle obtained by the IMU is greater than the threshold θ threshold , then go to step S23; if not, then re-enter step S21 to delete points, that is, recalculate the remaining points in the point set, find the nearest point among the remaining points, and delete all points within 1m of the nearest point until the angle between the line connecting the nearest point and the vehicle body and the deflection angle obtained by the IMU is greater than the threshold θ threshold ,θ threshold is 10°;

[0136] S23: After step S22, the two nearest points of all points are the positions of the two barrels that the smart car will collide with significantly after a period of time under the current vehicle body deflection angle. It is judged in advance and small angle adjustments are made in advance to ensure that the S-turn is not too extreme and can pass very smoothly.

[0137] The solution for small angle adjustment is to calculate the vector sum of the line connecting the two nearest points and the vehicle body. The angle of the straight line generated by the vector sum is the look-ahead angle.

[0138] Figure 5 The schematic diagram of the forward-looking algorithm effect is shown in Figure 1, where F is the point after the forward superposition of the vector quadrilateral. Figure 4 , u is the vector formed by the forward-looking method, and the angle between the most coordinate axis of the vector is the obtained angle value;

[0139] Example 4

[0140] A forward-looking cone-avoidance method based on grid dynamic occupancy, as described in Example 3, differs in that in step S3, all position information of the lidar is calculated using a grid dynamic occupancy algorithm to form an angle. This calculation method is the optimal method for determining the steering angle through surrounding points, ensuring that the vehicle does not collide with nearby cones. Because the grid is divided into multiple parts, the grids with points are blacked out, leaving only the empty grids, and then the remaining empty grids are merged, and the grid with the largest area is used for angle calculation. The entire process is dynamic and real-time, and has been experimentally verified to have extremely high stability. It is very smooth for passing nearby cones at high speeds, making it an innovative high-speed obstacle avoidance method.

[0141] The grid dynamic occupation algorithm is mathematically abstracted as follows:

[0142]

[0143]

[0144]

[0145]

[0146] mergeθ i and find max(θ)'s middle line*

[0147] Among them, dot left Refers to the leftmost point, that is, the position of the point where θ is the smallest;

[0148] dot right Refers to the rightmost point, that is, the position of the point with the largest θ;

[0149] x right ,y right dot right The x and y coordinates of

[0150] x left ,y left dot left The x and y coordinates of

[0151] θ i is the angle generated by iteration, which refers to the angular position of each grid;

[0152] *Merge θ i : The adjacent θ without points i Regions merged together;

[0153] *Find the largest θ and get the midline position of this θ;

[0154] The specific implementation steps of step S3 are:

[0155] S31: In the rectangular coordinate system, keep the point where y>0, that is, the point in front of the vehicle body. Find the two points with the smallest angles in the polar coordinate system, that is, the point with the smallest θ and the point with the largest θ in the polar coordinate system. Use the points with the smallest angles on the left and right sides in front of the smart car as reference points. Divide all points in this range into 10 grids, such as Figure 6 As shown;

[0156] Figure 2This is an example of a smart car passing through S, where the black dot is the origin, min-left refers to the leftmost point, i.e., the point where θ is the smallest; min-right refers to the rightmost point, i.e., the point where θ is the largest.

[0157] S32: Calculate the number of points occupied by each of the 10 grids, remove the grids with points, find the empty grids in multiple grids, merge the adjacent empty grids, search for the largest empty grid, and find the largest grid after the synthesis to occupy the space. Figure 7 , take the center line of the largest grid, the angle of the center line is the grid dynamic occupation angle calculated by the grid dynamic occupation algorithm, such as Figure 8 As shown, u is the largest grid formed by the merger, and its midline result is the vector passing through the cone barrel at this time.

[0158] Example 5

[0159] A forward-looking cone-barrel obstacle avoidance method based on grid dynamic occupancy is as described in Example 4, except that step S4 is specifically implemented as follows:

[0160] S41: Only the points with y>0 are retained in the rectangular coordinate system, and these points are clustered using the k-means or DBSCAN clustering algorithm, and the cluster centers are output;

[0161] S42: Take the two closest points after clustering and the connecting line of the smart car center point (ie, the origin) and perform vector superposition to obtain the clustering angle.

[0162] Example 6

[0163] A forward-looking cone obstacle avoidance method based on grid dynamic occupancy is described in Example 5. However, within the range of the laser radar, approximately 2-6 cones can be detected. If only two groups are clustered, the motion effect will be very poor if the radar captures six cones. Therefore, four or more groups are generally used. In the host computer, we found that there is little difference between four and six groups. Therefore, when selecting the traditional k-means or DBSCAN clustering algorithm in S41, clustering into four groups is preferred.

[0164] Figure 3 The clustering method generates four types of cluster points, with the largest point being the origin;

[0165] Figure 4 For Figure 3 The clustering result is , where A and B are the two closest points in the cluster, and C and D are the two distant points. Vector w is formed by superimposing vector u and vector v into a quadrilateral, the vector quadrilateral is uvfg, and point R is the coordinate formed by the superposition.

[0166] Example 7

[0167] A forward-looking cone-barrel obstacle avoidance method based on grid dynamic occupancy is as described in Example 5, except that the K-means++ clustering algorithm is selected for clustering in step S41. The clustering process is as follows:

[0168] S411: Automatically determine the number of point cloud clusters k by the elbow method

[0169] Define the classification error function as E, that is, the error square sum function:

[0170]

[0171] Among them, z is the input sample set, z={z1,z2,z3,...z m}; k is the number of clusters; C j is the output point cloud cluster, j is an integer from 1 to k, μ j is the initial cluster center, where j is an integer from 1 to k, that is, {μ1,μ2,μ3,...μ k};

[0172] As the number of cluster categories increases during machine learning, the decrease in E will decrease sharply, and then tend to be flat as the number of clusters k continues to increase. The elbow method requires selecting the inflection point, which corresponds to the optimal k value.

[0173] Assuming that the number of real point cloud clusters is J, when k is less than the sample J, each unit increase in k will greatly increase the degree of aggregation of each cluster, and the decrease in E will be very large; when k is close to J, the return of the degree of aggregation obtained by increasing k will quickly decrease, and the decrease in E will also decrease; as k continues to increase, the change in E will tend to be gentle, so the inflection point corresponds to the optimal k value.

[0174] S412: Initialize the cluster centers using K-means++. The initialization of the cluster centers of the unsupervised clustering algorithm will directly affect the effect of subsequent clustering. Therefore, it is necessary to initialize reasonable cluster points and make the cluster points as far apart as possible to get as close to the accurate situation as possible. Use K-means++initial to initialize. This method has little response to noise data and will not select mutation points or abnormal points. Specifically:

[0175] Randomly determine the first cluster center, then traverse each point that has not become a cluster center and calculate the closest distance to all the current cluster centers as a distance feature of the point; after traversal, compare the distance features of all points that have not become cluster centers, and select the point with the largest distance feature as the next cluster center. In this way, the uniformity of the distribution of cluster centers is maximized.

[0176] Determine whether the distance between cluster centers is greater than the nearest distance threshold q. If it is greater than q, the initialization is valid, otherwise it will be reinitialized.

[0177] The threshold q is generally one-third of the maximum distance between any two points in the point set obtained by the laser radar, and can also be flexibly selected according to actual conditions.

[0178] S413: Initialize different labels for point cloud data

[0179] The point cloud cluster C j Initialize to an empty set and calculate μ of sample x1 and each cluster center j Distance between: d ij =||x i -μ j || 2 , determine x based on the calculated minimum distance i The corresponding point cloud cluster, that is, the sample x i Classified as d ij The smallest point cloud cluster is labeled;

[0180] S414: Update new cluster center

[0181] Update the cluster center formula: After marking all sample points, recalculate the cluster center for j=1,2,...k:

[0182]

[0183] S415: Relabel point cloud data

[0184] Based on the new cluster center obtained in step S414, step S413 is repeated to relabel the point cloud data;

[0185] Steps S414 and S415 are iteratively performed, i.e., the process of updating the new cluster center and labeling is performed alternately, continuously approaching the true value;

[0186] In order to save hardware resources, a maximum number of iterations β is set, which is 200 times.

[0187] When either the update stop condition or the maximum number of iterations β is met, clustering stops. At this time, the output point cloud cluster center is the center of the cone obstacle.

[0188] Figure 9 This is the effect of superimposing three vectors.

[0189] In terms of technical effect, the algorithm of the present invention is put into the ROS smart car test to pass the S curve, the cone barrel style is as follows Figure 10 As shown, the curve is Figure 11 As shown, Figure 9 In order to obtain the route that should be taken at this time through this algorithm, the angle of the route is calculated and output through the servo. It can be seen that Figure 9 When passing an S-curve, the car does not simply take the center line. This is because although the center line can easily pass the first S-curve, this non-angled steering will cause the front of the smart car to deviate to the left on the S-curve at high speeds. When passing the cone barrel of the second S-curve, it will become very extreme, and the gradual accumulation will cause a crash. Even at ultra-high speeds, the second S-curve will crash. In the algorithm of the present invention, it can be seen that the forward-looking algorithm through the grid dynamic position will output an angle output that deviates to the left. This can ensure that the S-curve is passed and that the front of the car is facing the correct position in the second S lane, and the S-curve turn can still be completed. This relies on the forward-looking method to see the position of the cone barrel in front and make a small-angle prediction in advance. The grid dynamic position algorithm is dominant and can pass through the nearby barrels. The clustered angle can ensure that the smart car deviates at an angle and does not hit the nearest cone barrel. Curve driving is a relatively difficult task in the current second-level driving test of motor vehicles. However, the speed PWM level of our ROS smart car can reach 2200, which is the speed limit of the ROS smart car. It can still pass the curve smoothly, proving the effectiveness and efficiency of motion control and excellent pressure resistance at high speed. Figure 11 By using the algorithm of the present invention, a lap can be completed from the starting point to the end point in 9.7 seconds without hitting the barrel.

[0190] Current ROS robots all move at low speeds because current control algorithms do not achieve high-speed obstacle avoidance. The forward-looking algorithm of the grid dynamic occupancy invented by the present invention enables such robots to pass through complex environmental obstacles at high speed with high efficiency and strong stability.

[0191] The above is a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications should also be regarded as within the scope of protection of the present invention.

Claims

1. A forward-looking cone-barrel obstacle avoidance method based on grid dynamic occupancy, characterized in that: The steps include: S1: The laser radar used obtains the location information of 2880 points around it; S2: Obtain the forward-looking angle by using the forward-looking algorithm on the obtained position information; S3: Calculate the grid dynamic occupation angle using the grid dynamic occupation algorithm based on the obtained position information; S4: Extract the position information of the lidar through the clustering algorithm to obtain the clustering angle; S5: multiplying the forward-looking angle obtained in step S2 by α1, the grid dynamic occupation angle obtained in step S3 by α2, and the cluster angle obtained in step S4 by α3, and then superimposing them to obtain the final angle; In step S1, each point in the laser radar return data contains x, y, and z coordinate values, that is, the real geographic coordinates centered on the smart car. The coordinates are established by taking the center of the car body as the origin, the direction of the car's front as the positive half of the y-axis, and the x-axis perpendicular to the y-axis at the origin; When avoiding obstacles, the coordinate information of the points behind the smart car is not needed. The points between -180° and 0° are removed, simplifying the number of points to 1440. And convert the rectangular coordinates of each coordinate point into polar coordinates The specific implementation steps of step S2 are: S21: Find the nearest point in the coordinates where y>0 In the point set of all position information, calculate each point (x i -x c ) 2 +(y i -y c ) 2 value, find the minimum value, which is the nearest point, where x i ,y i Indicates the coordinate position of the i-th point in all points, x c ,y c To represent the vehicle's coordinate position, find the closest point in the first and third quadrants, and remove all points within 1m of these two points from the point set. S22: Using the IMU inertial measurement unit, perform a quadratic integration of the acceleration to obtain the vehicle's deflection angle. If the angle between the line connecting the nearest point and the vehicle and the extended line of the deflection angle obtained by the IMU is greater than the threshold θ threshold , then go to step S23; if not, then re-enter step S21 to delete points, that is, recalculate the remaining points in the point set, find the nearest point among the remaining points, and delete all points within 1m of the nearest point until the angle between the line connecting the nearest point and the vehicle body and the deflection angle obtained by the IMU is greater than the threshold θ threshold ; S23: Calculate the vector sum of the line connecting the two nearest points and the vehicle body. The angle of the straight line generated by the vector sum is the look-ahead angle. The specific implementation steps of step S3 are: S31: In the rectangular coordinate system, retain the points where y > 0, that is, the points in front of the vehicle. Find the two points with the smallest angles in the polar coordinate system, namely, the points with the smallest and largest θ in the polar coordinate system. Use the points with the smallest angles on the left and right sides in front of the smart vehicle as reference points, and divide all points within this range into multiple grids. S32: Calculate the number of points occupied by each of the multiple grids, remove grids with points, find empty grids in the multiple grids, merge adjacent empty grids, search for the largest empty grid, find the largest grid after the synthesis, and occupy the grid. The midline of the largest grid is taken, and the angle of the midline is the grid dynamic occupation angle calculated by the grid dynamic occupation algorithm. The specific implementation steps of step S4 are: S41: Only the points with y>0 are retained in the rectangular coordinate system, and these points are clustered using the k-means or DBSCAN clustering algorithm, and the cluster centers are output; S42: Take the two closest points after clustering and perform vector superposition with the line connecting the center point of the smart car to obtain the clustering angle.

2. The forward-looking cone-barrel obstacle avoidance method based on grid dynamic occupancy according to claim 1 is characterized in that: Step S5 and subsequent steps include: S6: Perform fuzzy PID control on the final angle to output the servo angle; S7: The PWM level value of the servo steering is finally output through the conversion of the output servo angle and the PWM level signal.

3. The forward-looking cone-barrel obstacle avoidance method based on grid dynamic occupancy according to claim 2 is characterized in that: θ threshold is 10°.

4. The forward-looking cone-barrel obstacle avoidance method based on grid dynamic occupancy according to claim 3 is characterized in that: In step S31 , the grid score is 10.

5. The forward-looking cone-barrel obstacle avoidance method based on grid dynamic occupancy according to claim 4 is characterized in that: The clustering process of step S41 is: S411: Automatically determine the number of point cloud clusters k by the elbow method Define the classification error function as E, that is, the error square sum function: Among them, z is the input sample set, z={z 1, z2,z3,...z m }; k is the number of clusters; C j is the output point cloud cluster, j is an integer from 1 to k, μ j is the initial cluster center, where j is an integer from 1 to k, that is, {μ 1, μ2,μ3,...μ k }; As the number of cluster categories increases during machine learning, the decrease in E will decrease sharply, and then tend to be flat as the number of clusters k continues to increase. The elbow method requires selecting the inflection point, which corresponds to the optimal k value. S412: Initialize the cluster centers using K-means++ to keep the cluster points as far apart as possible. Specifically: Randomly determine the first cluster center, then traverse each point that has not become a cluster center and calculate the closest distance to all the current cluster centers as a distance feature of the point; after traversal, compare the distance features of all points that have not become cluster centers, and select the point with the largest distance feature as the next cluster center. In this way, the uniformity of the distribution of cluster centers is maximized. Determine whether the distance between cluster centers is greater than the nearest distance threshold q. If it is greater than q, the initialization is valid, otherwise it will be reinitialized. S43: Initialize different labels for point cloud data The point cloud cluster C j Initialize to an empty set and calculate μ of sample x1 and each cluster center j Distance between: d ij =||x i -μ j || 2 , determine x based on the calculated minimum distance i The corresponding point cloud cluster, that is, the sample x i Classified as d ij The smallest point cloud cluster is labeled; S44: Update new cluster center Update the cluster center formula: After marking all sample points, recalculate the cluster center for j=1,2,...k: S45: Relabel point cloud data Repeat step S43 based on the new cluster center obtained in step S44 to re-label the point cloud data; Steps S44 and S45 are iteratively performed, i.e., the process of updating the new cluster center and labeling is performed alternately, continuously approaching the true value; When the update stops or the maximum number of iterations β is reached, clustering stops. The output point cloud cluster center is the center of the cone obstacle. The maximum number of iterations β is 200.

6. The forward-looking cone-barrel obstacle avoidance method based on grid dynamic occupancy according to claim 5 is characterized in that: In step S5, α1 = 0.5, α2 = 0.7, and α3 = 0.3.

Images