A method and storage medium for unmanned tracking driving through an obstacle array

Abstract

The present invention discloses a method and storage medium for unmanned tracking driving through an obstacle array. The method comprises the following steps: obtaining the latitude and longitude coordinates of a vehicle's current position and the latitude and longitude coordinates of all obstacles in front of the vehicle; selecting the closest obstacles on either side of the tracking trajectory for each row of obstacles in the obstacle array in front of the unmanned vehicle, and calculating the midpoint and perpendicular bisector of the line connecting the two obstacles; calculating a circle 1 and a circle 2 using the vehicle's minimum turning radius as the radius, and calculating the obstacle-passing common tangent of the two circles; obtaining an obstacle-passing route based on the circle 1, the circle 2, and the common tangent, and performing obstacle-passing; repeating steps S2 to S4 until the obstacle array is passed; and autonomously planning an optimal route through the obstacle array when an obstacle array exists on the original tracking driving route and the distances between each obstacle in the obstacle array are sufficient for the vehicle to travel, thereby completing the predetermined unmanned tracking driving task.

Description

Technical Field

[0001] The present invention relates to the field of unmanned driving technology, and in particular to a method and storage medium for unmanned tracking driving through an obstacle array. Background Art

[0002] Unmanned tracking is a common method for autonomous driving. Tracking refers to the automatic tracking of a vehicle along a pre-set route. Tracking is performed in a variety of environments, and various obstacles may hinder normal unmanned tracking. A pressing challenge is how to quickly plan a safe and navigable path when there are arrays of obstacles along the tracking trajectory that could affect the vehicle's normal operation, but the distance between adjacent obstacles is greater than the vehicle's width. Summary of the Invention

[0003] The technical problem to be solved by the present invention is to provide a method and storage medium for unmanned tracking driving through an obstacle array, which can provide optimal route planning when it is necessary to cross an obstacle array.

[0004] In order to solve the above technical problems, the technical solution adopted by the present invention is:

[0005] A method for unmanned tracking driving through an obstacle array, comprising the steps of:

[0006] S1. Obtain the latitude and longitude coordinates of the vehicle's current position and the latitude and longitude coordinates of all obstacles in front of the vehicle;

[0007] S2. For each row of obstacles in the obstacle array in front of the autonomous vehicle, select the closest obstacles on both sides of the tracking trajectory and calculate the midpoint and perpendicular bisector of the line connecting the two obstacles;

[0008] S3. Using the vehicle's minimum turning radius as its radius, calculate a circle (a first circle) that passes through the vehicle's current position, has the vehicle's current direction as its tangent, and has its center closest to the tracking track; and a circle (a second circle) that passes through the midpoint, has the perpendicular bisector as its tangent, and has its center closest to the tracking track. Calculate the common tangent line of the two circles.

[0009] S4. Obtain an obstacle traversal route based on the first circle, the second circle, and the common tangent line, and traverse the obstacle;

[0010] S5. Repeat steps S2 to S4 until the obstacle array is passed.

[0011] In order to solve the above technical problems, another technical solution adopted by the present invention is:

[0012] A computer-readable storage medium stores a computer program, which, when executed by a processor, implements the following steps:

[0013] S1. Obtain the latitude and longitude coordinates of the vehicle's current position and the latitude and longitude coordinates of all obstacles in front of the vehicle;

[0014] S2. For each row of obstacles in the obstacle array in front of the autonomous vehicle, select the closest obstacles on both sides of the tracking trajectory and calculate the midpoint and perpendicular bisector of the line connecting the two obstacles;

[0015] S3. Using the vehicle's minimum turning radius as its radius, calculate a circle (a first circle) that passes through the vehicle's current position, has the vehicle's current direction as its tangent, and has its center closest to the tracking track; and a circle (a second circle) that passes through the midpoint, has the perpendicular bisector as its tangent, and has its center closest to the tracking track. Calculate the common tangent line of the two circles.

[0016] S4. Obtain an obstacle traversal route based on the first circle, the second circle, and the common tangent line, and traverse the obstacle;

[0017] S5. Repeat steps S2 to S4 until the obstacle array is passed.

