Unmanned vehicle local path planning method and system based on feasible path

Abstract

The invention discloses a driverless vehicle local path planning method and system based on a feasible path. The driverless vehicle local path planning method comprises the steps of obtaining road information and information of a vehicle and other vehicles; determining a static impassable area according to the static obstacle, determining a three-section dynamic impassable area according to the dynamic obstacle, and removing the static impassable area and the three-section dynamic impassable area in the passable area boundary to obtain the passable area of the vehicle; planning the speed of the vehicle to obtain the real-time speed of the vehicle; various safety distances in the vehicle driving process are calculated; making a lane changing decision, and determining to change the lane or keep the current lane to run along with the vehicle; planned path points are obtained, a path is smoothed, path planning is completed, and the vehicle runs according to the currently planned path; the method has the advantages that the requirements of feasibility, timeliness and safety are met at the same time.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a method and system for local path planning of an unmanned vehicle based on a feasible path. Background Art

[0002] Autonomous driving technology achieves environmental perception through multi-sensor fusion (lidar, cameras, radar, etc.). A decision-making and planning system (global and local path planning) and a control system enable autonomous driving. Local path planning is one of the core technologies of autonomous driving.

[0003] The significance of local path planning in autonomous driving lies in: generating safe, smooth and traffic-compliant local trajectories in real time in dynamic and complex environments, ensuring that the vehicle can avoid obstacles, maintain comfort and reach the target point efficiently. Local path planning directly affects the reliability, riding experience and traffic efficiency of autonomous driving.

[0004] The current difficulties in local path planning are: the feasibility of selecting the local planning target position, the timeliness of the vehicle tracking the original path, and the safety of the vehicle path tracking process.

[0005] 1. Regarding feasibility, existing technologies use the midpoint of the desired lane line as the target location, ignoring that in real-world situations, other vehicles are not always located in the center of the road and that obstacles are often randomly positioned. Consequently, the planned path may not meet actual driving requirements. Setting a safety distance that is too large increases driving costs, while setting a safety distance that is too small can compromise safety. For example, the local trajectory planning method and device for intelligent vehicles disclosed in Chinese Patent Publication No. CN 106114507 A uses the center of the lane change lane as the target location. If there are small obstacles at the edge of the lane change lane that do not affect vehicle passage, this method may misjudge the situation as not meeting the lane change conditions because the safety distance is set too large.

[0006] 2. Regarding timeliness, existing technologies plan based on a fixed speed, which doesn't align with real-world driving scenarios. Unexpected circumstances can cause real-time speed fluctuations in real-time, leading to cumulative discrepancies between the overall path tracking time and the predicted time, resulting in poor timeliness. For example, Chinese Patent Publication No. CN112362074A discloses a method for local path planning for intelligent vehicles in structured environments. This method directly sets the vehicle speed to Va without performing speed planning, making it susceptible to the aforementioned issue of not aligning with real-world driving scenarios.

[0007] 3. Regarding safety, existing technologies plan based on a fixed safety distance, ignoring the speed of the vehicle and other vehicles. The required safety distance varies under different conditions, and excessively large distances increase driving costs. For example, Chinese Patent Publication No. CN 116125987 A discloses a method and device for vehicle local path planning. This method selects multiple control points using discrete sampling at fixed distances, ignoring the issue of real-time safety distances. Setting a small discrete distance creates the risk of collision due to fluid dynamics, while setting a large discrete distance increases driving costs. Summary of the Invention

[0008] The technical problem to be solved by the present invention is that the local path planning method of the existing unmanned driving process is difficult to meet the requirements of feasibility, timeliness and safety at the same time.

[0009] The present invention solves the above technical problems by the following technical means: a local path planning method for an unmanned vehicle based on a feasible path, comprising: S1. Obtain road information, self-vehicle information, and other vehicle information; S2. Determine a static impassable area based on static obstacles and a three-segment dynamic impassable area based on dynamic obstacles. The first segment is the position where the dynamic obstacle stops at maximum deceleration, the second segment is the position reached by the dynamic obstacle at a constant speed within the same time, and the third segment is the position reached by the dynamic obstacle at maximum acceleration within the same time. The area formed by the rear of the vehicle in the first segment and the front of the vehicle in the third segment is the three-segment dynamic impassable area. The same time refers to the time when the dynamic obstacle stops at maximum deceleration. Within the boundary of the traversable area, remove the static impassable area and the three-segment dynamic impassable area to obtain the traversable area for the vehicle. S3. Plan the speed of the vehicle and obtain the real-time speed of the vehicle; S4. Calculate various safety distances during vehicle driving; S5: Based on the information obtained in S1-S4, the driver makes a lane change decision, deciding whether to change lanes or stay in the current lane and follow the vehicle. S6. Obtain the planned path points and smooth the path to complete the path planning. The vehicle drives according to the currently planned path.

[0010] Furthermore, S2 includes: S2.1. Obtain the boundaries of the traversable area by road edge, median strip or central divider; S2.2. Identify static and dynamic obstacles based on the speed of other vehicles. S2.3. Determine the static impassable area based on the shape of the static obstacle, which is equivalent to a rectangle parallel to the lane line; S2.4. Determine the three-stage dynamic impassable area based on the speed and acceleration / deceleration capabilities of the dynamic obstacle; S2.5. Within the boundary of the traversable area, remove the static impassable area and the three-stage dynamic impassable area. If the distance to the obstacle close to the boundary of the traversable area is less than the minimum safe distance, remove the area between the obstacles and the boundary of the traversable area to obtain the traversable area for the vehicle.

[0011] Furthermore, S3 includes: S3.1. Denormalize the curvature, equating the maximum curvature to 0 and the minimum curvature to 1. Set the default minimum ego vehicle speed V0 and the default maximum ego vehicle speed V0 + V1, where V1 is the difference between the default maximum ego vehicle speed and the default minimum ego vehicle speed. Given the maximum turning radius Lmax and the minimum turning radius Lmin, the real-time position curvature of the globally planned vectorized path is K, with the minimum curvature Kmin = 1 / Lmax and the maximum curvature Kmax = 1 / Lmin. Denormalize the curvature Knorm = (Kmax - K) / (Kmax - Kmin). S3.2. Initial default speed V = V0 + V1 for each position of the globally planned vectorized path Knorm; S3.3. Given the total length of the route L, calculate the default travel time T required to fully follow the default speed based on the initial default speed V. S3.4. Solve the real-time velocity VZ=β V, where β is the real-time speed multiplier and β={T (L-Lk)} / {L (T-Tk)}, Tk is the actual driving time, Lk is the actual driving distance, and the upper and lower limits of VZ are set to constrain it.

[0012] S3.5. Depending on the lane change decision, decelerate, maintain a constant speed, or accelerate based on the real-time speed during lane changes and following. Furthermore, S4 includes: S4.1. Calculate the safe distance LA between the front of the vehicle and the rear of the vehicle ahead in the lane based on the vehicle's real-time speed VZ and its maximum deceleration AZ2. , where T1 is the system decision time; based on the real-time speed VZ of the ego vehicle, the speed VC of the vehicle in front of the lane changing lane, and the maximum deceleration AC2 of the vehicle in front of the lane changing lane, the safe distance LC between the front of the ego vehicle and the rear of the vehicle in front of the lane changing lane is calculated. , where T2 is the lane changing time; based on the real-time speed VZ of the ego vehicle, the speed VD of the vehicle behind the lane changing lane, and the maximum acceleration AD1 of the vehicle behind the lane changing lane, the safe distance LD between the rear of the ego vehicle and the front of the vehicle behind the lane changing lane is calculated. ; S4.2. Calculate the safe distance perpendicular to the lane line: Based on the vehicle's real-time speed VZ and the speeds of other vehicles in the left and right lanes VX, where VX is the speed of the vehicle ahead of you in your lane VA, the speed of the vehicle behind you in your lane VB, the speed of the vehicle ahead of you in the lane change lane VC, or the speed of the vehicle behind you in the lane change lane VD, calculate the safe distance LS perpendicular to the lane line between your vehicle and other vehicles in the left and right lanes. , where K1 and K2 are the first time parameter and the second time parameter respectively.

