CN111289008B - A local path planning method for unmanned vehicles - Google Patents

Abstract

The invention discloses a local path planning method for an unmanned vehicle, which comprises the steps of inputting parameters required by local path planning; selecting a preview point; planning a track based on a road coordinate system to generate a basic track; matching the basic track with the road shape to generate an alternative track; calculating a speed limit value based on the alternative track and a corresponding speed limit position; the alternative tracks are optimized to obtain tracks executed by the unmanned vehicle, and the tracks are called as optimized tracks; and planning the speed of the unmanned vehicle based on the preferred track. The local path planning method avoids planning failure in a complex road, simplifies the planning of the local path in the complex environment and approaches to the driving behavior of people. The local path planning method can effectively process the influence of static obstacles, dynamic objects, traffic lights, speed limit belts and various traffic facilities on the unmanned vehicle in real time in a complex urban road environment.

Description

Local path planning method for unmanned vehicle

Technical Field

The invention belongs to the technical field of unmanned path planning, and particularly relates to a local path planning method for an unmanned vehicle.

Background

Unmanned vehicles are a class of vehicles that can autonomously accomplish driving tasks in complex environments on the road. The unmanned control is mainly divided into the following parts: 1) the environment perception module perceives and understands external environment information such as static and dynamic obstacles such as vehicles and pedestrians, lane lines, traffic lights, traffic signs and the like on the road through sensors such as a camera, a laser radar, a millimeter wave radar and the like. 2) And the positioning module carries out high-precision positioning through satellite navigation, inertial navigation, SLAM and the like to obtain the position, the speed, the course, the running curvature and the like of the vehicle. 3) And the high-precision map provides all information on the road for unmanned driving. 4) The global path planning module generally calculates a global reference path by using information such as a global task file and a global map (e.g., global road network information), and the global task planning module outputs the global reference path in the form of a series of coordinate points. Due to the large spatial and temporal scale involved in global path planning, planning can be performed in an off-line manner, and periodic re-planning is not required. 5) Planning a local path; and finishing fine planning according to the global path planning. 6) And the module is used for deciding and controlling, and finally sending a control instruction to the executing mechanism to realize transverse and longitudinal control so as to finish the autonomous driving task. The birth and development of the unmanned automobile benefit from the rapid development of various disciplines such as artificial intelligence, pattern recognition, computer vision, control theory and the like, and the development is the perfect combination of the latest research results in the fields and the modern automobile industry. The unmanned technology has wide application prospect in the fields of national defense and national economy.

The local path planning is one of key technologies for controlling the motion of the unmanned vehicle, and calculates a path or a track to be executed by the vehicle in a future limited time domain by using the guide information of a global reference path or a global coordinate point and combining with the local environment perception information updated in real time. Since the external environment is dynamically changed and it is difficult for the control to accurately track the planning result, the local motion planning is periodically operated, and the calculation period is directly related to the update frequency of the perception information. The local path planning has different planning strategies according to different complexity of path planning problems, and can be roughly divided into two types: one is a local path planning method based on direct trajectory generation; the other type is a local path planning method for graph search. The former mainly aims at processing complex system model constraints and ensuring the performability of a planning result, and the latter mainly aims at dealing with complex barrier environments and converting a planning problem into a sequence decision problem. Since the latter is very computationally intensive and is used in special cases, direct trajectory planning is mostly used. The local path planning method for direct path generation is divided into a space-domain path planning and a time-domain speed planning. The trajectory planning is to plan the trajectory directly in cartesian coordinates.

Because urban roads are complex, structured roads and unstructured roads including paths have different shapes. The conventional direct-acting trajectory generation method is limited. According to the track planned by the starting point and the end point parameters, due to the fact that the road shape is complex, when the planned track point is complex in road shape, the planned track point is possibly outside the feasible road, and planning failure is caused. Also, the first problem to be considered in local path planning is obstacle avoidance, including static obstacle (e.g., curb, post, stationary pedestrian, vehicle, etc.) avoidance and dynamic obstacle (i.e., other traffic participants, e.g., vehicle, bicycle, pedestrian, etc.) avoidance. Since the static obstacle position is independent of time, it is only necessary to ensure that the planned path does not collide spatially with it. But if there are dynamic obstacles in the environment, both temporal and spatial factors need to be considered to ensure the safety of generating the trajectory. Therefore, the optimal trajectory and speed plan can be obtained only by predicting the trajectory of the dynamic obstacle and sampling the speed on planning a plurality of trajectories to perform dynamic collision detection, and the method has the defects of complexity and high calculation requirement.

Disclosure of Invention

The invention aims to provide a local path planning method for an unmanned vehicle, which can realize track generation in complex urban roads and unmanned safe operation in complex dynamic urban environments.

In order to realize the purpose, the invention adopts the following technical scheme:

a method of local path planning for an unmanned vehicle, comprising the steps of:

1) inputting parameters required by local path planning;

2) selecting a preview point;

3) planning a track based on a road coordinate system to generate a basic track;

4) matching the basic track with the road shape to generate an alternative track;

5) calculating a speed limit value based on the alternative track and a corresponding speed limit position;

6) the alternative tracks are optimized to obtain tracks executed by the unmanned vehicle, and the tracks are called as optimized tracks;

7) the speed of the unmanned vehicle is planned based on the preferred trajectory.

The step 3) performs track planning based on the road coordinate system, and the process of generating the basic track is as follows:

the basic track is a plurality of tracks which are possibly driven by the unmanned vehicle in the future according to different preview points, and N candidate tracks exist at N preview points. The basic track is obtained by solving an equation according to the initial parameters of the vehicle and the destination parameters of the preview in a road coordinate system, and the basic track equation is expressed as follows in the road coordinate system:

l_j(s)＝c_0j+c_1js+c_2js²+c_3js³+c_4js⁴+c_5js⁵ (1)

wherein s is the coordinate value of the horizontal axis of the basic track j in the road coordinate system, and l_j(s) is the ordinate of the base track j in the road coordinate system, which is a function of the abscissa of the base track in the road coordinate system; the subscript j denotes the index of the alternate track, j ═ 1, 2,. N; n is the total number of the alternative tracks. c. C_0j、c_1j、c_2j、c_3j、c_4j、c_5jThe unknown parameters of the curve equation are obtained by solving the following equation system:

wherein S is_c、l_cCoordinates of the unmanned vehicle at the current moment in the road coordinate system,

S_fj、l_fjcoordinates of the jth basic track end point in a road coordinate system,

tanθ_fjis the tangent value of the terminal course angle of the jth basic track_fj＝0，

tanθ_cIs the initial heading tangent, θ, of the unmanned vehicle_cThe positioning information is obtained by the positioning module,

κ_ccurvature of a driving track for the current position of the unmanned vehicle, related to steering wheel angle, is obtained by a positioning module, kappa_fjCurvature of the end point of the jth base trajectory,. kappa_fj＝0。

The step 4) of matching the basic track with the road shape and generating the alternative track comprises the following steps:

matching the basic track with the road shape under a road coordinate system to obtain an alternative track; the coordinates of the corresponding planning points on the alternative tracks in the Cartesian coordinate system are expressed by the following formula:

wherein S is_biFor the abscissa, theta, of the global path planning point in the road coordinate_biA point course angle is planned for the global path; x_biAnd Y_biFor the abscissa and ordinate of the global path planning point in cartesian coordinates, the subscript j denotes the number of candidate trajectories, j is 1, 2,. N, N is the total number of candidate trajectories, b denotes the global planning point, i denotes the global path planning point number, i is 1, 2,. Nb, and Nb is the total number of planning points.

The step 5) of calculating the speed limit value based on the alternative track and the corresponding speed limit position comprises the following steps:

the speed limit of the unmanned vehicle on the alternative track is divided into the following cases:

51) speed limit and speed limit position of the unmanned vehicle by road conditions:

the speed limit of the road condition to the unmanned vehicle is obtained according to the road curvature and the map, the speed limit of the road condition to the vehicle is obtained, and the global path planning point S is obtained_biPoint (S) on the corresponding candidate trajectory_bi，l_j(S_bi) Speed limit is calculated as follows:

if the speed limit V_1limjiSpeed limit V greater than road condition requirement_RlimThen V is_1limji＝V_Rlim(ii) a The speed-limiting position of the road condition corresponding to each alternative track j is recorded as:

S_1limji＝S_bi (5)

wherein, V_RlimThe speed of the vehicle is limited for the road condition,

S_bifor global path planning point inThe abscissa under the road coordinate is the coordinate of,

κ_jito be the curvature corresponding to the global routing point i on the alternative trajectory,

the index j is the alternate track number, the index i is the corresponding global path plan point number,

ACC_lmaxthe maximum lateral acceleration required for comfort is determined by the comfort level of the person.

52) Speed limit and speed limit position of unmanned vehicle by static barrier and dynamic barrier

521) Obtaining the position coordinates A of the static obstacle and the dynamic obstacle under a road coordinate system through the measurement of a sensor arranged on the unmanned vehicle_k(S_ak，l_ak) Width W_akVelocity V_akThe like;

522) calculating safe distance of unmanned vehicle to static obstacle and dynamic obstacle

Longitudinal safe distance calculation formula:

D_Sak＝k_cV_c+ΔD_Sak (6)

wherein, V_CIs the speed of the unmanned vehicle, k_cIs a coefficient, empirically determined, Δ D_SakIs a parameter related to the type of obstacle.

The transverse safe distance calculation formula is as follows:

wherein, W_akFor static or dynamic obstacle width, type-dependent, Δ D_lReserving a minimum transverse safety distance, W, for fixation_CThe width of the unmanned vehicle.

523) Static obstacle and dynamic obstacle speed limit V_2limkAnd corresponding speed limit position S_2k

5231) Calculating the speed limit of a static obstacle and the corresponding speed limit position

The speed limit of the static obstacle k to the alternative track j is calculated by adopting the following formula:

the horizontal coordinate of the lower speed limiting position in the road coordinate system is as follows:

S_2limjk＝S_ak-D_Sak (9)

wherein D is_lmaxFor the maximum lateral safety distance to be reached,

V_Rlimthe speed of the unmanned vehicle is limited for road conditions,

Δl_akjthe distance of the static obstacle k from the second alternative planned trajectory j.

5232) Calculating speed limit of dynamic barrier and corresponding speed limit position

Velocity V of dynamic obstacle_akThe decomposition is (V) under the road coordinate system_Sak，V_lak)，V_SakIs the longitudinal speed of the obstacle k along the road; v_lSakIs the lateral speed of the obstacle k along the road.

52321) The first condition is as follows: if V_lSak＝0

The speed limit of the unmanned vehicle on the alternative track j by the dynamic obstacle k is as follows:

the lateral coordinate of the speed limiting position of the dynamic barrier k on the unmanned vehicle is under a road coordinate system:

S_2limjk＝S_ak-D_Sak (11)

wherein D is_lmaxFor the maximum lateral safety distance to be reached,

V_Rlimto limit the speed of the unmanned vehicle for road conditions.

52322) Case two: V_lSakWhen the speed is not equal to 0, the step of calculating the speed limit of the dynamic barrier k to the unmanned vehicle is as follows:

523221) whether the dynamic obstacle is approaching the unmanned vehicle or leaving the candidate trajectory j:

Δl_akjrepresenting the distance of the dynamic obstacle k from the second alternative planned trajectory j;

Δl_akj> 0 indicates that the dynamic obstacle k is to the left of the alternative trajectory,

at Δ l_akjAt > 0:

V_lSak> 0 denotes that the dynamic obstacle k leaves the candidate trajectory j

V_lSak< 0 indicates that the dynamic obstacle k approaches the candidate trajectory j.

Δl_akj< 0 indicates that the dynamic obstacle k is to the right of the alternative trajectory;

at Δ l_akjWhen < 0:

V_lSak< 0 indicates that the dynamic obstacle k leaves the alternative trajectory j,

V_lSaka > 0 indicates that the dynamic obstacle k approaches the candidate trajectory j.

The dynamic obstacle k leaving the candidate trajectory j does not cause a hindrance to the unmanned vehicle.

523222) when the dynamic obstacle k approaches the candidate trajectory j, judging whether the unmanned vehicle is obstructed: estimating the time when the dynamic obstacle k enters the alternative track j:

and (3) estimating the position of the dynamic obstacle k entering the alternative track j:

S_jakin＝S_ak+V_Sakt_jakin (13)

the time when the unmanned vehicle drives to the position where the dynamic obstacle k enters the alternative track j:

Δt_akinfor redundant time, determined based on the type of dynamic obstacle and the moving speed,

when t is_jckin＜t_jakinThe vehicle can exceed the dynamic barrier and cannot block the unmanned vehicle;

when t is_jckin≥t_jakinIt may cause a hindrance to the vehicle.

523223) calculating the speed limit and speed limit position of the unmanned vehicle when the lateral speed of the dynamic obstacle is not zero

For dynamic obstacles that do not obstruct or restrict the unmanned vehicle, no calculation is needed.

For a dynamic obstacle that obstructs the unmanned vehicle, the speed limit of the dynamic obstacle k to the candidate trajectory j is:

V_2limjk＝0 (15)

speed limiting position:

S_2limjk＝S_ak-D_Sak (16)。

53) calculating the speed limit and the speed limit position of the traffic signal lamp on the unmanned vehicle

The abscissa of the position of the vehicle stop line at the intersection of the traffic signal lamps under the road coordinate system is S_stopCurrent vehicle speed V_c

531) The first condition is as follows: the current is green light or yellow light and the time for the unmanned vehicle to reach the intersection is less than 1s

Then: speed limit V of alternative trajectory j_3limjLimiting the speed of the intersection.

The speed limit position is a stop line S_3limj＝S_stop (17)

532) Case two: the current is red light or the current is yellow light, and the time for the unmanned vehicle to reach the intersection is more than 1s

Then: speed limit V of alternative trajectory j_3limj＝0(18)

The speed limit position is a stop line S_3limj＝S_stop (19)。

54) Calculating the speed limit required by the road traffic management department and the speed limit position

The road traffic management department requires speed limit, including crossing, speed bump, special position and other traffic safety requirements of local speed limit and corresponding speed limit position are obtained from map.