[0018] The beneficial effects of the present invention are as follows: the method and storage medium for unmanned tracking driving through an obstacle array of the present invention can autonomously plan the optimal route through the obstacle array to complete the established unmanned tracking driving task when an obstacle array exists on the original tracking driving route and the distance between each obstacle in the obstacle array is sufficient to accommodate the vehicle's travel. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 This is a flow chart of a method for unmanned tracking driving through an obstacle array according to an embodiment of the present invention;

[0020] Figure 2 Schematic diagram of a method for unmanned tracking driving through an obstacle array according to an embodiment of the present invention. DETAILED DESCRIPTION

[0021] To illustrate the technical content, achieved objectives and effects of the present invention in detail, the following description is given in conjunction with the embodiments and accompanying drawings.

[0022] Please refer to Figure 1 as well as Figure 2 A method for unmanned tracking driving through an obstacle array comprises the steps of:

[0023] S1. Obtain the latitude and longitude coordinates of the vehicle's current position and the latitude and longitude coordinates of all obstacles in front of the vehicle;

[0024] S2. For each row of obstacles in the obstacle array in front of the autonomous vehicle, select the closest obstacles on both sides of the tracking trajectory and calculate the midpoint and perpendicular bisector of the line connecting the two obstacles;

[0025] S3. Using the vehicle's minimum turning radius as its radius, calculate a circle (a first circle) that passes through the vehicle's current position, has the vehicle's current direction as its tangent, and has its center closest to the tracking track; and a circle (a second circle) that passes through the midpoint, has the perpendicular bisector as its tangent, and has its center closest to the tracking track. Calculate the common tangent line of the two circles.

[0026] S4. Obtain an obstacle traversal route based on the first circle, the second circle, and the common tangent line, and traverse the obstacle;

[0027] S5. Repeat steps S2 to S4 until the obstacle array is passed.

[0028] As can be seen from the foregoing description, the beneficial effects of the present invention lie in the following: The method and storage medium for unmanned tracking driving through an obstacle array of the present invention can autonomously plan an optimal route through the obstacle array to complete the predetermined unmanned tracking driving task, provided that an obstacle array exists on the original tracking driving route and the distances between each obstacle in the obstacle array are sufficient for vehicle travel.

[0029] Furthermore, the calculation of the obstacle common tangent line is specifically as follows:

[0030] Four common tangent lines are calculated based on the circle 1 and the circle 2;

[0031] Calculate the line connecting the centers of circle 1 and circle 2, and get the equation of the line connecting the centers:

[0032] y=kx+b;

[0033] Eliminate the two external common tangents whose slopes are equal to the slope k of the line connecting the centers of the circles, and obtain the two internal common tangents of the two circles;

[0034] Calculate the intersection points of the two inner common tangents with circle 2 respectively, and calculate the distance between the intersection points and the midpoint of the line connecting the two obstacles, and select the inner tangent line corresponding to the intersection point that is closest to the midpoint as the obstacle-penetrating common tangent line.

[0035] As can be seen from the above description, through the above steps, a common tangent line closest to the obstacle and the current position of the vehicle can be determined, so that the subsequently determined route is optimal.

[0036] Furthermore, the step S4 is specifically as follows:

[0037] Calculate the intersection point B of the common tangent with circle 1 and the intersection point C with circle 2, and splice the line segment BC, arc AB, and arc CD into an obstacle route AD, and control the vehicle to travel along the obstacle route AD to overcome the obstacle;

[0038] The arc AB is the minor arc between the vehicle's current position A and the intersection point B on circle one, and the arc CD is the minor arc between the intersection point C and the midpoint D of the line connecting the two obstacles on circle two.

[0039] From the above description, it can be seen that circle 1 and circle 2 are circles determined based on the minimum turning radius of the vehicle. Arc AB and arc CD on them comply with the vehicle's driving rules. Arc AB, line segment BC, and arc CD can determine an optimal obstacle-traveling route that the vehicle can follow.

[0040] Furthermore, the circle one is determined as follows:

[0041] Establish a first straight line passing through the vehicle's current position and perpendicular to the vehicle's forward direction, and determine two points on the first straight line whose distance from the vehicle's current position is the minimum turning radius. Calculate the distance between the two points and the tracking trajectory, and take the point with the smallest distance (x1, y1) as the center of circle one. Take the vehicle's minimum turning radius r as the radius of circle one. According to the formula:

[0042] (x-x1) 2 +(y-y1) 2 =r 2 ;

[0043] Calculate and obtain the circle one;

[0044] The determination of the circle 2 is:

[0045] Determine two points on the line connecting the two obstacles and their extension line that are at a distance from the midpoint equal to the minimum turning radius, and calculate the distance between the two points and the tracking trajectory. Take the point with the smallest distance (x2, y2) as the center of circle 2, and use the vehicle's minimum turning radius r as the radius of circle 1. According to the formula:

[0046] (x-x2) 2 +(y-y2) 2 =r 2 ;

[0047] The circle 2 is obtained by calculation.

[0048] From the above description, we can know that we can use the above method to determine the circle formed by the minimum turning route for the vehicle to turn and get closer to the original trajectory, which is used to determine the final turning route of the vehicle to form the optimal obstacle-breaking route.

[0049] Furthermore, the step S1 specifically includes:

[0050] S11. Obtain the vehicle's current latitude and longitude coordinates in real time, collect LiDAR point cloud data in real time, identify all obstacles directly in front of the vehicle through clustering algorithms, and record the radar coordinates of each obstacle directly in front of the unmanned vehicle;

[0051] S12. According to the longitude and latitude coordinates of the current position of the vehicle, the radar coordinates are converted into longitude and latitude coordinates using a rotation matrix.

[0052] From the above description, it can be seen that the position of the obstacle relative to the vehicle is determined based on the collected lidar point cloud data, and the longitude and latitude coordinates of the obstacle are determined in combination with the current longitude and latitude coordinates of the vehicle, achieving a unified measurement effect, which is convenient for subsequent calculations.

[0053] A computer-readable storage medium stores a computer program, which, when executed by a processor, implements the following steps:

[0054] S1. Obtain the latitude and longitude coordinates of the vehicle's current position and the latitude and longitude coordinates of all obstacles in front of the vehicle;

[0055] S2. For each row of obstacles in the obstacle array in front of the autonomous vehicle, select the closest obstacles on both sides of the tracking trajectory and calculate the midpoint and perpendicular bisector of the line connecting the two obstacles;

[0056] S3. Using the vehicle's minimum turning radius as its radius, calculate a circle (a first circle) that passes through the vehicle's current position, has the vehicle's current direction as its tangent, and has its center closest to the tracking track; and a circle (a second circle) that passes through the midpoint, has the perpendicular bisector as its tangent, and has its center closest to the tracking track. Calculate the common tangent line of the two circles.

[0057] S4. Obtain an obstacle traversal route based on the first circle, the second circle, and the common tangent line, and traverse the obstacle;

[0058] S5. Repeat steps S2 to S4 until the obstacle array is passed.

[0059] As can be seen from the foregoing description, the beneficial effects of the present invention lie in the following: The method and storage medium for unmanned tracking driving through an obstacle array of the present invention can autonomously plan an optimal route through the obstacle array to complete the predetermined unmanned tracking driving task, provided that an obstacle array exists on the original tracking driving route and the distances between each obstacle in the obstacle array are sufficient for vehicle travel.

[0060] Furthermore, the calculation of the obstacle common tangent line is specifically as follows:

[0061] Four common tangent lines are calculated based on the circle 1 and the circle 2;

[0062] Calculate the line connecting the centers of circle 1 and circle 2, and get the equation of the line connecting the centers:

[0063] y=kx+b;

[0064] Eliminate the two external common tangents whose slopes are equal to the slope k of the line connecting the centers of the circles, and obtain the two internal common tangents of the two circles;

[0065] Calculate the intersection points of the two inner common tangents with circle 2 respectively, and calculate the distance between the intersection points and the midpoint of the line connecting the two obstacles, and select the inner tangent line corresponding to the intersection point that is closest to the midpoint as the obstacle-penetrating common tangent line.