[0013] Furthermore, S5 includes: S5.1. Detect whether there are other vehicles within the forward detection range LF and rearward detection range LR of the vehicle; S5.2. Obtain the distance SA between the ego vehicle and the vehicle A in front of the ego vehicle's lane, the distance SC between the ego vehicle and the vehicle C in front of the ego vehicle's lane-changing lane, and the distance SD between the ego vehicle and the vehicle D in the lane-changing lane behind the ego vehicle; S5.3. If SA > LF, then maintain the original lane speed, which is the planned real-time speed VZ. If there is a vehicle A in the lane ahead of you within the forward detection range LF, and LA < SA ≤ LF, then maintain the lane speed, which is the real-time speed VZ of the vehicle just after detecting the vehicle A in the lane ahead of you. If SA ≤ LA, then compare the real-time speed VZ of the vehicle with the speed VA of the vehicle in the lane ahead of you. If , then keep following the vehicle in the lane, and the speed is the speed of the vehicle in front of the vehicle in the lane VA, where K3 and K4 are the first proportional coefficient and the second proportional coefficient respectively, and K3 < 1, K4 > 1; if , then keep driving at the original speed of the lane, which is the planned real-time speed VZ; if , then prepare to change lanes; S5.4 Lane Change Direction Decision: Determine the width of the narrowest traversable area to the left and right of the vehicle and choose to change lanes to the wider lane. If the front-to-rear safety distance to the wider lane is insufficient, change lanes to the narrower lane. If the front-to-rear safety distance to the narrower lane is insufficient, maintain the lane and follow the vehicle. S5.5. If there are no other vehicles within the forward detection distance LF and the rearward detection distance LR of the lane change lane, that is, SC>LF and SD>LR, then the lane change will proceed at the original speed, which is the planned real-time speed VZ. If LC<SC≤LF and SD>LR, then the lane change will be accelerated, with the speed being K5. VZ, where K5 is the third proportional coefficient and K5>1; if LD<SD≤LR, and SC>LF, then change lanes and accelerate, with a speed of K6 VZ, where K6 is the fourth proportional coefficient and K6>1; if LC<SC≤LF, and LD<SD≤LR, then change lanes and accelerate, the speed is K7 VZ, where K7 is the fifth proportional coefficient and K7>1; if SC≤LC, or SD≤LD, then maintain the lane and follow the vehicle at the speed of the preceding vehicle VA; where K5<K6<K7.

[0014] Furthermore, S5.2 includes: Define the direction parallel to the lane line as the X axis, the direction perpendicular to the lane line as the Y axis, and the geometric center of the starting position of the vehicle as the origin to calculate the distance between the vehicle and the vehicle in front of it. ,in, is the X-axis coordinate of the geometric center of the vehicle in front of the lane, is the X-axis coordinate of the vehicle’s geometric center, is the length of the vehicle ahead in the lane, is the length of the vehicle; the distance between the vehicle and the vehicle C in front of the lane where the vehicle changes lanes ,in, is the X-axis coordinate of the geometric center of the front vehicle in the lane change lane, is the length of the vehicle in front of the lane changing lane; the distance between the vehicle and the vehicle behind it in the lane changing lane D ,in, is the X-axis coordinate of the geometric center of the vehicle behind the lane change lane, is the length of the vehicle behind in the lane change.

[0015] Furthermore, the S6 includes: S6.1. In the lane-changing scenario, if the speed of vehicle A in the ego vehicle lane is zero, the coordinates of two feature points are obtained in the passable area, namely (X2, Y2) and (X3, Y3), where X2 is the X-axis coordinate of the geometric center of the static impassable area in the ego vehicle lane, and Y2 is the Y-axis coordinate of the geometric center of the static impassable area in the ego vehicle lane and the lane-changing direction. of and, is the vehicle width, is the width of the vehicle ahead in the ego vehicle lane; X3 is the X-axis coordinate of the front position of vehicle A in the ego vehicle lane in the static impassable area; Y3 is the Y-axis coordinate of the center position of the lane change lane; S6.2. In the lane-changing scenario, if the speed of vehicle A in the ego vehicle's lane is non-zero, the coordinates of three feature points are obtained within the passable area, namely (X1', Y1'), (X2', Y2'), and (X3', Y3'), where X1' is the X-axis coordinate of the rear position of vehicle A in the ego vehicle's lane in the first segment of the three-segment dynamic impassable area, and Y1' is the Y-axis coordinate of the geometric center position of vehicle A in the ego vehicle's lane in the three-segment dynamic impassable area and the lane-changing direction. X2' is the X-axis coordinate of the geometric center position of the vehicle A in front of the vehicle lane in the second section of the three-section dynamic impassable area, and Y2' is the Y-axis coordinate of the geometric center position of the vehicle A in front of the vehicle lane in the three-section dynamic impassable area and the lane change direction. X3' is the X-axis coordinate of the front position of vehicle A in the third segment of the three-segment dynamic impassable area in the ego vehicle lane, and Y3' is the Y-axis coordinate of the center position of the lane change lane; S6.3. Use cubic spline interpolation to make the feature points continuous, obtain the local path trajectory, and complete the path planning.

[0016] Furthermore, the method is executed once every 10 ms according to the complete process of S1-S6 to plan the local path trajectory under the current environmental state.

[0017] The present invention also provides a local path planning system for an unmanned vehicle based on a feasible path, comprising: Data acquisition module, used to obtain road information, self-vehicle and other vehicle information; The passable area determination module is used to determine a static impassable area based on static obstacles and a three-stage dynamic impassable area based on dynamic obstacles. The first stage is the position where the dynamic obstacle stops at maximum deceleration, the second stage is the position reached by the dynamic obstacle at a constant speed within the same time, and the third stage is the position reached by the dynamic obstacle at maximum acceleration within the same time. The area formed between the rear end of the first stage and the front end of the third stage is the three-stage dynamic impassable area; the same time refers to the time when the dynamic obstacle stops at maximum deceleration. Within the boundary of the passable area, the static impassable area and the three-stage dynamic impassable area are removed to obtain the passable area of ​​the vehicle. Speed ​​planning module, used to plan the speed of the vehicle and obtain the real-time speed of the vehicle; Safety distance calculation module, used to calculate various safety distances during vehicle driving; The lane change decision module is used to make lane change decisions based on the information obtained from the data acquisition module to the safety distance calculation module, and decide whether to change lanes or stay in the current lane and follow the vehicle; The path planning module is used to obtain the planned path points and smooth the path to complete the path planning. The vehicle drives according to the currently planned path.

[0018] Furthermore, the traffic area determination module is further configured to: S2.1. Obtain the boundaries of the traversable area by road edge, median strip or central divider; S2.2. Identify static and dynamic obstacles based on the speed of other vehicles. S2.3. Determine the static impassable area based on the shape of the static obstacle, which is equivalent to a rectangle parallel to the lane line; S2.4. Determine the three-stage dynamic impassable area based on the speed and acceleration / deceleration capabilities of the dynamic obstacle; S2.5. Within the boundary of the traversable area, remove the static impassable area and the three-stage dynamic impassable area. If the distance to the obstacle close to the boundary of the traversable area is less than the minimum safe distance, remove the area between the obstacles and the boundary of the traversable area to obtain the traversable area for the vehicle.

[0019] Furthermore, the speed planning module is further configured to: S3.1. Denormalize the curvature, equating the maximum curvature to 0 and the minimum curvature to 1. Set the default minimum ego vehicle speed V0 and the default maximum ego vehicle speed V0 + V1, where V1 is the difference between the default maximum ego vehicle speed and the default minimum ego vehicle speed. Given the maximum turning radius Lmax and the minimum turning radius Lmin, the real-time position curvature of the globally planned vectorized path is K, with the minimum curvature Kmin = 1 / Lmax and the maximum curvature Kmax = 1 / Lmin. Denormalize the curvature Knorm = (Kmax - K) / (Kmax - Kmin). S3.2. Initial default speed V = V0 + V1 for each position of the globally planned vectorized path Knorm; S3.3. Given the total length of the route L, calculate the default travel time T required to fully follow the default speed based on the initial default speed V. S3.4. Solve the real-time velocity VZ=β V, where β is the real-time speed multiplier and β={T (L-Lk)} / {L (T-Tk)}, Tk is the actual driving time, Lk is the actual driving distance, and the upper and lower limits of VZ are set to constrain it.