The abscissa of the speed-limiting position on the road coordinate system is marked as S_4limjmThe corresponding speed limit is marked as V_4limjmThe subscript m is the number of locations in the alternative trajectory that must be slowed down as required by the road traffic authority.

And (3) limiting the speed corresponding to the speed limiting position of each candidate track j:

at the speed-limiting position S_{1lim ji}The speed limit is V_1limji，

At the speed-limiting position S_21limjkThe speed limit is V_2limjk，

At the speed-limiting position S_3limjThe speed limit is V_3limj，

At the speed-limiting position S_4limjmThe speed limit is V_4limjm。

Arranging all speed limit points on the j tracks from near to far according to the distance from the unmanned vehicle to recombine the sequence (S)_limjn，V_limjn) The subscript n represents the serial number of the speed limiting total sequence of the alternative track j, and n is 1, 2. And the speed limit of the speed limit position is processed according to the following method:

under the condition that multiple speed limit constraints exist at the same position, calculating by using a minimum speed limit value;

and secondly, if the speed limit value of the planning point at the same alternative track position with a longer distance is greater than the speed limit value of the previous point, the speed limit value is the speed limit value of the previous point.

The step 6) is to optimize the alternative tracks, and the process of obtaining the track executed by the unmanned vehicle is as follows:

and the candidate track corresponding to the minimum value calculated according to the optimization objective function calculated by each candidate track is the track executed by the last unmanned vehicle.

The calculation formula of the optimization objective function is as follows:

wherein, ω is_rThe subscript r represents index serial number, r is 1, 2,. M, MThe total index number is; j is an alternative track, j is 1, 2.. N, and N is the total number of the alternative tracks; .

First index J₁(j) For the track length index, the following is calculated:

wherein S is_j(t0) is the farthest track length that can be traveled according to the speed limit within the alternative track j within a period of time t 0.

Wherein S is_maxFor the journey calculated at the maximum speed limit at time t0,

S_minthe distance calculated at the lowest speed limit at time t 0.

Second index J₂(j) For the curvature term index, the following is calculated:

wherein:

S_ffor the abscissa of the end point of the alternative trajectory j in the road coordinate system,

S_Cfor the unmanned vehicle to be on the abscissa under the road coordinates,

k_j(s) curvature at the candidate trajectory j coordinate point s,

κ_maxthe maximum curvature of travel of the vehicle.

Third index J₃(j) For the trajectory deviation planned path index, the calculation formula is as follows:

wherein:

l_j(s) is the ordinate of the current alternative track j in the road coordinate system

l_maxThe longitudinal coordinate of the pre-aiming point with the maximum longitudinal coordinate in the road coordinate system is taken as the longitudinal coordinate of all the pre-aiming points,

fourth index J₄(j) For the consistency index before and after the alternative track, the calculation formula is as follows:

wherein:

l_*(s) is the ordinate of the last local planned trajectory preview point in the road coordinate system,

l_j(s) is the ordinate of the preview point of the candidate track j in the road coordinate system,

l is the road width.

Further, the process of inputting the parameters required by the local path planning in step 1) is as follows:

11) firstly, taking the road length direction as a coordinate horizontal axis S axis, and taking the road length extending direction as a positive direction; taking the direction vertical to the road as the axis of longitudinal axis l, and the left side as the positive direction; the coordinates of the starting point of the unmanned vehicle are set to(s)_I，l_I，θ_I) Setting the coordinate of the local path planning track terminal point as(s)_f，l_f，θ_f) Wherein, theta_IAnd theta_fPlanning a track end course angle, S, for the unmanned vehicle starting point and the local path_IAnd S_fPlanning an abscissa value, l, of a trajectory end point in a road coordinate system for a starting point and a local path of the unmanned vehicle_IAnd l_fAnd planning a longitudinal coordinate value of the track terminal point under a road coordinate system for the starting point and the local path of the unmanned vehicle.

12) Then, inputting parameters required by local path planning:

121) the global path planning module inputs parameters to the local path planning module:

under a series of planned points on the global planned path in Cartesian coordinatesCoordinate B of_i(X_bi，Y_bi) Course angle theta of planning point_biPlanning point road curvature kappa_biPlanning point speed limit V_biWherein b represents a global planning point; i denotes a global planning point index, i 1, 2,. Nb, Nb being the total number of planning points.

122) The environment perception module inputs parameters to the local path planning module:

the output of the environment perception module is the position A of the static obstacle and the dynamic obstacle under the road coordinate system_k(S_ak，l_ak) Width W_akVelocity V_akAnd type, etc., where k denotes the kth static obstacle or dynamic obstacle, and k 1, 2.

123) The positioning module inputs parameters to the local path planning module: position coordinates, speed, heading, etc. of the unmanned vehicle.

124) Inputting vehicle body parameters to the local path planning module: steering wheel angle, vehicle track, etc.

The sequence of steps 121) to 124) can be arbitrarily adjusted.

Further, the process of selecting the preview point in the step 2) is as follows:

firstly, a pre-aiming section is determined, the pre-aiming section is a section perpendicular to a planned path according to a global path planning point, and the intersection point of the section and a road surface is a line segment. And selecting a point of the travelable area on the line segment as a preview point. And selecting the distance of the pre-aiming section according to the speed. The preview point is selected on a line segment determined by the preview section according to a certain interval distance. The course, curvature and speed limit of the preview point are the same as the global path planning point on the preview section. The coordinate of the preview point in the road coordinate system is represented as: (s)_fy，l_fy，θ_fy) The subscript f represents a point on the preview cross section, y represents a preview point number, and if N preview points are selected, y is 1, 2,. N.

Further, the step 7) of planning the speed of the unmanned vehicle comprises the following steps:

the planning of the speed of the unmanned vehicle is performed on the preferred trajectory already obtained in the preceding step 6).

The speed of the unmanned vehicle is planned according to the following method:

71) selecting the point position S with the minimum speed limit value on the optimized track_vminSpeed limit note V_min；

72) And planning a speed curve from the current unmanned vehicle to the point according to the point position and the speed limit, wherein the curve is constructed according to a trapezoidal curve. Where the abscissa is time and the ordinate is speed. The trapezoidal curve is divided into an acceleration section, a stable section and a deceleration section.

V_maxlimFor unmanned vehicles to a lowest speed limit position S_vminThe speed of the inner maximum speed limit is limited,

V_Cfor the current speed of the unmanned vehicle,

V_fspeed limit V which is a speed limit position on an optimized track_min，

ACC_SThe acceleration of the acceleration section is determined according to the comfort degree during the normal running of the vehicle,

DEC_Sthe deceleration of the deceleration section is determined according to the comfort degree during the normal running of the vehicle,

t_steadyis the plateau time, which cannot be too short and is greater than a certain value.

Given the above parameters, the plateau speed cannot exceed V_maxlimA speed profile is established.

73) The speed limit of the upper speed limit position of the optimized track is verified by using the speed curve, and if all the speed limits are met, the function curve is a planned function curve; otherwise, steps 71), 72) and 73) are repeated with the previous speed limit position until all are satisfied.

The invention provides a local path planning method for an unmanned vehicle. The local path planning method simultaneously considers the vehicle kinematic constraint and the geometric constraint of the road coordinates, ensures the performability of the path planning result and the consistency with the road geometric shape, and avoids the situation that the direct planning exceeds the road on a special road.