[0066] As can be seen from the above description, through the above steps, a common tangent line closest to the obstacle and the current position of the vehicle can be determined, so that the subsequently determined route is optimal.

[0067] Furthermore, the step S4 is specifically as follows:

[0068] Calculate the intersection point B of the common tangent with circle 1 and the intersection point C with circle 2, and splice the line segment BC, arc AB, and arc CD into an obstacle route AD, and control the vehicle to travel along the obstacle route AD to overcome the obstacle;

[0069] The arc AB is the minor arc between the vehicle's current position A and the intersection point B on circle one, and the arc CD is the minor arc between the intersection point C and the midpoint D of the line connecting the two obstacles on circle two.

[0070] From the above description, it can be seen that circle 1 and circle 2 are circles determined based on the minimum turning radius of the vehicle. Arc AB and arc CD on them comply with the vehicle's driving rules. Arc AB, line segment BC, and arc CD can determine an optimal obstacle-traveling route that the vehicle can follow.

[0071] Furthermore, the circle one is determined as follows:

[0072] Establish a first straight line passing through the vehicle's current position and perpendicular to the vehicle's forward direction, and determine two points on the first straight line whose distance from the vehicle's current position is the minimum turning radius. Calculate the distance between the two points and the tracking trajectory, and take the point with the smallest distance (x1, y1) as the center of circle one. Take the vehicle's minimum turning radius r as the radius of circle one. According to the formula:

[0073] (x-x1) 2 +(y-y1) 2 =r 2 ;

[0074] Calculate and obtain the circle one;

[0075] The determination of the circle 2 is:

[0076] Determine two points on the line connecting the two obstacles and their extension line that are at a distance from the midpoint equal to the minimum turning radius, and calculate the distance between the two points and the tracking trajectory. Take the point with the smallest distance (x2, y2) as the center of circle 2, and use the vehicle's minimum turning radius r as the radius of circle 1. According to the formula:

[0077] (x-x2) 2 +(y-y2) 2 =r 2 ;

[0078] The circle 2 is obtained by calculation.

[0079] From the above description, we can know that we can use the above method to determine the circle formed by the minimum turning route for the vehicle to turn and get closer to the original trajectory, which is used to determine the final turning route of the vehicle to form the optimal obstacle-breaking route.

[0080] Furthermore, the step S1 specifically includes:

[0081] S11. Obtain the vehicle's current latitude and longitude coordinates in real time, collect LiDAR point cloud data in real time, identify all obstacles directly in front of the vehicle through clustering algorithms, and record the radar coordinates of each obstacle directly in front of the unmanned vehicle;

[0082] S12. According to the longitude and latitude coordinates of the current position of the vehicle, the radar coordinates are converted into longitude and latitude coordinates using a rotation matrix.

[0083] From the above description, it can be seen that the position of the obstacle relative to the vehicle is determined based on the collected lidar point cloud data, and the longitude and latitude coordinates of the obstacle are determined in combination with the current longitude and latitude coordinates of the vehicle, achieving a unified measurement effect, which is convenient for subsequent calculations.

[0084] The present invention provides a method and storage medium for unmanned tracking driving through an obstacle array, which is applicable to scenarios where an obstacle array exists on the original tracking driving route and the obstacle array needs to be traversed, and the distance between each obstacle in the obstacle array is large enough to accommodate the vehicle's travel.

[0085] Please refer to Figure 1 and Figure 2 , embodiment 1 of the present invention is:

[0086] A method for unmanned tracking driving through an obstacle array, comprising the steps of:

[0087] In this embodiment, the vehicle is driven once on the route to be tracked in advance, and the GPS accuracy and latitude data (longitude, latitude) on the route are collected and stored in a file.

[0088] For example, consider placing an array of obstacles on the tracking route, forming a row perpendicular to the tracking route. The spacing between obstacles in the same row is at least greater than the vehicle width, ensuring the autonomous vehicle can pass between two adjacent obstacles. The spacing between the front and rear rows is at least twice the vehicle length, ensuring the autonomous vehicle can plan a path through the obstacle array.