[0020] Furthermore, the safety distance calculation module is also used to: S4.1. Calculate the safe distance LA between the front of the vehicle and the rear of the vehicle ahead in the lane based on the vehicle's real-time speed VZ and its maximum deceleration AZ2. , where T1 is the system decision time; based on the real-time speed VZ of the ego vehicle, the speed VC of the vehicle in front of the lane changing lane, and the maximum deceleration AC2 of the vehicle in front of the lane changing lane, the safe distance LC between the front of the ego vehicle and the rear of the vehicle in front of the lane changing lane is calculated. , where T2 is the lane changing time; based on the real-time speed VZ of the ego vehicle, the speed VD of the vehicle behind the lane changing lane, and the maximum acceleration AD1 of the vehicle behind the lane changing lane, the safe distance LD between the rear of the ego vehicle and the front of the vehicle behind the lane changing lane is calculated. ; S4.2. Calculate the safe distance perpendicular to the lane line: Based on the vehicle's real-time speed VZ and the speeds of other vehicles in the left and right lanes VX, where VX is the speed of the vehicle ahead of you in your lane VA, the speed of the vehicle behind you in your lane VB, the speed of the vehicle ahead of you in the lane change lane VC, or the speed of the vehicle behind you in the lane change lane VD, calculate the safe distance LS perpendicular to the lane line between your vehicle and other vehicles in the left and right lanes. , where K1 and K2 are the first time parameter and the second time parameter respectively.

[0021] Furthermore, the lane change decision module is further configured to: S5.1. Detect whether there are other vehicles within the forward detection range LF and rearward detection range LR of the vehicle; S5.2. Obtain the distance SA between the ego vehicle and the vehicle A in front of the ego vehicle's lane, the distance SC between the ego vehicle and the vehicle C in front of the ego vehicle's lane-changing lane, and the distance SD between the ego vehicle and the vehicle D in the lane-changing lane behind the ego vehicle; S5.3. If SA > LF, then maintain the original lane speed, which is the planned real-time speed VZ. If there is a vehicle A in the lane ahead of you within the forward detection range LF, and LA < SA ≤ LF, then maintain the lane speed, which is the real-time speed VZ of the vehicle just after detecting the vehicle A in the lane ahead of you. If SA ≤ LA, then compare the real-time speed VZ of the vehicle with the speed VA of the vehicle in the lane ahead of you. If , then keep following the vehicle in the lane, and the speed is the speed of the vehicle in front of the vehicle in the lane VA, where K3 and K4 are the first proportional coefficient and the second proportional coefficient respectively, and K3 < 1, K4 > 1; if , then keep driving at the original speed of the lane, which is the planned real-time speed VZ; if , then prepare to change lanes; S5.4 Lane Change Direction Decision: Determine the width of the narrowest traversable area to the left and right of the vehicle and choose to change lanes to the wider lane. If the front-to-rear safety distance to the wider lane is insufficient, change lanes to the narrower lane. If the front-to-rear safety distance to the narrower lane is insufficient, maintain the lane and follow the vehicle. S5.5. If there are no other vehicles within the forward detection distance LF and the rearward detection distance LR of the lane change lane, that is, SC>LF and SD>LR, then the lane change will proceed at the original speed, which is the planned real-time speed VZ. If LC<SC≤LF and SD>LR, then the lane change will be accelerated, with the speed being K5. VZ, where K5 is the third proportional coefficient and K5>1; if LD<SD≤LR, and SC>LF, then change lanes and accelerate, with a speed of K6 VZ, where K6 is the fourth proportional coefficient and K6>1; if LC<SC≤LF, and LD<SD≤LR, then change lanes and accelerate, the speed is K7 VZ, where K7 is the fifth proportional coefficient and K7>1; if SC≤LC, or SD≤LD, then maintain the lane and follow the vehicle at the speed of the preceding vehicle VA; where K5<K6<K7.

[0022] Furthermore, S5.2 includes: Define the direction parallel to the lane line as the X axis, the direction perpendicular to the lane line as the Y axis, and the geometric center of the starting position of the vehicle as the origin to calculate the distance between the vehicle and the vehicle in front of it. ,in, is the X-axis coordinate of the geometric center of the vehicle in front of the lane, is the X-axis coordinate of the vehicle’s geometric center, is the length of the vehicle ahead in the lane, is the length of the vehicle; the distance between the vehicle and the vehicle C in front of the lane where the vehicle changes lanes ,in, is the X-axis coordinate of the geometric center of the front vehicle in the lane change lane, is the length of the vehicle in front of the lane changing lane; the distance between the vehicle and the vehicle behind it in the lane changing lane D ,in, is the X-axis coordinate of the geometric center of the vehicle behind the lane change lane, is the length of the vehicle behind in the lane change.

[0023] Furthermore, the path planning module is further configured to: S6.1. In the lane-changing scenario, if the speed of vehicle A in the ego vehicle lane is zero, the coordinates of two feature points are obtained in the passable area, namely (X2, Y2) and (X3, Y3), where X2 is the X-axis coordinate of the geometric center of the static impassable area in the ego vehicle lane, and Y2 is the Y-axis coordinate of the geometric center of the static impassable area in the ego vehicle lane and the lane-changing direction. of and, is the vehicle width, is the width of the vehicle ahead in the ego vehicle lane; X3 is the X-axis coordinate of the front position of vehicle A in the ego vehicle lane in the static impassable area; Y3 is the Y-axis coordinate of the center position of the lane change lane; S6.2. In the lane-changing scenario, if the speed of vehicle A in the ego vehicle's lane is non-zero, the coordinates of three feature points are obtained within the passable area, namely (X1', Y1'), (X2', Y2'), and (X3', Y3'), where X1' is the X-axis coordinate of the rear position of vehicle A in the ego vehicle's lane in the first segment of the three-segment dynamic impassable area, and Y1' is the Y-axis coordinate of the geometric center position of vehicle A in the ego vehicle's lane in the three-segment dynamic impassable area and the lane-changing direction. X2' is the X-axis coordinate of the geometric center position of the vehicle A in front of the vehicle lane in the second section of the three-section dynamic impassable area, and Y2' is the Y-axis coordinate of the geometric center position of the vehicle A in front of the vehicle lane in the three-section dynamic impassable area and the lane change direction. X3' is the X-axis coordinate of the front position of vehicle A in the third segment of the three-segment dynamic impassable area in the ego vehicle lane, and Y3' is the Y-axis coordinate of the center position of the lane change lane; S6.3. Use cubic spline interpolation to make the feature points continuous, obtain the local path trajectory, and complete the path planning.

[0024] Furthermore, the system is executed once every 10ms according to the complete process from the data acquisition module to the path planning module to plan the local path trajectory under the current environmental state.

[0025] The advantages of the present invention are: (1) The present invention sets a static impassable area and a three-stage dynamic impassable area and finally determines a traversable area. Path planning is performed within the traversable area. The planned path meets actual driving requirements, thereby avoiding the problem of the prior art directly using the midpoint of the desired lane line as the target position. The solution is highly feasible. At the same time, the present invention plans the speed and obtains the real-time speed of the vehicle to avoid the problem of driving at a fixed speed that does not conform to the driving scenario, ensuring the timeliness of the planned path. In addition, various safety distances are calculated to improve safety. The overall solution meets the requirements of feasibility, timeliness, and safety at the same time.

[0026] (2) In the traditional method, planning is performed according to the lane lines. Due to the uncertainty of the actual position of other vehicles, some drivable space may be sacrificed. The drivable area in the present invention takes the entire road drivable area as a whole, and makes the most reasonable local path planning according to the specific driving environment under the premise of complying with road traffic regulations, which is more in line with people's driving habits under the premise of ensuring safety; in the three-stage dynamic impassable area, the probability of change of the speed of other vehicles conforms to the normal distribution, and the probability of driving at maximum deceleration and maximum acceleration is relatively small, that is, the probability of the first and third impassable areas is relatively small. Therefore, in the selection of feature points, the position of the first feature point does not require a large safety distance, and the driving cost of the vehicle is reduced under the premise of ensuring safety.

[0027] (3) The present invention performs real-time dynamic speed planning, which not only ensures the controllability of the entire planning process time, but also adjusts the speed to a speed that is more in line with the current state according to the specific situation, ensuring that the tracking time is closer to the expected time while ensuring safety. Real-time dynamic safety distance planning can obtain the most suitable safety distance under the current environmental conditions; the complete technical solution can adjust various control parameters in real time to solve the optimal solution under the current environmental conditions, and plan the best operation trajectory while ensuring safety.