According to the technical scheme, all obstacles in the environment, including static obstacles (such as curbs, piles, static pedestrians, vehicles and the like) obstacle avoidance and dynamic object (other traffic participants such as vehicles, bicycles, pedestrians and the like) obstacle avoidance, traffic signal lamps, speed reduction belts and the like, are treated as the speed limit of a certain position on the alternative track in the same way as the road speed limit, and then the track is optimized and the speed is planned according to all the speed limits of the alternative track. Collision detection is not needed, a large amount of speed sampling is avoided, the method is suitable for all road conditions, and the calculation pressure is reduced.

The time domain speed planning method of the technical scheme of the invention provides a multipoint speed planning scheme from the aspects of safety and comfort, and the scheme carries out speed planning according to the speed limit of each speed limit position on an optimal track. And then finding out a speed planning scheme suitable for all speed limits according to all speed plans, namely the final executed speed planning scheme. A trapezoidal speed planning method is adopted, and speed limit planning is performed for each speed limit position.

The local path planning method for the unmanned vehicle avoids planning failure in a complex road, simplifies the local path planning in a complex environment and approaches the driving behavior of people. The local path planning method can effectively process the influence of static obstacles, dynamic objects, traffic lights, speed limit belts and various traffic facilities on the unmanned vehicle in real time in a complex urban road environment.

Drawings

FIG. 1 is a block flow diagram of a method of local path planning for an unmanned vehicle according to the present invention;

FIG. 2 is a schematic diagram illustrating a process of defining road coordinates according to the present invention;

fig. 3 is a schematic diagram of a trapezoidal plan in the speed plan of the present invention.

Detailed Description

The following describes a local path planning method for an unmanned vehicle according to the present invention in detail with reference to the accompanying drawings. In the description of the present invention, it is to be understood that the terms "left side", "right side", "upper", "lower", "bottom", etc., indicate orientations or positional relationships based on those shown in the drawings, and are only for convenience of describing the present invention and simplifying the description, but do not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, "first", "second", etc., do not represent an important degree of the component parts, and thus are not to be construed as limiting the present invention. The specific dimensions used in the present example are only for illustrating the technical solution and do not limit the scope of protection of the present invention.

As shown in fig. 1, a local path planning method for an unmanned vehicle comprises the steps of:

8) inputting parameters required by local path planning;

9) selecting a preview point;

10) planning a track based on a road coordinate system to generate a basic track;

11) matching the basic track with the road shape to generate an alternative track;

12) calculating a speed limit value based on the alternative track and a corresponding speed limit position;

13) the alternative tracks are optimized to obtain tracks executed by the unmanned vehicle, and the tracks are called as optimized tracks;

14) the speed of the unmanned vehicle is planned based on the preferred trajectory.

Specifically, as shown in fig. 2, the process of inputting parameters required for local path planning in step 1) is as follows:

11) firstly, taking the road length direction as a coordinate horizontal axis S axis, and taking the road length extending direction as a positive direction; taking the direction vertical to the road as the axis of longitudinal axis l, and the left side as the positive direction; the coordinates of the starting point of the unmanned vehicle are set to(s)_I，l_I，θ_I) Setting the coordinate of the local path planning track terminal point as(s)_f，l_f，θ_f) Wherein, theta_IAnd theta_fPlanning a track end course angle, S, for the unmanned vehicle starting point and the local path_IAnd S_fPlanning an abscissa value, l, of a trajectory end point in a road coordinate system for a starting point and a local path of the unmanned vehicle_IAnd l_fAnd planning a longitudinal coordinate value of the track terminal point under a road coordinate system for the starting point and the local path of the unmanned vehicle.

12) Then, inputting parameters required by local path planning:

121) the global path planning module inputs parameters to the local path planning module:

coordinate B under Cartesian coordinates of a series of planned points on the global planned path_i(X_bi，Y_bi) Course angle theta of planning point_biPlanning point road curvature kappa_biPlanning point speed limit V_biWherein b represents a global planning point; i denotes a global planning point index, i 1, 2,. Nb, Nb being the total number of planning points.

122) The environment perception module inputs parameters to the local path planning module:

the output of the environment perception module is the position A of the static obstacle and the dynamic obstacle under the road coordinate system_k(S_ak，l_ak) Width W_akVelocity V_akAnd type, etc., where k denotes the kth static obstacle or dynamic obstacle, and k 1, 2.

123) The positioning module inputs parameters to the local path planning module: position coordinates, speed, heading, etc. of the unmanned vehicle.

124) Inputting vehicle body parameters to the local path planning module: steering wheel angle, vehicle track, etc.

The sequence of steps 121) to 124) can be arbitrarily adjusted.

Step 2) the process of selecting the preview point is as follows:

firstly, a pre-aiming section is determined, the pre-aiming section is a section perpendicular to a planned path according to a global path planning point, and the intersection point of the section and a road surface is a line segment. And selecting a point of the travelable area on the line segment as a preview point. And selecting the distance of the pre-aiming section according to the speed. The preview point is selected on a line segment determined by the preview section according to a certain interval distance. Heading, curvature, speed limit and preview of preview pointThe global path planning points on the cross section are the same. The coordinate of the preview point in the road coordinate system is represented as: (s)_fy，l_fy，θ_fy) The subscript f represents a point on the preview cross section, y represents a preview point number, and if N preview points are selected, y is 1, 2,. N.

Step 3) planning the track based on the road coordinate system, wherein the process of generating the basic track is as follows:

the basic track is a plurality of tracks which are possibly driven by the unmanned vehicle in the future according to different preview points, and N candidate tracks exist at N preview points. The basic track is obtained by solving an equation according to the initial parameters of the vehicle and the destination parameters of the preview in a road coordinate system, and the basic track equation is expressed as follows in the road coordinate system:

l_j(s)＝c_0j+c_1js+c_2js²+c_3js³+c_4js⁴+c_5js⁵ (1)

wherein s is the coordinate value of the horizontal axis of the basic track j in the road coordinate system, and l_j(s) is the ordinate of the basic track j in the road coordinate system, which is a function of the abscissa of the basic track in the road coordinate system; the subscript j denotes the index of the alternate track, j ═ 1, 2,. N; n is the total number of the alternative tracks. c. C_0j、c_1j、c_2j、c_3j、c_4j、c_5jThe unknown parameters of the curve equation are obtained by solving the following equation system:

wherein S is_c、l_cCoordinates of the unmanned vehicle at the current moment in the road coordinate system,

S_fj、l_fjcoordinates of the jth basic track end point in a road coordinate system,

tanθ_fjis the tangent value of the jth basic track endpoint course angle_fj＝0，

tanθ_cIs the initial heading tangent, θ, of the unmanned vehicle_cThe positioning information is obtained by the positioning module,

κ_ccurvature of a driving track for the current position of the unmanned vehicle, related to steering wheel angle, is obtained by a positioning module, kappa_fjCurvature of the end point of the jth base trajectory,. kappa_fj＝0。

Step 4) matching the basic track with the road shape, and generating an alternative track comprises the following steps:

matching the basic track with the road shape under a road coordinate system to obtain an alternative track; the coordinates of the corresponding planning point on the alternative trajectory in the cartesian coordinate system are expressed by the following formula:

wherein S is_biFor the abscissa, theta, of the global path planning point in the road coordinate_biA point course angle is planned for the global path; x_biAnd Y_biFor the abscissa and ordinate of the global path planning point in cartesian coordinates, the subscript j denotes the number of candidate trajectories, j is 1, 2,. N, N is the total number of candidate trajectories, b denotes the global planning point, i denotes the global path planning point number, i is 1, 2,. Nb, and Nb is the total number of planning points.

Step 5) the process of calculating the speed limit value based on the alternative track and the corresponding speed limit position is as follows:

the speed limit of the unmanned vehicle on the alternative track is divided into the following cases:

51) speed limit and speed limit position of the unmanned vehicle by road conditions:

the speed limit of the road condition to the unmanned vehicle is obtained according to the road curvature and the map, the speed limit of the road condition to the vehicle is obtained, and the global path planning point S is obtained_biPoint (S) on the corresponding candidate trajectory_bi，l_j(S_bi) Speed limit is calculated as follows:

if the speed limit V_1limjiSpeed limit V greater than road condition requirement_RlimThen V is_1limji＝V_Rlim(ii) a The speed-limiting position of the road condition corresponding to each alternative track j is recorded as:

S_1limji＝S_bi (5)

wherein, V_RlimThe speed of the vehicle is limited for the road condition,

S_bifor the abscissa of the global path planning point under the road coordinates,

κ_jito be the curvature corresponding to the global routing point i on the alternative trajectory,

the index j is the alternate track number, the index i is the corresponding global path plan point number,

ACC_lmaxthe maximum lateral acceleration required for comfort is determined by the comfort level of the person.

52) Speed limit and speed limit position of unmanned vehicle by static barrier and dynamic barrier

521) Obtaining the position coordinates A of the static obstacle and the dynamic obstacle under a road coordinate system through the measurement of a sensor arranged on the unmanned vehicle_k(S_ak，l_ak) Width W_akVelocity V_akThe like;

522) calculating safe distance of unmanned vehicle to static obstacle and dynamic obstacle

Longitudinal safe distance calculation formula:

D_Sak＝k_cV_c+ΔD_Sak (6)

wherein, V_CIs the speed of the unmanned vehicle, k_cIs a coefficient, empirically determined, Δ D_SakIs a parameter related to the type of obstacle.

The transverse safe distance calculation formula is as follows:

wherein, W_akFor static or dynamic obstacle width, type-dependent, Δ D_lReserving a minimum transverse safety distance, W, for fixation_CThe width of the unmanned vehicle.

523) Static obstacle and dynamic obstacle speed limit V_2limkAnd corresponding speed limit position S_2k

5231) Calculating the speed limit of a static obstacle and the corresponding speed limit position

The speed limit of the static obstacle k to the alternative track j is calculated by adopting the following formula:

the horizontal coordinate of the lower speed limiting position in the road coordinate system is as follows:

S_2limjk＝S_ak-D_Sak (9)

wherein D is_lmaxFor the maximum lateral safety distance to be reached,

V_Rlimthe speed of the unmanned vehicle is limited for road conditions,

Δl_akjthe distance of the static obstacle k from the second alternative planned trajectory j.

5232) Calculating speed limit of dynamic barrier and corresponding speed limit position

Velocity V of dynamic obstacle_akThe decomposition is (V) under the road coordinate system_Sak，V_lak)，V_SakIs the longitudinal speed of the obstacle k along the road; v_lSakIs the lateral speed of the obstacle k along the road.

52321) The first condition is as follows: if V_lSak＝0

The speed limit of the unmanned vehicle on the alternative track j by the dynamic obstacle k is as follows:

the lateral coordinate of the speed limiting position of the dynamic barrier k on the unmanned vehicle is under a road coordinate system:

S_2limjk＝S_ak-D_Sak (11)

wherein D is_lmaxFor the maximum lateral safety distance to be reached,

V_Rlimto limit the speed of the unmanned vehicle for road conditions.

52322) Case two: V_lSakWhen the speed is not equal to 0, the step of calculating the speed limit of the dynamic barrier k to the unmanned vehicle is as follows:

523221) whether the dynamic obstacle is approaching the unmanned vehicle or leaving the candidate trajectory j:

Δl_akjrepresenting the distance of the dynamic obstacle k from the second alternative planned trajectory j;

Δl_akj> 0 indicates that the dynamic obstacle k is to the left of the alternative trajectory,

at Δ l_akjAt > 0:

V_lSak> 0 denotes that the dynamic obstacle k leaves the candidate trajectory j

V_lSak< 0 indicates that the dynamic obstacle k approaches the candidate trajectory j.

Δl_akj< 0 indicates that the dynamic obstacle k is to the right of the alternative trajectory;

at Δ l_akjWhen < 0:

V_lSak< 0 indicates that the dynamic obstacle k leaves the alternative trajectory j,

V_lSaka > 0 indicates that the dynamic obstacle k approaches the candidate trajectory j.

The dynamic obstacle k leaving the candidate trajectory j does not cause a hindrance to the unmanned vehicle.

523222) when the dynamic obstacle k approaches the candidate trajectory j, judging whether the unmanned vehicle is obstructed: estimating the time when the dynamic obstacle k enters the alternative track j:

and (3) estimating the position of the dynamic obstacle k entering the alternative track j:

S_jakin＝S_ak+V_Sakt_jakin (13)

the time when the unmanned vehicle drives to the position where the dynamic obstacle k enters the alternative track j:

Δt_akinfor redundant time, determined based on the type of dynamic obstacle and the moving speed,

when t is_jckin＜t_jakinThe vehicle can exceed the dynamic barrier and cannot block the unmanned vehicle;

when t is_jckin≥t_jakinIt may cause a hindrance to the vehicle.

523223) calculating the speed limit and speed limit position of the unmanned vehicle when the lateral speed of the dynamic obstacle is not zero

For dynamic obstacles that do not obstruct or restrict the unmanned vehicle, no calculation is needed.

For a dynamic obstacle that obstructs the unmanned vehicle, the speed limit of the dynamic obstacle k to the candidate trajectory j is:

V_2limjk＝0 (15)

speed limiting position:

S_2limjk＝S_ak-D_Sak (16)。

53) calculating the speed limit and the speed limit position of the traffic signal lamp on the unmanned vehicle

The abscissa of the position of the vehicle stop line at the intersection of the traffic signal lamps under the road coordinate system is S_stopCurrent vehicle speed V_c

531) The first condition is as follows: the current is green light or yellow light and the time for the unmanned vehicle to reach the intersection is less than 1s

Then: speed limit V of alternative trajectory j_3limjLimiting the speed of the intersection.

The speed limit position is a stop line S_3limj＝S_stop (17)

532) Case two: the current is red light or the current is yellow light, and the time for the unmanned vehicle to reach the intersection is more than 1s

Then: speed limit V of alternative trajectory j_3limj＝0(18)

The speed limit position is a stop line S_3limj＝S_stop (19)。

54) Calculating the speed limit required by the road traffic management department and the speed limit position