[0089] The autonomous vehicle then departs from its starting point on the pre-collected tracking route and begins its unmanned tracking driving mission. The unmanned tracking algorithm obtains the vehicle's current location (latitude and longitude) from real-time GPS data. It then selects a point 5 meters ahead of the vehicle from the pre-collected tracking route as the target point for tracking. Using trigonometric functions, it calculates the direction of the line between the two points in the GPS coordinate system. This is then subtracted from the vehicle's current heading angle to determine the desired rotation angle. The algorithm then controls the vehicle's rotation direction, enabling unmanned tracking driving.

[0090] S1. Obtain the latitude and longitude coordinates of the vehicle's current position and the latitude and longitude coordinates of all obstacles in front of the vehicle;

[0091] The step S1 specifically includes:

[0092] S11. Obtain the vehicle's current latitude and longitude coordinates in real time, collect LiDAR point cloud data in real time, identify all obstacles directly in front of the vehicle through clustering algorithms, and record the radar coordinates of each obstacle directly in front of the unmanned vehicle;

[0093] S12. According to the longitude and latitude coordinates of the current position of the vehicle, the radar coordinates are converted into longitude and latitude coordinates using a rotation matrix.

[0094] In this embodiment, the vehicle collects lidar point cloud data in real time during tracking. Using a clustering algorithm, it calculates all obstacles in the array of obstacles identified directly in front of the unmanned vehicle. The radar coordinates (x, y) of each obstacle directly in front of the unmanned vehicle are recorded, and the radar coordinates are converted to longitude and latitude coordinates using a rotation matrix. The conversion method is as follows: Taking GPS data at the same moment, including longitude lat, latitude lon, and heading angle θ, and using the rotation matrix x' = xcosθ - ysinθ and y' = xsinθ + ycosθ, the longitude and latitude of the obstacle are calculated: lon_x = lon + (x' * 0.00000899281) and lat_y = lat + (y' * 0.00000899281).

[0095] In this embodiment, lidar obstacle identification utilizes a Euclidean clustering algorithm to traverse the data of all points in the lidar point cloud and calculate the distance between them. Points are considered to belong to a single object if the distance is less than a first distance. The minimum and maximum x values ​​of all points in the clustered point cloud are used to determine the obstacle's width and distance from the vehicle, while the minimum and maximum y values ​​are used to determine the obstacle's width. This 3D information is then transmitted to the obstacle traversal module, which compares the real-time obstacle array information with the trajectory of the vehicle's forward path. If the trajectory is within the obstacle array and the distance from the vehicle is less than a second distance, the obstacle traversal algorithm is run to calculate the obstacle traversal path. In this embodiment, the first distance is 0.3m and the second distance is 10m. In other equivalent embodiments, the first and second distances can be adjusted based on actual needs or conditions.

[0096] S2. For each row of obstacles in the obstacle array in front of the unmanned vehicle, select the closest obstacles on both sides of the tracking trajectory and calculate the midpoint and perpendicular bisector of the line connecting the two obstacles.

[0097] In this embodiment, the first row of obstacles is determined based on the distance between the unmanned vehicle and the obstacle. The distance between each obstacle in the first row and the tracking track is calculated to obtain the closest obstacles on both sides of the tracking track. The center point and perpendicular median of the line connecting the two obstacles are further calculated.

[0098] S3. Using the vehicle's minimum turning radius as its radius, calculate a circle (a first circle) that passes through the vehicle's current position, has the vehicle's current direction as its tangent, and has its center closest to the tracking track; and a circle (a second circle) that passes through the midpoint, has the perpendicular bisector as its tangent, and has its center closest to the tracking track. Calculate the common tangent line of the two circles.

[0099] The circle one is determined as follows:

[0100] Establish a first straight line passing through the vehicle's current position and perpendicular to the vehicle's forward direction, and determine two points on the first straight line whose distance from the vehicle's current position is the minimum turning radius. Calculate the distance between the two points and the tracking trajectory, and take the point with the smallest distance (x1, y1) as the center of circle one. Take the vehicle's minimum turning radius r as the radius of circle one. According to the formula:

[0101] (x-x1) 2 +(y-y1) 2 =r 2 ;