[0028] (4) In terms of feasibility, the existing technology (CN112362074A) only obtains a passable area based on the current state. In terms of timeliness, the speed planning only includes two local speed states: maintaining the current speed and stopping. In terms of safety, the safety distance during the local path curve cluster planning is the set safety margin constant D. In terms of feasibility, the present application obtains static impassable areas and three-stage dynamic impassable areas by obtaining road information, self-vehicle and other vehicle information, and then finally obtains the passable area based on the road boundary, predicting the environmental state within a period of time, which is more in line with the actual scene. In terms of timeliness, it includes global speed planning and local speed planning under different decision-making scenarios. During the entire path tracking process, it can ensure that even if different obstacles appear, the destination can be reached within the specified time. Local speed planning includes maintaining the original speed of the lane, constant speed, following the vehicle, changing to the original speed, accelerating, etc., which is not only more in line with the actual driving state, but also improves safety. In terms of safety, the safety distance during local path planning uses the real-time speed of the self-vehicle and the speed of other vehicles in the left and right lanes as variables, which conforms to physical constraints such as fluid dynamics. In addition, the method of obtaining the optimal path in this application is different from the existing technology. The optimal path is directly obtained through the feature points that meet the current state. There is no need to generate multiple candidate curve clusters and then calculate the optimal path through complex cost functions, which reduces the amount of calculation. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 This is a flow chart of a method for local path planning of an unmanned vehicle based on feasible paths disclosed in an embodiment of the present invention; Figure 2 A Frenet coordinate system diagram in the local path planning method for an unmanned vehicle based on a feasible path disclosed in an embodiment of the present invention; Figure 3 A schematic diagram of a traversable area in a local path planning method for an unmanned vehicle based on feasible paths disclosed in an embodiment of the present invention; Figure 4 A schematic diagram of distance definition in a local path planning method for an unmanned vehicle based on feasible paths disclosed in an embodiment of the present invention; Figure 5A schematic diagram of a lane change decision process in a local path planning method for an unmanned vehicle based on feasible paths disclosed in an embodiment of the present invention; Figure 6 Schematic diagram of different lane-changing scenarios in the local path planning method for an unmanned vehicle based on feasible paths disclosed in an embodiment of the present invention; Figure 7 This is a schematic diagram of different scene feature points and smooth paths in the local path planning method for an unmanned vehicle based on feasible paths disclosed in an embodiment of the present invention. DETAILED DESCRIPTION

[0030] To make the objectives, technical solutions, and advantages of the embodiments of the present invention more clear, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts shall fall within the scope of protection of the present invention.

[0031] Example 1 like Figure 1 As shown, embodiment 1 of the present invention provides a local path planning method for an unmanned vehicle based on a feasible path, comprising the following steps: S1. Obtain the globally planned vectorized path, vehicle, and environment information. This mainly involves obtaining road information, information about the vehicle itself, and information about other vehicles. The specific process is as follows: S1.1 Road Information The vectorized path coordinates of the global plan are converted to the Frenet coordinate system. The Frenet coordinate system uses the distance parallel to the lane line as S and the distance perpendicular to the lane line as d. Figure 2 Schematic diagram of the Frenet coordinate system.

[0032] Vectorized path turning radius R; Types of lane dividers and center dividers, road edges, median strips, etc.

[0033] The above information acquisition methods are prior art, and the information acquisition methods are not improvements of the present invention and are not within the scope of protection of this application, so they will not be described in detail. For example, lane line recognition can adopt the lane line recognition method described in the "Lane Line Recognition Model Training Method, Device and Lane Line Recognition Method and Device" disclosed in Chinese Patent Publication No. CN113298050A.

[0034] S1.2. Information about your own vehicle and other vehicles The road where each vehicle is traveling is defined as the own vehicle lane, and the lane where the vehicle is preparing to change lanes is defined as the lane-changing lane. Define each vehicle as the ego vehicle Z, the vehicle in front of the ego lane A, the vehicle behind the ego lane B, the vehicle in front of the lane C, and the vehicle behind the lane D; The forward detection range LF of the vehicle sensor and the rearward detection range LR of the vehicle sensor; The geometric center position, shape, speed, speed direction, and maximum acceleration / deceleration of the self-vehicle and the other vehicle are obtained. The self-vehicle information is obtained from data stored in the self-vehicle memory. This information about the other vehicle is obtained using existing methods. For example, the method for obtaining the shape and center of mass of the other vehicle described in "A Local Path Planning and Guidance Method for a Differential-Speed ​​Mobile Robot in a Mixed Environment" disclosed in Chinese Patent Publication No. CN115268460A can be used to calculate the speed and direction of the other vehicle using the center of mass position at different times. The solution disclosed in Chinese Patent Publication No. CN117689842A describes the shape and center of mass of the other vehicle, and then calculates the speed and direction of the other vehicle using the center of mass position at different times. The solution disclosed in CN117889867A describes a method for obtaining the center of mass position and speed direction of the other vehicle. The solution disclosed in CN114973123A describes a method for identifying the type of the other vehicle, and then obtains the maximum acceleration / deceleration of the other vehicle using publicly available information online. The solution disclosed in CN 114503174 A introduces a method for identifying the model of another vehicle, and then obtains the maximum acceleration and deceleration of the other vehicle through publicly available information online. Therefore, the other vehicle information can be fully obtained using the solution described in the above prior art. The present invention does not improve this part of the technology and does not specifically limit it.

[0035] The geometric center positions are converted to the Frenet coordinate system: the geometric center position of the ego vehicle (XZ, YZ), the geometric center position of the vehicle in front of the ego vehicle lane (XA, YA), the geometric center position of the vehicle behind the ego vehicle lane (XB, YB), the geometric center position of the vehicle in front of the lane changing lane (XC, YC), and the geometric center position of the vehicle behind the lane changing lane (XD, YD). The length of the own vehicle is XLZ, the length of the vehicle in front of the own vehicle lane is XLA, the length of the vehicle behind the own vehicle lane is XLB, the length of the vehicle in front of the lane changing lane is XLC, and the length of the vehicle behind the lane changing lane is XLD; The width of the own vehicle WZ, the width of the vehicle in front of the own vehicle lane WA, the width of the vehicle behind the own vehicle lane WB, the width of the vehicle in front of the lane changing lane WC, the width of the vehicle behind the lane changing lane WD; The speed of the vehicle is VZ, the speed of the vehicle in front of the vehicle in the lane is VA, the speed of the vehicle behind the vehicle in the lane is VB, the speed of the vehicle in front of the lane where the lane is being changed is VC, and the speed of the vehicle behind the lane where the lane is being changed is VD; The maximum acceleration of the own vehicle AZ1, the maximum deceleration of the own vehicle AZ2, the maximum acceleration of the vehicle in front of the own lane AA1, the maximum deceleration of the vehicle in front of the own lane AA2, the maximum acceleration of the vehicle behind the own lane AB1, the maximum deceleration of the vehicle behind the own lane AB2, the maximum acceleration of the vehicle in front of the lane changing lane AC1, the maximum deceleration of the vehicle in front of the lane changing lane AC2, the maximum acceleration of the vehicle behind the lane changing lane AD1, the maximum deceleration of the vehicle behind the lane changing lane AD2.

[0036] S2. Determine a static impassable area based on static obstacles, and determine a three-stage dynamic impassable area based on dynamic obstacles. Within the boundary of the passable area, remove the static impassable area and the three-stage dynamic impassable area to obtain the passable area for the vehicle; Figure 3 As shown, the specific process is as follows: S2.1. Obtain the boundaries of the traversable area by road edge, median strip or central divider; S2.2. Identify static obstacles and dynamic obstacles based on the speed of other vehicles. When the speed is 0, it is a static obstacle; when the speed is not 0, it is a dynamic obstacle.

[0037] S2.3. Determine the static impassable area based on the shape of the static obstacle, which is equivalent to a rectangle parallel to the lane line; S2.4. Determine a three-segment dynamic impassable zone based on the speed and acceleration / deceleration capabilities of the dynamic obstacle. The first segment is the position where the dynamic obstacle stops at maximum deceleration; the second segment is the position reached by the dynamic obstacle at a constant speed within the same time; and the third segment is the position reached by the dynamic obstacle at maximum acceleration within the same time. The area between the rear of the vehicle in the first segment and the front of the vehicle in the third segment is the three-segment dynamic impassable zone. The same time refers to the time it takes for the dynamic obstacle to stop at maximum deceleration. S2.5. Within the boundary of the traversable area, remove the static impassable area and the three-stage dynamic impassable area. If the distance to the obstacle close to the boundary of the traversable area is less than the minimum safe distance, remove the area between the obstacles and the boundary of the traversable area to obtain the traversable area for the vehicle.