The road traffic management department requires speed limit, and the local speed limit and the corresponding speed limit position required by traffic safety, such as intersections, speed bumps, specific positions (such as school doorways) and the like, are obtained from a map.

The abscissa of the speed-limiting position on the road coordinate system is marked as S_4limjmThe corresponding speed limit is marked as V_4limjmThe subscript m is the number of locations in the alternative trajectory that must be slowed down as required by the road traffic authority.

And (3) limiting the speed corresponding to the speed limiting position of each candidate track j:

at the speed-limiting position S_1limjiThe speed limit is V_1limji，

At the speed-limiting position S_21limjkThe speed limit is V_2limjk，

At the speed-limiting position S_3limjThe speed limit is V_3limj，

At the speed-limiting position S_4limjmThe speed limit is V_4limjm。

Arranging all speed limit points on the j tracks from near to far according to the distance from the unmanned vehicle to recombine the sequence (S)_limjn，V_limjn) The subscript n represents the serial number of the speed limiting total sequence of the alternative track j, and n is 1, 2. And the speed limit of the speed limit position is processed according to the following method:

under the condition that multiple speed limit constraints exist at the same position, calculating by using a minimum speed limit value;

and secondly, if the speed limit value of the planning point at the same alternative track position with a longer distance is greater than the speed limit value of the previous point, the speed limit value is the speed limit value of the previous point.

Step 6) selecting the candidate tracks preferably, wherein the process of obtaining the track executed by the unmanned vehicle comprises the following steps:

and the candidate track corresponding to the minimum value calculated according to the optimization objective function calculated by each candidate track is the track executed by the last unmanned vehicle.

The calculation formula of the optimization objective function is as follows:

wherein, ω is_rThe subscript r represents the index serial number, r is 1, 2 and M, and M is the total index number; j is an alternative track, j is 1, 2.. N, and N is the total number of the alternative tracks; .

First index J₁(j) For the track length index, the following is calculated:

wherein S is_j(t0) is the farthest track length that can be traveled according to the speed limit within the alternative track j within a period of time t 0.

Wherein S is_maxFor the journey calculated at the maximum speed limit at time t0,

S_minthe distance calculated at the lowest speed limit at time t 0.

Second index J₂(j) For the curvature term index, the following is calculated:

wherein:

S_ffor the abscissa of the end point of the alternative trajectory j in the road coordinate system,

S_Cis unmannedThe driving vehicle is on the abscissa under the road coordinate,

k_j(s) curvature at the candidate trajectory j coordinate point s,

κ_maxthe maximum curvature of travel of the vehicle.

Third index J₃(j) For the trajectory deviation planned path index, the calculation formula is as follows:

wherein:

l_j(s) is the ordinate of the current candidate track j in the road coordinate system,

l_maxthe longitudinal coordinate of the pre-aiming point with the maximum longitudinal coordinate in the road coordinate system is taken as the longitudinal coordinate of all the pre-aiming points,

fourth index J₄(j) For the consistency index before and after the alternative track, the calculation formula is as follows:

wherein:

l_*(s) is the ordinate of the last local planned trajectory preview point in the road coordinate system,

l_j(s) is the ordinate of the preview point of the candidate track j in the road coordinate system,

l is the road width.

Step 7) the process of planning the speed of the unmanned vehicle is as follows:

the planning of the speed of the unmanned vehicle is performed on the preferred trajectory already obtained in the preceding step 6).

The speed of the unmanned vehicle is planned according to the following method:

71) selecting the point position S with the minimum speed limit value on the optimized track_vminSpeed limit note V_min；

72) As shown in fig. 3, a speed curve from the current unmanned vehicle to the point is planned according to the point position and the speed limit, and the curve is constructed according to a trapezoidal curve. Where the abscissa is time and the ordinate is speed. The trapezoidal curve is divided into an acceleration section, a stable section and a deceleration section.

V_maxlimFor unmanned vehicles to a lowest speed limit position S_vminThe speed of the inner maximum speed limit is limited,

V_Cfor the current speed of the unmanned vehicle,

V_fspeed limit V which is a speed limit position on an optimized track_min，

ACC_SThe acceleration of the acceleration section is determined according to the comfort degree during the normal running of the vehicle,

DEC_Sthe deceleration of the deceleration section is determined according to the comfort degree during the normal running of the vehicle,

t_steadyis the plateau time, which cannot be too short and is greater than a certain value.

Given the above parameters, the plateau speed cannot exceed V_maxlimA speed profile is established.

73) Verifying the speed limit of the speed limit position on the optimal track by using a speed curve, and if all the speed limits are met, determining the function curve as a planned function curve; otherwise, steps 71), 72) and 73) are repeated with the previous speed limit position until all are satisfied.

Based upon the foregoing description of the preferred embodiment of the invention, it should be apparent that the invention defined by the appended claims is not limited solely to the specific details set forth in the foregoing description, as many apparent variations thereof are possible without departing from the spirit or scope thereof.

Claims (4)

1. A method of local path planning for an unmanned vehicle, comprising the steps of:

1) inputting parameters required by local path planning;

2) selecting a preview point;

3) planning a track based on a road coordinate system to generate a basic track;

4) matching the basic track with the road shape to generate an alternative track;

5) calculating a speed limit value based on the alternative track and a corresponding speed limit position;

6) the alternative tracks are optimized to obtain tracks executed by the unmanned vehicle, and the tracks are called as optimized tracks;

7) planning a speed of the unmanned vehicle based on the preferred trajectory;

the step 3) performs track planning based on the road coordinate system, and the process of generating the basic track is as follows:

the basic track is a plurality of tracks which are possibly driven by the unmanned vehicle in the future according to different preview points, and N alternative tracks exist at N preview points; the basic track is obtained by solving an equation according to the initial parameters of the vehicle and the destination parameters of the preview in a road coordinate system, and the basic track equation is expressed as follows in the road coordinate system:

l_j(s)＝c_0j+c_1js+c_2js²+c_3js³+c_4js⁴+c_5js⁵ (1)

wherein s is the coordinate value of the horizontal axis of the basic track j in the road coordinate system, and l_j(s) is the ordinate of the base track j in the road coordinate system, which is a function of the abscissa of the base track in the road coordinate system; the subscript j denotes the index of the alternate track, j ═ 1, 2,. N; n is the total number of the alternative tracks; c. C_0j、c_1j、c_2j、c_3j、c_4j、c_5jThe unknown parameters of the curve equation are obtained by solving the following equation system:

wherein S is_c、l_cCoordinates of the unmanned vehicle at the current moment in the road coordinate system,

S_fj、l_fjcoordinates of the jth basic track end point in a road coordinate system,

tanθ_fjis the tangent value of the terminal course angle of the jth basic track_fj＝0，

tanθ_cIs the initial heading tangent, θ, of the unmanned vehicle_cThe positioning information is obtained by the positioning module,

κ_ccurvature of a driving track for the current position of the unmanned vehicle, related to steering wheel angle, is obtained by a positioning module, kappa_fjCurvature of the end point of the jth base trajectory,. kappa_fj＝0；

The step 4) of matching the basic track with the road shape and generating the alternative track comprises the following steps:

matching the basic track with the road shape under a road coordinate system to obtain an alternative track; the coordinates of the corresponding planning points on the alternative tracks in the Cartesian coordinate system are expressed by the following formula:

wherein S is_biFor the abscissa, theta, of the global path planning point in the road coordinate_biA point course angle is planned for the global path; x_biAnd Y_biThe abscissa and the ordinate of the global path planning point in cartesian coordinates are given, the subscript j represents the number of the candidate tracks, j is 1, 2,. N, N is the total number of the candidate tracks, b represents the global planning point, i represents the global path planning point number, i is 1, 2,. Nb, and Nb is the total number of the planning points;

the step 5) of calculating the speed limit value based on the alternative track and the corresponding speed limit position comprises the following steps:

the speed limit of the unmanned vehicle on the alternative track is divided into the following cases:

51) speed limit and speed limit position of the unmanned vehicle by road conditions:

the speed limit of the road condition to the unmanned vehicle is obtained according to the road curvature and the map, the speed limit of the road condition to the vehicle is obtained, and the global path planning point S is obtained_biPoint (S) on the corresponding candidate trajectory_bi，l_j(S_bi) Speed limit is calculated as follows:

if the speed limit V_1limjiSpeed limit V greater than road condition requirement_RlimThen V is_1limji＝V_Rlim(ii) a The speed-limiting position of the road condition corresponding to each alternative track j is recorded as:

S_1limji＝S_bi (5)

wherein, V_RlimThe speed of the vehicle is limited for the road condition,

S_bifor the abscissa of the global path planning point under the road coordinates,

κ_jito be the curvature corresponding to the global routing point i on the alternative trajectory,

the index j is the alternate track number, the index i is the corresponding global path plan point number,

ACC_lmaxthe maximum lateral acceleration is required according to the comfort level and is determined by the comfort level of a person;

52) speed limit and speed limit position of unmanned vehicle by static barrier and dynamic barrier

521) Obtaining the position coordinates A of the static obstacle and the dynamic obstacle under a road coordinate system through the measurement of a sensor arranged on the unmanned vehicle_k(S_ak，l_ak) Width W_akVelocity V_akA parameter; wherein k represents the kth static obstacle or dynamic obstacle, and k is 1, 2.

522) Calculating safe distance of unmanned vehicle to static obstacle and dynamic obstacle

Longitudinal safe distance calculation formula:

D_Sak＝k_cV_c+ΔD_Sak (6)

wherein, V_CIs the speed of the unmanned vehicle, k_cIs a coefficient, empirically determined, Δ D_SakIs a parameter related to the type of obstacleCounting;

the transverse safe distance calculation formula is as follows:

wherein, W_akFor static or dynamic obstacle width, type-dependent, Δ D_lReserving a minimum transverse safety distance, W, for fixation_CIs the width of the unmanned vehicle;

523) static obstacle and dynamic obstacle speed limit V_2limkAnd corresponding speed limit position S_2k

5231) Calculating the speed limit of a static obstacle and the corresponding speed limit position

The speed limit of the static obstacle k to the alternative track j is calculated by adopting the following formula:

the horizontal coordinate of the lower speed limiting position in the road coordinate system is as follows:

S_2limjk＝S_ak-D_Sak (9)

wherein D is_lmaxFor the maximum lateral safety distance to be reached,

V_Rlimthe speed of the unmanned vehicle is limited for road conditions,

Δl_akjthe distance between the static obstacle k and the second alternative planning track;

5232) calculating speed limit of dynamic barrier and corresponding speed limit position

Velocity V of dynamic obstacle_akThe decomposition is (V) under the road coordinate system_Sak，V_lak)，V_SakIs the longitudinal speed of the dynamic barrier k along the road; v_lSakIs the lateral speed of the dynamic barrier k along the road;

52321) The first condition is as follows: if V_lSak＝0

The speed limit of the unmanned vehicle on the alternative track j by the dynamic obstacle k is as follows:

the lateral coordinate of the speed limiting position of the dynamic barrier k on the unmanned vehicle is under a road coordinate system:

S_2limjk＝S_ak-D_Sak (11)

wherein D is_lmaxFor the maximum lateral safety distance to be reached,

V_Rlimlimiting the speed of the unmanned vehicle for road conditions;

52322) Case two: V_lSakWhen the speed is not equal to 0, the step of calculating the speed limit of the dynamic barrier k to the unmanned vehicle is as follows:

523221) whether the dynamic obstacle is approaching the unmanned vehicle or leaving the candidate trajectory j:

Δl_akjrepresenting the distance of the dynamic obstacle k from the second alternative planned trajectory;

Δl_akj> 0 indicates that the dynamic obstacle k is to the left of the alternative trajectory,

at Δ l_akjAt > 0:

V_lSak> 0 denotes that the dynamic obstacle k leaves the candidate trajectory j

V_lSak< 0 indicates that the dynamic obstacle k approaches the candidate trajectory j;

Δl_akj< 0 indicates that the dynamic obstacle k is to the right of the alternative trajectory;

at Δ l_akjWhen < 0:

V_lSak< 0 indicates that the dynamic obstacle k leaves the alternative trajectory j,

V_lSak> 0 indicates that the dynamic obstacle k approaches the candidate trajectory j;

the dynamic obstacle k leaves the alternative track j without causing obstruction to the unmanned vehicle;

523222) when the dynamic obstacle k approaches the candidate trajectory j, judging whether the unmanned vehicle is obstructed:

estimating the time when the dynamic obstacle k enters the alternative track j:

and (3) estimating the position of the dynamic obstacle k entering the alternative track j:

S_jakin＝S_ak+V_Sakt_jakin (13)

the time when the unmanned vehicle drives to the position where the dynamic obstacle k enters the alternative track j:

Δt_akinfor redundant time, determined based on the type of dynamic obstacle and the moving speed,

when t is_jckin＜t_jakinThe vehicle can exceed the dynamic barrier and cannot block the unmanned vehicle;

when t is_jckin≥t_jakinIt may cause a hindrance to the vehicle;

523223) calculating the speed limit and speed limit position of the unmanned vehicle when the lateral speed of the dynamic obstacle is not zero

For dynamic obstacles which do not obstruct and limit the unmanned vehicle, calculation is not needed;

for a dynamic obstacle that obstructs the unmanned vehicle, the speed limit of the dynamic obstacle k to the candidate trajectory j is:

V_2limjk＝0 (15)

speed limiting position:

S_2limjk＝S_ak-D_Sak (16)；

53) calculating the speed limit and the speed limit position of the traffic signal lamp on the unmanned vehicle

The abscissa of the position of the vehicle stop line at the intersection of the traffic signal lamps under the road coordinate system is S_stopCurrent vehicle speed V_c

531) The first condition is as follows: is currently green or is currently yellow and is unmannedAnd if the intersection reaching time is less than 1 s: speed limit V of alternative trajectory j_3limjLimiting the speed of the intersection;

the speed limit position is a stop line S_3limj＝S_stop (17)

532) Case two: when the current is red light or the current is yellow light and the time of the unmanned vehicle reaching the intersection is more than 1 s: speed limit V of alternative trajectory j_3limj＝0 (18)

The speed limit position is a stop line S_3limj＝S_stop (19)；

54) Calculating the speed limit required by the road traffic management department and the speed limit position