[0102] Calculate and obtain the circle one;

[0103] The determination of the circle 2 is:

[0104] Determine two points on the line connecting the two obstacles and their extension line that are at a distance from the midpoint equal to the minimum turning radius, and calculate the distance between the two points and the tracking trajectory. Take the point with the smallest distance (x2, y2) as the center of circle 2, and use the vehicle's minimum turning radius r as the radius of circle 1. According to the formula:

[0105] (x-x2) 2 +(y-y2) 2 =r 2 ;

[0106] The circle 2 is obtained by calculation.

[0107] In this embodiment, through the above method, it is possible to calculate a circle one passing through the current position of the unmanned vehicle with the minimum turning radius of the unmanned vehicle as the radius r, with the unmanned vehicle's forward direction as the tangent, and with the center of the circle closest to the original trajectory, and a circle two passing through the midpoint of the obstacle target line with the perpendicular bisector of the line as the tangent, and with the center of the circle closest to the original trajectory.

[0108] Among them, if the unmanned vehicle is located on the tracking trajectory when the circle one is determined, then it is determined whether the midpoint is relatively located in the left front or the right front of the unmanned vehicle. If it is in the left front, a point r away from the current position of the vehicle is selected from the left as the center of circle one. Otherwise, a point r away from the current position of the vehicle is selected from the right as the center of circle one.

[0109] The calculation of the obstacle common tangent is specifically as follows:

[0110] Four common tangent lines are calculated based on the circle 1 and the circle 2;

[0111] Calculate the line connecting the centers of circle 1 and circle 2, and get the equation of the line connecting the centers:

[0112] y=kx+b;

[0113] Eliminate the two external common tangents whose slopes are equal to the slope k of the line connecting the centers of the circles, and obtain the two internal common tangents of the two circles;

[0114] Calculate the intersection points of the two inner common tangents with circle 2 respectively, and calculate the distance between the intersection points and the midpoint of the line connecting the two obstacles, and select the inner tangent line corresponding to the intersection point that is closest to the midpoint as the obstacle-penetrating common tangent line.

[0115] In this example, we need to solve the common tangent of the two circles. Assume that the equation of the common tangent of the two circles is y i =k i x+b i , the distances from the centers of the two circles to the common tangent lines are equal, so we can solve for the four common tangent lines.

[0116] Solve for the line formed by the centers of the two circles. Since the radii of the two circles are equal, the external common tangent is parallel to the line formed by the centers of the two circles, and the slopes of the line equations are equal. Therefore, the two external common tangents can be eliminated and the two internal common tangents can be determined.

[0117] Solve the two inner tangents and the two corner points of circle 2 respectively, and calculate the distance between the intersection and the midpoint of the line connecting the two obstacles mentioned above. Select the inner tangent corresponding to the closest intersection as the obstacle-penetrating tangent.

[0118] S4. Obtain an obstacle traversal route based on the first circle, the second circle, and the common tangent line, and traverse the obstacle;

[0119] The step S4 is specifically as follows:

[0120] Calculate the intersection point B of the common tangent with circle 1 and the intersection point C with circle 2, and splice the line segment BC, arc AB, and arc CD into an obstacle route AD, and control the vehicle to travel along the obstacle route AD to overcome the obstacle;

[0121] The arc AB is the minor arc between the vehicle's current position A and the intersection point B on circle one, and the arc CD is the minor arc between the intersection point C and the midpoint D of the line connecting the two obstacles on circle two.

[0122] In this embodiment, the intersection point B of the common tangent line with circle one and the intersection point C with circle two are calculated. Based on the current position A of the vehicle and the midpoint D of the line connecting the two obstacles, the shortest obstacle-passing route is formed by arc AB, line segment BC, and arc CD. Track points on the obstacle-passing route are generated at intervals of 20 cm, and obstacles are passed along these track points.

[0123] S5. Repeat steps S2 to S4 until the obstacle array is passed.

[0124] In this embodiment, the above steps are executed in a loop to complete the traversal of the obstacle array.

[0125] The second embodiment of the present invention is:

[0126] A computer-readable storage medium stores a computer program, which implements the steps in the above embodiment 1 when executed by a processor.