[0038] S3. Plan the speed of the vehicle and obtain the real-time speed of the vehicle. The specific process is as follows: S3.1. Denormalize the curvature, equating the maximum curvature to 0 and the minimum curvature to 1. Set the default minimum ego vehicle speed V0 and the default maximum ego vehicle speed V0 + V1, where V1 is the difference between the default maximum ego vehicle speed and the default minimum ego vehicle speed. Given the maximum turning radius Lmax and the minimum turning radius Lmin, the real-time position curvature of the globally planned vectorized path is K, with the minimum curvature Kmin = 1 / Lmax and the maximum curvature Kmax = 1 / Lmin. Denormalize the curvature Knorm = (Kmax - K) / (Kmax - Kmin). S3.2. Initial default speed V = V0 + V1 for each position of the globally planned vectorized path Knorm; S3.3. Given a total path length of L, calculate the default travel time T required to fully comply with the default speed based on the initial default speed V. The default travel time T is calculated using existing techniques, such as integration, where the total path length L is equal to the integration of the initial default speed V within the default travel time T. Substituting the total path length L and the initial default speed V into the equation yields the default travel time T.

[0039] S3.4. Solve the real-time velocity VZ=β V, where β is the real-time speed multiplier and β={T (L-Lk)} / {L (T-Tk)}, Tk is the actual driving time, Lk is the actual driving distance, and the upper and lower limits of VZ are set to constrain it, VZmin≤VZ≤VZmax.

[0040] S3.5. Depending on the lane change decision, the vehicle may decelerate, maintain a constant speed, or accelerate based on the real-time speed during lane changes and following.

[0041] S4. Calculate various safety distances during vehicle travel. The specific process is as follows: S4.1. Calculate the safe distance parallel to the lane line: Based on the vehicle's real-time speed VZ and its maximum deceleration AZ2, calculate the safe distance LA between the front of the vehicle and the rear of the vehicle in front of it. , where T1 is the system decision time; based on the real-time speed VZ of the ego vehicle, the speed VC of the vehicle in front of the lane changing lane, and the maximum deceleration AC2 of the vehicle in front of the lane changing lane, the safe distance LC between the front of the ego vehicle and the rear of the vehicle in front of the lane changing lane is calculated. , where T2 is the lane changing time; based on the real-time speed VZ of the ego vehicle, the speed VD of the vehicle behind the lane changing lane, and the maximum acceleration AD1 of the vehicle behind the lane changing lane, the safe distance LD between the rear of the ego vehicle and the front of the vehicle behind the lane changing lane is calculated. ; S4.2. Calculate the safe distance perpendicular to the lane line: Based on the vehicle's real-time speed VZ and the speeds of other vehicles in the left and right lanes VX, where VX is the speed of the vehicle ahead of you in your lane VA, the speed of the vehicle behind you in your lane VB, the speed of the vehicle ahead of you in the lane change lane VC, or the speed of the vehicle behind you in the lane change lane VD, calculate the safe distance LS perpendicular to the lane line between your vehicle and other vehicles in the left and right lanes. , where K1 and K2 are the first time parameter and the second time parameter respectively.

[0042] S5: Based on the information obtained from S1-S4, make a lane change decision, deciding whether to change lanes or stay in the current lane and follow the vehicle; Figures 4 to 6 As shown, the specific process is as follows: S5.1. Detect whether there are other vehicles within the forward detection distance LF and rearward detection distance LR of the own vehicle's lane and the lane change lane; the forward detection distance LF and rearward detection distance LR are the distances that can be detected by the forward and rearward sensors of the own vehicle, respectively.

[0043] S5.2. Calculate the distance between your vehicle and the detected other vehicles, including the distance SA between your vehicle and the vehicle A in front of you in your lane, the distance SC between your vehicle and the vehicle C in front of you in your lane-changing lane, and the distance SD between your vehicle and the vehicle D in the lane-changing lane. The specific calculation method is: with the direction parallel to the lane line as the X-axis, the direction perpendicular to the lane line as the Y-axis, and the geometric center of your vehicle's starting position as the origin, calculate the distance between your vehicle and the vehicle A in front of you in your lane. ,in, is the X-axis coordinate of the geometric center of the vehicle in front of the lane, is the X-axis coordinate of the vehicle’s geometric center, is the length of the vehicle ahead in the lane, is the length of the vehicle; the distance between the vehicle and the vehicle C in front of the lane where the vehicle changes lanes ,in, is the X-axis coordinate of the geometric center of the front vehicle in the lane change lane, is the length of the vehicle in front of the lane changing lane; the distance between the vehicle and the vehicle behind it in the lane changing lane D ,in, is the X-axis coordinate of the geometric center of the vehicle behind the lane change lane, is the length of the vehicle behind in the lane change.

[0044] S5.3. If there is no vehicle A ahead of you in the lane within the forward detection distance LF, that is, SA>LF, then maintain the original lane speed, which is the planned real-time speed VZ. If there is a vehicle A ahead of you in the lane within the forward detection distance LF, and LA<SA≤LF, then maintain the lane speed, which is the real-time speed VZ of the vehicle just detected. If SA≤LA, then compare the real-time speed VZ of the vehicle with the speed VA of the vehicle ahead of you in the lane. If , then keep following the vehicle in the lane, and the speed is the speed of the vehicle in front of the vehicle in the lane VA, where K3 and K4 are the first proportional coefficient and the second proportional coefficient respectively, and K3 < 1, K4 > 1; if , then keep driving at the original speed of the lane, which is the planned real-time speed VZ; if , then prepare to change lanes; S5.4 Lane Change Direction Decision: Determine the width of the narrowest traversable area to the left and right of the vehicle and choose to change lanes to the wider lane. If the front-to-rear safety distance to the wider lane is insufficient, change lanes to the narrower lane. If the front-to-rear safety distance to the narrower lane is insufficient, maintain the lane and follow the vehicle. S5.5. If there are no other vehicles within the forward detection distance LF and the rearward detection distance LR of the lane change lane, that is, SC>LF and SD>LR, then the lane change will proceed at the original speed, which is the planned real-time speed VZ. If LC<SC≤LF and SD>LR, then the lane change will be accelerated, with the speed being K5. VZ, where K5 is the third proportional coefficient and K5>1; if LD<SD≤LR, and SC>LF, then change lanes and accelerate, with a speed of K6 VZ, where K6 is the fourth proportional coefficient and K6>1; if LC<SC≤LF, and LD<SD≤LR, then change lanes and accelerate, the speed is K7 VZ, where K7 is the fifth proportional coefficient and K7>1; if SC≤LC, or SD≤LD, then maintain the lane and follow the vehicle at the speed of the preceding vehicle VA; where K5<K6<K7.