Speed limit required by a road traffic management department, including intersection, speed bump, local speed limit required by traffic safety at a specific position and corresponding speed limit position are obtained from a map;

the abscissa of the speed-limiting position on the road coordinate system is marked as S_4limjmThe corresponding speed limit is marked as V_4limjmThe subscript m is the number of positions which are required to be decelerated by a road traffic management department in the alternative track;

and (3) limiting the speed corresponding to the speed limiting position of each candidate track j:

at the speed-limiting position S_1limjiThe speed limit is V_1limji，

At the speed-limiting position S_21limjkThe speed limit is V_2limjk，

At the speed-limiting position S_3limjThe speed limit is V_3limj，

At the speed-limiting position S_4limjmThe speed limit is V_4limjm；

Arranging all speed limit points on the j tracks from near to far according to the distance from the unmanned vehicle to recombine the sequence (S)_limjn，V_limjn) The subscript n represents the serial number of the speed limiting total sequence of the alternative track j, and n is 1, 2. And the speed limit of the speed limit position is processed according to the following method:

under the condition that multiple speed limit constraints exist at the same position, calculating by using a minimum speed limit value;

secondly, if the speed limit value of the planning point at the same alternative track position with longer distance is greater than the speed limit value of the previous point, the speed limit value is the speed limit value of the previous point;

the step 6) is to optimize the alternative tracks, and the process of obtaining the track executed by the unmanned vehicle is as follows:

the candidate track corresponding to the minimum value calculated according to the optimization objective function calculated by each candidate track is the track executed by the last unmanned vehicle;

the calculation formula of the optimization objective function is as follows:

wherein, ω is_rThe subscript r represents the index serial number, r is 1, 2 and M, and M is the total index number; j is an alternative track, j is 1, 2.. N, and N is the total number of the alternative tracks;

first index J₁(j) For the track length index, the following is calculated:

wherein S is_j(t0) is the farthest track length which can be driven according to the speed limit in the alternative track j in a period of time t 0;

wherein S is_maxFor the journey calculated at the maximum speed limit at time t0,

S_minthe distance calculated at the lowest speed limit within the time t 0;

second index J₂(j) For the curvature term index, the following is calculated:

wherein:

S_ffor the abscissa of the end point of the alternative trajectory j in the road coordinate system,

S_Cfor the unmanned vehicle to be on the abscissa under the road coordinates,

κ_j(s) curvature at the candidate trajectory j coordinate point s,

κ_maxmaximum curvature for vehicle travel;

third index J₃(j) For the trajectory deviation planned path index, the calculation formula is as follows:

wherein:

l_j(s) is the ordinate of the current candidate track j in the road coordinate system,

l_maxthe longitudinal coordinate of the pre-aiming point with the maximum longitudinal coordinate in the road coordinate system is taken as the longitudinal coordinate of all the pre-aiming points,

fourth index J₄(j) For the consistency index before and after the alternative track, the calculation formula is as follows:

wherein:

l_*(s) is the ordinate of the last local planned trajectory preview point in the road coordinate system,

l_j(s) is the ordinate of the preview point of the candidate track j in the road coordinate system,

l is the road width.

2. The local path planning method for the unmanned vehicle according to claim 1, wherein the step 1) of inputting parameters required for local path planning is as follows:

11) first, the road length direction is set as the coordinate horizontal axis S axis, the road length extending directionThe direction is a positive direction; taking the direction vertical to the road as the axis of longitudinal axis l, and the left side as the positive direction; the coordinates of the starting point of the unmanned vehicle are set to(s)_I，l_I，θ_I) Setting the coordinate of the local path planning track terminal point as(s)_f，l_f，θ_f) Wherein, theta_IAnd theta_fPlanning a track end course angle, S, for the unmanned vehicle starting point and the local path_IAnd S_fPlanning an abscissa value, l, of a trajectory end point in a road coordinate system for a starting point and a local path of the unmanned vehicle_IAnd l_fPlanning longitudinal coordinate values of a starting point and a local path of the unmanned vehicle in a road coordinate system for a track terminal point;

12) then, inputting parameters required by local path planning:

121) the global path planning module inputs parameters to the local path planning module:

coordinate B under Cartesian coordinates of a series of planned points on the global planned path_i(X_bi，Y_bi) Course angle theta of planning point_biPlanning point road curvature kappa_biPlanning point speed limit V_biWherein b represents a global planning point; i represents a global planning point index, i is 1, 2,. Nb, Nb is the total number of planning points;

122) the environment perception module inputs parameters to the local path planning module:

the output of the environment perception module is the position A of the static obstacle and the dynamic obstacle under the road coordinate system_k(S_ak，l_ak) Width W_akVelocity V_akAnd type, wherein k denotes the kth static or dynamic obstacle, k 1, 2;

123) the positioning module inputs parameters to the local path planning module: position coordinates, speed and course of the unmanned vehicle;

124) inputting vehicle body parameters to the local path planning module: steering wheel angle and vehicle track;

the sequence of steps 121) to 124) can be arbitrarily adjusted.

3. The local path planning method for unmanned vehicle according to claim 1, wherein the step 2) of selecting the preview point comprises the following steps:

firstly, determining a preview section, wherein the preview section is a section perpendicular to a planned path according to a global path planning point, and the intersection point of the section and a road surface is a line segment; selecting a point of a travelable area on the line segment as a preview point; selecting a pre-aiming section distance according to the speed; the preview point is selected on a line segment determined by a preview section according to a certain interval distance; the course, curvature and speed limit of the preview point are the same as the global path planning point on the preview section; the coordinate of the preview point in the road coordinate system is represented as: (s)_fy，l_fy，θ_fy) The subscript f represents a point on the preview cross section, y represents a preview point number, and if N preview points are selected, y is 1, 2,. N.

4. The local path planning method for unmanned vehicle according to claim 1, wherein the step 7) of planning the speed of the unmanned vehicle is as follows:

planning the speed of the unmanned vehicle is carried out on the obtained preferred track in the step 6);

the speed of the unmanned vehicle is planned according to the following method:

71) selecting the point position S with the minimum speed limit value on the optimized track_vminSpeed limit note V_min；

72) Planning a speed curve from the current unmanned vehicle to the point according to the point position and the speed limit, wherein the curve is constructed according to a trapezoidal curve; wherein the abscissa is time and the ordinate is speed; the trapezoidal curve is divided into an acceleration section, a stable section and a deceleration section;

V_maxlimfor unmanned vehicles to a lowest speed limit position S_vminThe speed of the inner maximum speed limit is limited,

V_Cfor the current speed of the unmanned vehicle,

V_fspeed limit V which is a speed limit position on an optimized track_min，

ACC_SThe acceleration of the acceleration section is determined according to the comfort degree during the normal running of the vehicle,

DEC_Sthe deceleration of the deceleration section is determined according to the comfort degree during the normal running of the vehicle,

t_steadyis a plateau time, the plateau cannot be too short, being greater than a certain value;

given the above parameters, the plateau speed cannot exceed V_maxlimEstablishing a speed curve;

73) the speed limit of the upper speed limit position of the optimized track is verified by using the speed curve, and if all the speed limits are met, the function curve is a planned function curve; otherwise, steps 71), 72) and 73) are repeated with the previous speed limit position until all are satisfied.

Images