[0127] In summary, the present invention provides a method for unmanned tracking driving through an obstacle array. When an obstacle array exists on the original tracking driving route and the distances between obstacles in the obstacle array are sufficient to accommodate vehicle travel, the method can autonomously plan the optimal route through the obstacle array to complete the established unmanned tracking driving task.

[0128] The above descriptions are merely embodiments of the present invention and are not intended to limit the patent scope of the present invention. Any equivalent transformations made using the contents of the present invention's description and drawings, or directly or indirectly applied in related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A method for unmanned tracking driving through an obstacle array, characterized in that: Including steps: S1. Obtain the latitude and longitude coordinates of the vehicle's current position and the latitude and longitude coordinates of all obstacles in front of the vehicle; S2. For each row of obstacles in the obstacle array in front of the autonomous vehicle, select the closest obstacles on both sides of the tracking trajectory and calculate the midpoint and perpendicular bisector of the line connecting the two obstacles; S3. Using the vehicle's minimum turning radius as its radius, calculate a circle (a first circle) that passes through the vehicle's current position, has the vehicle's current direction as its tangent, and has its center closest to the tracking track; and a circle (a second circle) that passes through the midpoint, has the perpendicular bisector as its tangent, and has its center closest to the tracking track. Calculate the common tangent line of the two circles. The circle one is determined as follows: Establish a first straight line passing through the vehicle's current position and perpendicular to the vehicle's forward direction, and determine two points on the first straight line whose distance from the vehicle's current position is the minimum turning radius. Calculate the distance between the two points and the tracking trajectory, and take the point with the smallest distance (x1, y1) as the center of circle one. Take the vehicle's minimum turning radius r as the radius of circle one. According to the formula: (x-x1) 2 +(y-y1) 2 =r 2 ; Calculate and obtain the circle one; The determination of the circle 2 is: Determine two points on the line connecting the two obstacles and their extension line whose distance from the midpoint is the minimum turning radius, and calculate the distance between the two points and the tracking trajectory. Take the point with the smallest distance (x2, y2) as the center of circle 2, and take the vehicle's minimum turning radius r as the radius of circle 1. According to the formula: (x-x2) 2 +(y-y2) 2 =r 2 ; Calculate and obtain the circle 2; When determining the first circle, if the unmanned vehicle is located on the tracking trajectory, it is determined whether the midpoint is relatively located in the left front or right front of the unmanned vehicle. If it is in the left front, a point r from the vehicle's current position is selected from the left side as the center of the first circle. Otherwise, a point r from the vehicle's current position is selected from the right side as the center of the first circle. The calculation of the obstacle common tangent is specifically as follows: Four common tangent lines are calculated based on the circle 1 and the circle 2; Calculate the line connecting the centers of circle 1 and circle 2, and get the equation of the line connecting the centers: y=kx+b; Eliminate the two external common tangents whose slopes are equal to the slope k of the line connecting the centers of the circles, and obtain the two internal common tangents of the two circles; Calculate the intersection points of the two inner common tangents with the second circle respectively, and calculate the distance between the intersection points and the midpoint of the line connecting the two obstacles, and select the inner tangent line corresponding to the intersection point closest to the midpoint as the obstacle-penetrating common tangent line; S4. Obtain an obstacle traversal route based on the first circle, the second circle, and the common tangent line, and traverse the obstacle; The step S4 is specifically as follows: Calculate the intersection point B of the common tangent with circle 1 and the intersection point C with circle 2, and splice the line segment BC, arc AB, and arc CD into an obstacle route AD, and control the vehicle to travel along the obstacle route AD to overcome the obstacle; The arc AB is the minor arc between the vehicle's current position A and the intersection point B on circle 1, and the arc CD is the minor arc between the intersection point C and the midpoint D of the line connecting the two obstacles on circle 2; S5. Repeat steps S2 to S4 until the obstacle array is passed.