[0045] S6. Obtain the planned path points and smooth the path to complete the path planning. The vehicle drives according to the currently planned path. Figure 7 As shown, the specific process is as follows: S6.1. In the lane-changing scenario, if the speed of vehicle A in the ego vehicle lane is zero, the coordinates of two feature points are obtained in the passable area, namely (X2, Y2) and (X3, Y3), where X2 is the X-axis coordinate of the geometric center of the static impassable area in the ego vehicle lane, and Y2 is the Y-axis coordinate of the geometric center of the static impassable area in the ego vehicle lane and the lane-changing direction. of and, is the vehicle width, is the width of the vehicle ahead in the ego vehicle lane; X3 is the X-axis coordinate of the front position of vehicle A in the ego vehicle lane in the static impassable area; Y3 is the Y-axis coordinate of the center position of the lane change lane; S6.2. In the lane-changing scenario, if the speed of vehicle A in the ego vehicle's lane is non-zero, the coordinates of three feature points are obtained within the passable area, namely (X1', Y1'), (X2', Y2'), and (X3', Y3'), where X1' is the X-axis coordinate of the rear position of vehicle A in the ego vehicle's lane in the first segment of the three-segment dynamic impassable area, and Y1' is the Y-axis coordinate of the geometric center position of vehicle A in the ego vehicle's lane in the three-segment dynamic impassable area and the lane-changing direction. X2' is the X-axis coordinate of the geometric center position of the vehicle A in front of the vehicle lane in the second section of the three-section dynamic impassable area, and Y2' is the Y-axis coordinate of the geometric center position of the vehicle A in front of the vehicle lane in the three-section dynamic impassable area and the lane change direction. X3' is the X-axis coordinate of the front position of vehicle A in the third segment of the three-segment dynamic impassable area in the ego vehicle lane, and Y3' is the Y-axis coordinate of the center position of the lane change lane; S6.3, Smooth Path: Cubic spline interpolation is used to continuousize feature points, obtain a local path trajectory, and complete path planning. The specific process of cubic spline interpolation is as follows: When the speed of the preceding vehicle A in the ego vehicle lane is non-zero, three feature points are selected. Together with the coordinates of the ego vehicle Z at that moment, the coordinates of the four points are known. Furthermore, the direction of the ego vehicle Z at that moment and the direction of the feature point (X3', Y3') are both parallel to the lane line. A piecewise cubic polynomial is constructed, forcing the function values, first-order derivatives, and second-order derivatives of all interior and boundary points to be continuous. The directions of the start and end points are used as clamping boundary conditions. The derivatives of the intermediate points are solved using a global system of equations to obtain a highly smooth curve.

[0046] When the speed of the preceding vehicle A in the ego vehicle lane is zero, two feature points are selected. Together with the coordinates of the ego vehicle Z at that moment, the coordinates of the three points are known. Furthermore, the direction of the ego vehicle Z at that moment and the direction of the feature point (X3, Y3) are both parallel to the lane line. A piecewise cubic polynomial is constructed, forcing the function values, first-order derivatives, and second-order derivatives of all interior and boundary points to be continuous. The directions of the start and end points serve as clamping boundary conditions. The derivatives of the intermediate points are solved using a global system of equations to obtain a highly smooth curve. This method balances sudden centrifugal acceleration changes and improves driving comfort. Cubic spline interpolation is a well-known technique and will not be elaborated on here.

[0047] It should be noted that, according to the operating cycle of the system, the present invention executes the method once every 10 ms according to the complete process of S1-S6 to plan a local path trajectory under the current environmental state.

[0048] Through the above technical solution, the present invention sets static impassable areas, three-stage dynamic impassable areas, and ultimately determines a traversable area. Path planning is performed within the traversable area, and the planned path meets actual driving requirements, thereby avoiding the problem of the prior art directly using the desired lane line midpoint as the target location. The solution is highly feasible. At the same time, the present invention plans speed and obtains the real-time speed of the vehicle, avoiding the problem of driving at a fixed speed that does not meet the driving scenario, ensuring the timeliness of the planned path. In addition, various safety distances are calculated to improve safety. The overall solution simultaneously meets the requirements of feasibility, timeliness, and safety.

[0049] Example 2 Based on Example 1, Example 2 of the present invention further provides a local path planning system for an unmanned vehicle based on a feasible path, including: Data acquisition module, used to obtain road information, self-vehicle and other vehicle information; The passable area determination module is used to determine a static impassable area based on static obstacles and a three-stage dynamic impassable area based on dynamic obstacles. The first stage is the position where the dynamic obstacle stops at maximum deceleration, the second stage is the position reached by the dynamic obstacle at a constant speed within the same time, and the third stage is the position reached by the dynamic obstacle at maximum acceleration within the same time. The area formed between the rear end of the first stage and the front end of the third stage is the three-stage dynamic impassable area; the same time refers to the time when the dynamic obstacle stops at maximum deceleration. Within the boundary of the passable area, the static impassable area and the three-stage dynamic impassable area are removed to obtain the passable area of ​​the vehicle. Speed ​​planning module, used to plan the speed of the vehicle and obtain the real-time speed of the vehicle; Safety distance calculation module, used to calculate various safety distances during vehicle driving; The lane change decision module is used to make lane change decisions based on the information obtained from the data acquisition module to the safety distance calculation module, and decide whether to change lanes or stay in the current lane and follow the vehicle; The path planning module is used to obtain the planned path points and smooth the path to complete the path planning. The vehicle drives according to the currently planned path.

[0050] Specifically, the traffic area determination module is further configured to: S2.1. Obtain the boundaries of the traversable area by road edge, median strip or central divider; S2.2. Identify static and dynamic obstacles based on the speed of other vehicles. S2.3. Determine the static impassable area based on the shape of the static obstacle, which is equivalent to a rectangle parallel to the lane line; S2.4. Determine the three-stage dynamic impassable area based on the speed and acceleration / deceleration capabilities of the dynamic obstacle; S2.5. Within the boundary of the traversable area, remove the static impassable area and the three-stage dynamic impassable area. If the distance to the obstacle close to the boundary of the traversable area is less than the minimum safe distance, remove the area between the obstacles and the boundary of the traversable area to obtain the traversable area for the vehicle.

[0051] Specifically, the speed planning module is further used to: S3.1. Denormalize the curvature, equating the maximum curvature to 0 and the minimum curvature to 1. Set the default minimum ego vehicle speed V0 and the default maximum ego vehicle speed V0 + V1, where V1 is the difference between the default maximum ego vehicle speed and the default minimum ego vehicle speed. Given the maximum turning radius Lmax and the minimum turning radius Lmin, the real-time position curvature of the globally planned vectorized path is K, with the minimum curvature Kmin = 1 / Lmax and the maximum curvature Kmax = 1 / Lmin. Denormalize the curvature Knorm = (Kmax - K) / (Kmax - Kmin). S3.2. Initial default speed V = V0 + V1 for each position of the globally planned vectorized path Knorm; S3.3. Given the total length of the route L, calculate the default travel time T required to fully follow the default speed based on the initial default speed V. S3.4. Solve the real-time velocity VZ=β V, where β is the real-time speed multiplier and β={T (L-Lk)} / {L (T-Tk)}, Tk is the actual driving time, Lk is the actual driving distance, and the upper and lower limits of VZ are set to constrain it.

[0052] Specifically, the safety distance calculation module is further used to: S4.1. Calculate the safe distance LA between the front of the vehicle and the rear of the vehicle ahead in the lane based on the vehicle's real-time speed VZ and its maximum deceleration AZ2. , where T1 is the system decision time; based on the real-time speed VZ of the ego vehicle, the speed VC of the vehicle in front of the lane changing lane, and the maximum deceleration AC2 of the vehicle in front of the lane changing lane, the safe distance LC between the front of the ego vehicle and the rear of the vehicle in front of the lane changing lane is calculated. , where T2 is the lane changing time; based on the real-time speed VZ of the ego vehicle, the speed VD of the vehicle behind the lane changing lane, and the maximum acceleration AD1 of the vehicle behind the lane changing lane, the safe distance LD between the rear of the ego vehicle and the front of the vehicle behind the lane changing lane is calculated. ; S4.2. Calculate the safe distance perpendicular to the lane line: Based on the vehicle's real-time speed VZ and the speeds of other vehicles in the left and right lanes VX, where VX is the speed of the vehicle ahead of you in your lane VA, the speed of the vehicle behind you in your lane VB, the speed of the vehicle ahead of you in the lane change lane VC, or the speed of the vehicle behind you in the lane change lane VD, calculate the safe distance LS perpendicular to the lane line between your vehicle and other vehicles in the left and right lanes. , where K1 and K2 are the first time parameter and the second time parameter respectively.

[0053] More specifically, the lane change decision module is further configured to: S5.1. Detect whether there are other vehicles within the forward detection range LF and rearward detection range LR of the vehicle; S5.2. Obtain the distance SA between the ego vehicle and the vehicle A in front of the ego vehicle's lane, the distance SC between the ego vehicle and the vehicle C in front of the ego vehicle's lane-changing lane, and the distance SD between the ego vehicle and the vehicle D in the lane-changing lane behind the ego vehicle; S5.3. If SA > LF, then maintain the original lane speed, which is the planned real-time speed VZ. If there is a vehicle A in the lane ahead of you within the forward detection range LF, and LA < SA ≤ LF, then maintain the lane speed, which is the real-time speed VZ of the vehicle just after detecting the vehicle A in the lane ahead of you. If SA ≤ LA, then compare the real-time speed VZ of the vehicle with the speed VA of the vehicle in the lane ahead of you. If , then keep following the vehicle in the lane, and the speed is the speed of the vehicle in front of the vehicle in the lane VA, where K3 and K4 are the first proportional coefficient and the second proportional coefficient respectively, and K3 < 1, K4 > 1; if , then keep driving at the original speed of the lane, which is the planned real-time speed VZ; if , then prepare to change lanes; S5.4 Lane Change Direction Decision: Determine the width of the narrowest traversable area to the left and right of the vehicle and choose to change lanes to the wider lane. If the front-to-rear safety distance to the wider lane is insufficient, change lanes to the narrower lane. If the front-to-rear safety distance to the narrower lane is insufficient, maintain the lane and follow the vehicle. S5.5. If there are no other vehicles within the forward detection distance LF and the rearward detection distance LR of the lane change lane, that is, SC>LF and SD>LR, then the lane change will proceed at the original speed, which is the planned real-time speed VZ. If LC<SC≤LF and SD>LR, then the lane change will be accelerated, with the speed being K5. VZ, where K5 is the third proportional coefficient and K5>1; if LD<SD≤LR, and SC>LF, then change lanes and accelerate, with a speed of K6 VZ, where K6 is the fourth proportional coefficient and K6>1; if LC<SC≤LF, and LD<SD≤LR, then change lanes and accelerate, the speed is K7 VZ, where K7 is the fifth proportional coefficient and K7>1; if SC≤LC, or SD≤LD, then maintain the lane and follow the vehicle at the speed of the preceding vehicle VA; where K5<K6<K7.

[0054] More specifically, S5.2 includes: Define the direction parallel to the lane line as the X axis, the direction perpendicular to the lane line as the Y axis, and the geometric center of the starting position of the vehicle as the origin to calculate the distance between the vehicle and the vehicle in front of it. ,in, is the X-axis coordinate of the geometric center of the vehicle in front of the lane, is the X-axis coordinate of the vehicle’s geometric center, is the length of the vehicle ahead in the lane, is the length of the vehicle; the distance between the vehicle and the vehicle C in front of the lane where the vehicle changes lanes ,in, is the X-axis coordinate of the geometric center of the front vehicle in the lane change lane, is the length of the vehicle in front of the lane changing lane; the distance between the vehicle and the vehicle behind it in the lane changing lane D ,in, is the X-axis coordinate of the geometric center of the vehicle behind the lane change lane, is the length of the vehicle behind in the lane change.

[0055] More specifically, the path planning module is further configured to: S6.1. In the lane-changing scenario, if the speed of vehicle A in the ego vehicle lane is zero, the coordinates of two feature points are obtained in the passable area, namely (X2, Y2) and (X3, Y3), where X2 is the X-axis coordinate of the geometric center of the static impassable area in the ego vehicle lane, and Y2 is the Y-axis coordinate of the geometric center of the static impassable area in the ego vehicle lane and the lane-changing direction. of and, is the vehicle width, is the width of the vehicle ahead in the ego vehicle lane; X3 is the X-axis coordinate of the front position of vehicle A in the ego vehicle lane in the static impassable area; Y3 is the Y-axis coordinate of the center position of the lane change lane; S6.2. In the lane-changing scenario, if the speed of vehicle A in the ego vehicle's lane is non-zero, the coordinates of three feature points are obtained within the passable area, namely (X1', Y1'), (X2', Y2'), and (X3', Y3'), where X1' is the X-axis coordinate of the rear position of vehicle A in the ego vehicle's lane in the first segment of the three-segment dynamic impassable area, and Y1' is the Y-axis coordinate of the geometric center position of vehicle A in the ego vehicle's lane in the three-segment dynamic impassable area and the lane-changing direction. X2' is the X-axis coordinate of the geometric center position of the vehicle A in front of the vehicle lane in the second section of the three-section dynamic impassable area, and Y2' is the Y-axis coordinate of the geometric center position of the vehicle A in front of the vehicle lane in the three-section dynamic impassable area and the lane change direction. X3' is the X-axis coordinate of the front position of vehicle A in the third segment of the three-segment dynamic impassable area in the ego vehicle lane, and Y3' is the Y-axis coordinate of the center position of the lane change lane; S6.3. Use cubic spline interpolation to make the feature points continuous, obtain the local path trajectory, and complete the path planning.

[0056] Specifically, the system is executed once every 10ms according to the complete process from the data acquisition module to the path planning module to plan the local path trajectory under the current environmental state.

[0057] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present invention.

Claims

1. A local path planning method for unmanned vehicles based on feasible paths, characterized in that: include: S1. Obtain road information, self-vehicle information, and other vehicle information; S2. Determine a static impassable area based on static obstacles and a three-segment dynamic impassable area based on dynamic obstacles. The first segment is the position where the dynamic obstacle stops at maximum deceleration, the second segment is the position reached by the dynamic obstacle at a constant speed within the same time, and the third segment is the position reached by the dynamic obstacle at maximum acceleration within the same time. The area formed by the rear of the vehicle in the first segment and the front of the vehicle in the third segment is the three-segment dynamic impassable area. The same time refers to the time when the dynamic obstacle stops at maximum deceleration. Within the boundary of the traversable area, remove the static impassable area and the three-segment dynamic impassable area to obtain the traversable area for the vehicle. S3. Plan the speed of the vehicle and obtain the real-time speed of the vehicle; S4. Calculate various safety distances during vehicle driving; S5: Based on the information obtained in S1-S4, the driver makes a lane change decision, deciding whether to change lanes or stay in the current lane and follow the vehicle. S6. Obtain the planned path points and smooth the path to complete the path planning. The vehicle drives according to the currently planned path.

2. The method for local path planning of an unmanned vehicle based on a feasible path according to claim 1, wherein S2 include: S2.

1. Obtain the boundaries of the traversable area by road edge, median strip or central divider; S2.

2. Identify static and dynamic obstacles based on the speed of other vehicles. S2.

3. Determine the static impassable area based on the shape of the static obstacle, which is equivalent to a rectangle parallel to the lane line; S2.

4. Determine the three-stage dynamic impassable area based on the speed and acceleration / deceleration capabilities of the dynamic obstacle; S2.

5. Within the boundary of the traversable area, remove the static impassable area and the three-stage dynamic impassable area. If the distance to the obstacle close to the boundary of the traversable area is less than the minimum safe distance, remove the area between the obstacles and the boundary of the traversable area to obtain the traversable area for the vehicle.

3. The method for local path planning of an unmanned vehicle based on a feasible path according to claim 1, characterized in that: S3 includes: S3.

1. Denormalize the curvature, equating the maximum curvature to 0 and the minimum curvature to 1. Set the default minimum ego vehicle speed V0 and the default maximum ego vehicle speed V0 + V1, where V1 is the difference between the default maximum ego vehicle speed and the default minimum ego vehicle speed. Given the maximum turning radius Lmax and the minimum turning radius Lmin, the real-time position curvature of the globally planned vectorized path is K, with the minimum curvature Kmin = 1 / Lmax and the maximum curvature Kmax = 1 / Lmin. Denormalize the curvature Knorm = (Kmax - K) / (Kmax - Kmin). S3.

2. Initial default speed V = V0 + V1 for each position of the globally planned vectorized path Knorm; S3.

3. Given the total length of the route L, calculate the default travel time T required to fully follow the default speed based on the initial default speed V. S3.

4. Solve the real-time velocity VZ=β V, where β is the real-time speed multiplier and β={T (L-Lk)} / {L (T-Tk)}, Tk is the actual driving time, Lk is the actual driving distance, and the upper and lower limits of VZ are set to constrain it.

4. The method for local path planning of an unmanned vehicle based on a feasible path according to claim 1, wherein S4 include: S4.

1. Calculate the safe distance LA between the front of the vehicle and the rear of the vehicle ahead in the lane based on the vehicle's real-time speed VZ and its maximum deceleration AZ2. , where T1 is the system decision time; based on the real-time speed VZ of the ego vehicle, the speed VC of the vehicle in front of the lane changing lane, and the maximum deceleration AC2 of the vehicle in front of the lane changing lane, the safe distance LC between the front of the ego vehicle and the rear of the vehicle in front of the lane changing lane is calculated. , where T2 is the lane changing time; based on the real-time speed VZ of the ego vehicle, the speed VD of the vehicle behind the lane changing lane, and the maximum acceleration AD1 of the vehicle behind the lane changing lane, the safe distance LD between the rear of the ego vehicle and the front of the vehicle behind the lane changing lane is calculated. ; S4.