2. The method for unmanned tracking driving through an obstacle array according to claim 1, characterized in that: The step S1 specifically includes: S11. Obtain the vehicle's current latitude and longitude coordinates in real time, collect LiDAR point cloud data in real time, identify all obstacles directly in front of the vehicle through clustering algorithms, and record the radar coordinates of each obstacle directly in front of the unmanned vehicle; S12. According to the longitude and latitude coordinates of the current position of the vehicle, the radar coordinates are converted into longitude and latitude coordinates using a rotation matrix.

3. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by the processor, the following steps are implemented: S1. Obtain the latitude and longitude coordinates of the vehicle's current position and the latitude and longitude coordinates of all obstacles in front of the vehicle; S2. For each row of obstacles in the obstacle array in front of the autonomous vehicle, select the closest obstacles on both sides of the tracking trajectory and calculate the midpoint and perpendicular bisector of the line connecting the two obstacles; S3. Using the vehicle's minimum turning radius as its radius, calculate a circle (a first circle) that passes through the vehicle's current position, has the vehicle's current direction as its tangent, and has its center closest to the tracking track; and a circle (a second circle) that passes through the midpoint, has the perpendicular bisector as its tangent, and has its center closest to the tracking track. Calculate the common tangent line of the two circles. The circle one is determined as follows: Establish a first straight line passing through the vehicle's current position and perpendicular to the vehicle's forward direction, and determine two points on the first straight line whose distance from the vehicle's current position is the minimum turning radius. Calculate the distance between the two points and the tracking trajectory, and take the point with the smallest distance (x1, y1) as the center of circle one. Take the vehicle's minimum turning radius r as the radius of circle one. According to the formula: (x-x1) 2 +(y-y1) 2 =r 2 ; Calculate and obtain the circle one; The determination of the circle 2 is: Determine two points on the line connecting the two obstacles and their extension line whose distance from the midpoint is the minimum turning radius, and calculate the distance between the two points and the tracking trajectory. Take the point with the smallest distance (x2, y2) as the center of circle 2, and take the vehicle's minimum turning radius r as the radius of circle 1. According to the formula: (x-x2) 2 +(y-y2) 2 =r 2 ; Calculate and obtain the circle 2; When determining the first circle, if the unmanned vehicle is located on the tracking trajectory, it is determined whether the midpoint is relatively located in the left front or right front of the unmanned vehicle. If it is in the left front, a point r from the vehicle's current position is selected from the left side as the center of the first circle. Otherwise, a point r from the vehicle's current position is selected from the right side as the center of the first circle. The calculation of the obstacle common tangent is specifically as follows: Four common tangent lines are calculated based on the circle 1 and the circle 2; Calculate the line connecting the centers of circle 1 and circle 2, and get the equation of the line connecting the centers: y=kx+b; Eliminate the two external common tangents whose slopes are equal to the slope k of the line connecting the centers of the circles, and obtain the two internal common tangents of the two circles; Calculate the intersection points of the two inner common tangents with the second circle respectively, and calculate the distance between the intersection points and the midpoint of the line connecting the two obstacles, and select the inner tangent line corresponding to the intersection point closest to the midpoint as the obstacle-penetrating common tangent line; S4. Obtain an obstacle traversal route based on the first circle, the second circle, and the common tangent line, and traverse the obstacle; The step S4 is specifically as follows: Calculate the intersection point B of the common tangent with circle 1 and the intersection point C with circle 2, and splice the line segment BC, arc AB, and arc CD into an obstacle route AD, and control the vehicle to travel along the obstacle route AD to overcome the obstacle; The arc AB is the minor arc between the vehicle's current position A and the intersection point B on circle 1, and the arc CD is the minor arc between the intersection point C and the midpoint D of the line connecting the two obstacles on circle 2; S5. Repeat steps S2 to S4 until the obstacle array is passed.

4. A computer-readable storage medium according to claim 3, characterized in that: The step S1 specifically includes: S11. Obtain the vehicle's current latitude and longitude coordinates in real time, collect LiDAR point cloud data in real time, identify all obstacles directly in front of the vehicle through clustering algorithms, and record the radar coordinates of each obstacle directly in front of the unmanned vehicle; S12. According to the longitude and latitude coordinates of the current position of the vehicle, the radar coordinates are converted into longitude and latitude coordinates using a rotation matrix.

Images