2. Calculate the safe distance perpendicular to the lane line: Based on the vehicle's real-time speed VZ and the speeds of other vehicles in the left and right lanes VX, where VX is the speed of the vehicle ahead of you in your lane VA, the speed of the vehicle behind you in your lane VB, the speed of the vehicle ahead of you in the lane change lane VC, or the speed of the vehicle behind you in the lane change lane VD, calculate the safe distance LS perpendicular to the lane line between your vehicle and other vehicles in the left and right lanes. , where K1 and K2 are the first time parameter and the second time parameter respectively.

5. The method for local path planning of an unmanned vehicle based on a feasible path according to claim 4, wherein S5 include: S5.

1. Detect whether there are other vehicles within the forward detection range LF and rearward detection range LR of the vehicle; S5.

2. Obtain the distance SA between the ego vehicle and the vehicle A in front of the ego vehicle's lane, the distance SC between the ego vehicle and the vehicle C in front of the ego vehicle's lane-changing lane, and the distance SD between the ego vehicle and the vehicle D in the lane-changing lane behind the ego vehicle; S5.

3. If SA > LF, then maintain the original lane speed, which is the planned real-time speed VZ. If there is a vehicle A in the lane ahead of you within the forward detection range LF, and LA < SA ≤ LF, then maintain the lane speed, which is the real-time speed VZ of the vehicle just after detecting the vehicle A in the lane ahead of you. If SA ≤ LA, then compare the real-time speed VZ of the vehicle with the speed VA of the vehicle in the lane ahead of you. If , then keep following the vehicle in the lane, and the speed is the speed of the vehicle in front of the vehicle in the lane VA, where K3 and K4 are the first proportional coefficient and the second proportional coefficient respectively, and K3 < 1, K4 > 1; if , then keep driving at the original speed of the lane, which is the planned real-time speed VZ; if , then prepare to change lanes; S5.4 Lane Change Direction Decision: Determine the width of the narrowest traversable area to the left and right of the vehicle and choose to change lanes to the wider lane. If the front-to-rear safety distance to the wider lane is insufficient, change lanes to the narrower lane. If the front-to-rear safety distance to the narrower lane is insufficient, maintain the lane and follow the vehicle. S5.

5. If there are no other vehicles within the forward detection distance LF and the rearward detection distance LR of the lane change lane, that is, SC>LF and SD>LR, then the lane change will proceed at the original speed, which is the planned real-time speed VZ. If LC<SC≤LF and SD>LR, then the lane change will be accelerated, with the speed being K5. VZ, where K5 is the third proportional coefficient and K5>1; if LD<SD≤LR, and SC>LF, then change lanes and accelerate, with a speed of K6 VZ, where K6 is the fourth proportional coefficient and K6>1; if LC<SC≤LF, and LD<SD≤LR, then change lanes and accelerate, the speed is K7 VZ, where K7 is the fifth proportional coefficient and K7>1; if SC≤LC, or SD≤LD, then maintain the lane and follow the vehicle at the speed of the preceding vehicle VA; where K5<K6<K7.

6. The method for local path planning of an unmanned vehicle based on a feasible path according to claim 5, characterized in that: S5.2 includes: Define the direction parallel to the lane line as the X axis, the direction perpendicular to the lane line as the Y axis, and the geometric center of the starting position of the vehicle as the origin to calculate the distance between the vehicle and the vehicle in front of it. ,in, is the X-axis coordinate of the geometric center of the vehicle in front of the lane, is the X-axis coordinate of the vehicle’s geometric center, is the length of the vehicle ahead in the lane, is the length of the vehicle; the distance between the vehicle and the vehicle C in front of the lane where the vehicle changes lanes ,in, is the X-axis coordinate of the geometric center of the front vehicle in the lane change lane, is the length of the vehicle in front of the lane changing lane; the distance between the vehicle and the vehicle behind it in the lane changing lane D ,in, is the X-axis coordinate of the geometric center of the vehicle behind the lane change lane, is the length of the vehicle behind in the lane change.

7. The method for local path planning of an unmanned vehicle based on a feasible path according to claim 5, wherein S6 include: S6.

1. In the lane-changing scenario, if the speed of vehicle A in the ego vehicle lane is zero, the coordinates of two feature points are obtained in the passable area, namely (X2, Y2) and (X3, Y3), where X2 is the X-axis coordinate of the geometric center of the static impassable area in the ego vehicle lane, and Y2 is the Y-axis coordinate of the geometric center of the static impassable area in the ego vehicle lane and the lane-changing direction. of and, is the vehicle width, is the width of the vehicle ahead in the ego vehicle lane; X3 is the X-axis coordinate of the front position of vehicle A in the ego vehicle lane in the static impassable area; Y3 is the Y-axis coordinate of the center position of the lane change lane; S6.

2. In the lane-changing scenario, if the speed of vehicle A in the ego vehicle's lane is non-zero, the coordinates of three feature points are obtained within the passable area, namely (X1', Y1'), (X2', Y2'), and (X3', Y3'), where X1' is the X-axis coordinate of the rear position of vehicle A in the ego vehicle's lane in the first segment of the three-segment dynamic impassable area, and Y1' is the Y-axis coordinate of the geometric center position of vehicle A in the ego vehicle's lane in the three-segment dynamic impassable area and the lane-changing direction. X2' is the X-axis coordinate of the geometric center position of the vehicle A in front of the vehicle lane in the second section of the three-section dynamic impassable area, and Y2' is the Y-axis coordinate of the geometric center position of the vehicle A in front of the vehicle lane in the three-section dynamic impassable area and the lane change direction. X3' is the X-axis coordinate of the front position of vehicle A in the third segment of the three-segment dynamic impassable area in the ego vehicle lane, and Y3' is the Y-axis coordinate of the center position of the lane change lane; S6.

3. Use cubic spline interpolation to make the feature points continuous, obtain the local path trajectory, and complete the path planning.

8. The method for local path planning of an unmanned vehicle based on a feasible path according to claim 1, characterized in that: This method is executed once every 10ms according to the complete process of S1-S6 to plan the local path trajectory under the current environmental state.

9. The local path planning system for unmanned vehicles based on feasible paths is characterized by: include: Data acquisition module, used to obtain road information, self-vehicle and other vehicle information; The passable area determination module is used to determine a static impassable area based on static obstacles and a three-stage dynamic impassable area based on dynamic obstacles. The first stage is the position where the dynamic obstacle stops at maximum deceleration, the second stage is the position reached by the dynamic obstacle at a constant speed within the same time, and the third stage is the position reached by the dynamic obstacle at maximum acceleration within the same time. The area formed between the rear end of the first stage and the front end of the third stage is the three-stage dynamic impassable area; the same time refers to the time when the dynamic obstacle stops at maximum deceleration. Within the boundary of the passable area, the static impassable area and the three-stage dynamic impassable area are removed to obtain the passable area of ​​the vehicle. Speed ​​planning module, used to plan the speed of the vehicle and obtain the real-time speed of the vehicle; Safety distance calculation module, used to calculate various safety distances during vehicle driving; The lane change decision module is used to make lane change decisions based on the information obtained from the data acquisition module to the safety distance calculation module, and decide whether to change lanes or stay in the current lane and follow the vehicle; The path planning module is used to obtain the planned path points and smooth the path to complete the path planning. The vehicle drives according to the currently planned path.

10. The local path planning system for unmanned vehicles based on feasible paths according to claim 9, characterized in that: The traffic area determination module is further configured to: S2.

1. Obtain the boundaries of the traversable area by road edge, median strip or central divider; S2.

2. Identify static and dynamic obstacles based on the speed of other vehicles. S2.

3. Determine the static impassable area based on the shape of the static obstacle, which is equivalent to a rectangle parallel to the lane line; S2.

4. Determine the three-stage dynamic impassable area based on the speed and acceleration / deceleration capabilities of the dynamic obstacle; S2.

5. Within the boundary of the traversable area, remove the static impassable area and the three-stage dynamic impassable area. If the distance to the obstacle close to the boundary of the traversable area is less than the minimum safe distance, remove the area between the obstacles and the boundary of the traversable area to obtain the traversable area for the vehicle.

Images