Track planning method

Abstract

The invention provides a trajectory planning method, which comprises the following steps of: 1, determining boundaries, obstacles and vehicle poses in a narrow lane scene, establishing a narrow lane passing model, and establishing a local coordinate system of trajectory planning according to the vehicle poses; step 2, determining a target reference line and a target point in the narrow channel passing model; step 3, performing mixed A * trajectory planning based on the passable area according to the target point, calculating and recording the trajectory in the vehicle driving process, and judging whether the trajectory planning fails or not, if so, entering the next step, otherwise, repeating the step; and 4, reading the recorded historical driving track, carrying out tracking rollback, and returning to the step 2. According to the method, the situation that an existing path planning algorithm cannot escape after being blocked by obstacles in a narrow lane is effectively avoided, and the trajectory planning success rate is increased.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a trajectory planning method. Background Art

[0002] With the development of autonomous driving technology, the ability of vehicles to navigate autonomously in various complex environments has become particularly important, especially in narrow roads, where vehicles face higher challenges. Path planning and local obstacle avoidance technology are one of the key technologies to ensure safe driving of vehicles. Existing path planning algorithms, such as the hybrid A* algorithm, can provide effective path planning in most cases. However, when a vehicle is stuck on a narrow road or in a complex environment, path planning will fail and the vehicle will be unable to escape. Therefore, the existing trajectory planning technology still has certain limitations, and a new trajectory planning method is urgently needed to improve the vehicle's obstacle avoidance and escape capabilities in narrow roads and complex environments. Summary of the invention

[0003] The present invention provides a trajectory planning method, which aims to solve the defects in the prior art, effectively avoid the situation where the current path planning algorithm is stuck in a narrow road and cannot escape from an obstacle, and improve the success rate of trajectory planning.

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

[0005] The present invention provides a trajectory planning method, comprising:

[0006] Step 1: Determine the boundaries, obstacles and vehicle postures in the narrow road scene, establish a narrow road traffic model, and establish a local coordinate system for trajectory planning according to the vehicle posture;

[0007] Step 2: determining a target reference line and a target point in the narrow passage model;

[0008] Step 3: Perform hybrid A* trajectory planning based on the target point and the passable area, calculate and record the trajectory of the vehicle during driving, and determine whether the trajectory planning fails. If yes, proceed to the next step, otherwise repeat this step;

[0009] Step 4: Read the recorded historical driving trajectory, perform tracking and fallback, and return to step 2.

[0010] Specifically, the step 2 includes:

[0011] Step 201: Establish a front view of the vehicle and determine the narrow road boundary and obstacle distribution;

[0012] Step 202: determine whether there is an obstacle in the narrow passage. If yes, establish a circumscribed circle of the obstacle with the center point of the obstacle as the center as a first auxiliary circle, and take the corner point of the narrow passage opposite to the obstacle as an equivalent obstacle, and establish a second auxiliary circle with the center point of the equivalent obstacle and a preset radius. Otherwise, take the corner points on both sides of the narrow passage as equivalent obstacles, and establish a first auxiliary circle and a second auxiliary circle with the center point of the equivalent obstacle and a preset radius, respectively.

[0013] Step 203: determining a key passable point and a first auxiliary passable point and a second auxiliary passable point according to the relative positions of the first auxiliary circle and the second auxiliary circle;

[0014] Step 204, determine the target arc according to the key passable point and the first auxiliary passable point and the second auxiliary passable point, the target arc is the target reference line, the point on the target arc is the target point, and at the same time obtain the tangent of the key passable point, and use the angle of the tangent as the yaw angle of the target point.

[0015] Specifically, step 203 includes: if the relative positions of the first auxiliary circle and the second auxiliary circle are tangent, determining the key passable point and the first auxiliary passable point and the second auxiliary passable point according to a first preset rule, wherein the first preset rule is:

[0016] Step A1: Use the tangent point as the key passable point;

[0017] Step A2, connecting the key passable point, the first circle center, and the second circle center to generate a first auxiliary line;

[0018] The first circle center is the center of the first auxiliary circle, and the second circle center is the center of the second auxiliary circle;

[0019] Step A3, generating a perpendicular line of the first auxiliary line through the key passable point, recorded as a first perpendicular line;

[0020] Step A4, taking the key passable point as the center, selecting two endpoints on the first vertical line according to a preset length as an initial first auxiliary passable point and an initial second auxiliary passable point;

[0021] Step A5, generating a window area to be selected with the first auxiliary passable point and the initial second auxiliary passable point as the center respectively;

[0022] Step A6: Calculate the distance cost value in the candidate window area according to the obstacle distribution, and use the point with the minimum distance cost as the first auxiliary passable point and the second auxiliary passable point.

[0023] Specifically, step 203 includes: if the relative positions of the first auxiliary circle and the second auxiliary circle are separated, determining the key passable point and the first auxiliary passable point and the second auxiliary passable point according to a second preset rule, wherein the second preset rule is:

[0024] Step B1, connecting the first circle center and the second circle center to generate a second auxiliary line;

[0025] Step B2, taking the midpoint of the line segment of the second auxiliary line between the two circles as the key passable point;

[0026] Step B3, generating a perpendicular line of the second auxiliary line through the key passable point, recorded as a second perpendicular line;

[0027] Step B4, taking the key passable point as the center, selecting two endpoints on the second vertical line according to a preset length as an initial first auxiliary passable point and an initial second auxiliary passable point;

[0028] Step B5, generating a window area to be selected with the first auxiliary passable point and the initial second auxiliary passable point as the center respectively;

[0029] Step B6: Calculate the distance cost value in the candidate window area according to the obstacle distribution, and use the point with the minimum distance cost as the first auxiliary passable point and the second auxiliary passable point.

[0030] Specifically, step 203 includes: if the relative positions of the first auxiliary circle and the second auxiliary circle intersect, determining the key passable point and the first auxiliary passable point and the second auxiliary passable point according to a third preset rule, and the third preset rule is:

[0031] Step C1, taking the intersection of the first auxiliary circle and the second auxiliary circle as the first initial auxiliary passable point and the second initial auxiliary passable point;

[0032] Step C2, connecting the intersection points of the first auxiliary circle and the second auxiliary circle to generate a third auxiliary line;

[0033] Step C3, taking the midpoint of the third auxiliary line as a key passable point;

[0034] Step C4, generating a window area to be selected with the first auxiliary passable point and the initial second auxiliary passable point as the center respectively;

[0035] Step C5: Calculate the distance cost value in the candidate window area according to the obstacle distribution, and use the point with the minimum distance cost as the first auxiliary passable point and the second auxiliary passable point.

[0036] Specifically, the hybrid A* trajectory planning based on the traversable area includes:

[0037] Step D1, obtaining the current position of the vehicle as the current node, and expanding the surrounding nodes to obtain several nodes to be selected;

[0038] Step D2: Calculate the cost value of the candidate nodes and select the node with the smallest cost value as the next node;

[0039] Step D3: According to all the nodes and target points, the path is extended by using the Dubins curve through the hybrid A* algorithm to obtain the driving trajectory.

[0040] Specifically, the calculating and recording of the trajectory of the vehicle during driving includes:

[0041] Step E1, calculating the yaw angle change according to a first preset formula;

[0042] Step E2, judging the driving type of the vehicle according to the yaw angle change, if it is straight driving, calculating the coordinates and yaw angle of the rear axle of the vehicle at the current moment according to the second preset formula, if it is curved driving, calculating the coordinates and yaw angle of the rear axle of the vehicle at the current moment according to the third preset formula;

[0043] Step E3: record the coordinates and yaw angle of the rear axle of the vehicle at all times.

[0044] Specifically, the first preset formula is:

[0045]

[0046] Among them, ΔC represents the vehicle's travel distance, Δδ represents the change in the vehicle's front wheel angle, and L axies Specifically, the second preset formula is:

[0047] X(i+1)=X(i)+λΔCcos(θ(i))

[0048] Y(i+1)=Y(i)+λΔCsin(θ(i))

[0049] θ(i+1)=θ(i)

[0050] Where x(i) and y(i) represent the coordinates of the rear axle of the vehicle at the previous moment, and x(i+1) and y(i+1) represent the coordinates of the rear axle of the vehicle at the current moment; θ(i) represents the yaw angle at the previous moment, and θ(i+1) represents the yaw angle at the current moment; ΔC represents the distance traveled by the vehicle; λ represents the direction of travel of the vehicle, λ=1 represents forward movement, and λ=-1 represents backward movement; L axies Indicates wheelbase.

[0051] Specifically, the third preset formula is:

[0052]

[0053]

[0054] θ(i+1)=θ(i)+Δθ

[0055] Where x(i) and y(i) represent the coordinates of the rear axle of the vehicle at the previous moment, and x(i+1) and y(i+1) represent the coordinates of the rear axle of the vehicle at the current moment; θ(i) represents the yaw angle at the previous moment, and θ(i+1) represents the yaw angle at the current moment; Δθ represents the change in yaw angle; Δδ represents the change in the front wheel steering angle of the vehicle; λ represents the direction of vehicle travel, λ=1 represents forward movement, and λ=-1 represents backward movement; L axies Indicates wheelbase.

[0056] The beneficial effects of the present invention are as follows: the present invention obtains information on passable areas, calculates target points for passing through narrow roads, expands trajectory nodes, calculates Dubins curves and records vehicle trajectories in real time, and when path planning fails, combines a tracking algorithm to track historical trajectories and retreat a preset distance, thereby increasing obstacle avoidance space, and repeats the above steps until the Dubins curve / target point is found and the route is successfully planned, thereby effectively avoiding the situation in which the current path planning algorithm is stuck in a narrow road and cannot escape after being stuck by an obstacle. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] Figure 1 It is a schematic flow chart of the trajectory planning method of the present invention;

[0058] Figure 2 It is a schematic diagram of a narrow passage model of the present invention;

[0059] Figure 3 is a schematic diagram of a front view of the present invention;

[0060] Figure 4 It is a schematic diagram of two auxiliary circles being tangent to each other in the present invention;

[0061] Figure 5 It is a schematic diagram of two auxiliary circles separated from each other in the present invention;

[0062] Figure 6 It is a schematic diagram of the intersection of two auxiliary circles of the present invention;

[0063] Figure 7 It is a schematic diagram of the vehicle motion plane model of the present invention. DETAILED DESCRIPTION

[0064] The following specifically describes the embodiments of the present invention in conjunction with the accompanying drawings, which are only for reference and illustration and do not constitute a limitation on the scope of patent protection of the present invention.

[0065] In the process described in the specification, claims or drawings of the present invention, the serial numbers of each step (such as step 10, 20, etc.) are included. The serial numbers are only used to distinguish the steps, and the serial numbers themselves do not represent any execution order. It should be noted that the descriptions such as "first" and "second" in this article are only used to distinguish the description objects, etc., and do not represent the order of precedence, nor do they mean that "first", "second", etc. are different types.

[0066] like Figure 1 As shown, this embodiment provides an automatic driving trajectory planning method, including:

[0067] Step 1: Determine the boundaries, obstacles and vehicle postures in the narrow road scene, establish a narrow road traffic model, and establish a local coordinate system for trajectory planning based on the vehicle posture.

[0068] In the specific implementation, the perception module obtains the boundaries, obstacle coordinates, passable areas, and vehicle posture in the narrow road scene, establishes a narrow road passage model, and establishes a local coordinate system for trajectory planning according to the vehicle posture. The horizontal axis of the local coordinate system is parallel to the narrow road entrance line, and the vertical axis is parallel to the narrow road side line. The coordinate origin is obtained based on the offset of the vehicle body coordinate system, that is, the narrow road scene and obstacles will change dynamically in the local coordinate system.

[0069] like Figure 2 As shown, the map area is 10*10 meters, W is the width of the narrow lane, L is the length of the narrow lane, and L p is the obstacle length and C is the center point of the rear axle.

[0070] Step 2: Determine the target reference line and target point in the narrow lane traffic model.

[0071] In this embodiment, the target reference line is an arc driving trajectory at a preset distance from the narrow road exit, and the target point refers to the target position that the vehicle is to reach.

[0072] In this embodiment, step 2 includes:

[0073] Step 201: Create a front view of the vehicle and determine the narrow road boundary and obstacle distribution.

[0074] As shown in FIG. 3 , the front view is 3.5 m long and 6 m wide. The vehicle is in a curve and there is an obstacle on the left.

[0075] Step 202: determine whether there is an obstacle in the narrow passage. If yes, establish a circumscribed circle of the obstacle with the center point of the obstacle as the center as a first auxiliary circle, and take the corner point of the narrow passage opposite to the obstacle as an equivalent obstacle, and establish a second auxiliary circle with the center point of the equivalent obstacle and a preset radius. Otherwise, take the corner points on both sides of the narrow passage as equivalent obstacles, and establish a first auxiliary circle and a second auxiliary circle with the center point of the equivalent obstacle and a preset radius, respectively.

[0076] In this embodiment, the preset radius is 0.2 m.

[0077] like Figure 3 As shown, if the obstacle is located on the left side of the narrow road, a circumscribed circle is established with the center point of the left obstacle as the first auxiliary circle, and the corner point on the right side of the narrow road is used as an equivalent obstacle, and a second auxiliary circle with a radius of 0.2 meters is established with the equivalent obstacle as the center of the circle; conversely, if the obstacle is located on the right side of the narrow road, a circumscribed circle is established with the center point of the right obstacle as the first auxiliary circle, and the corner point on the left side of the narrow road is used as an equivalent obstacle, and a second auxiliary circle with a radius of 0.2 meters is established with the equivalent obstacle as the center of the circle. If there is no obstacle in the narrow road, the corner points on both sides of the narrow road are equated as two obstacles, and the first auxiliary circle and the second auxiliary circle are established with the centers of the two equivalent obstacles and the preset radius, respectively.

[0078] The specific method of establishing the circumscribed circle of the obstacle can be: firstly form a convex hull of the irregular obstacle, that is, a convex polygon, and then find the circumscribed circle, and the radius of the circle is determined according to the size of the obstacle.

[0079] There are many algorithms for realizing the convex hull, such as the extreme point method, the extreme edge method, the wrapping method, and the divide-and-conquer method. The present invention selects the Graham-Scan algorithm as the algorithm for realizing the convex hull.

[0080] Step 203: Determine a key passable point and a first auxiliary passable point and a second auxiliary passable point according to the relative positions of the first auxiliary circle and the second auxiliary circle.

[0081] In this embodiment, step 203 includes:

[0082] If the relative positions of the first auxiliary circle and the second auxiliary circle are tangent, determining the key passable point and the first auxiliary passable point and the second auxiliary passable point according to the first preset rule;

[0083] If the relative positions of the first auxiliary circle and the second auxiliary circle are separated, determining the key passable point and the first auxiliary passable point and the second auxiliary passable point according to the second preset rule;

[0084] If the relative positions of the first auxiliary circle and the second auxiliary circle intersect, the key passable point and the first auxiliary passable point and the second auxiliary passable point are determined according to a third preset rule.

[0085] In this embodiment, the first preset rule is:

[0086] Step A1: Use the tangent point as the key passable point;

[0087] Step A2, connecting the key passable point, the first circle center, and the second circle center to generate a first auxiliary line;

[0088] The first center is the center of the first auxiliary circle, and the second center is the center of the second auxiliary circle.

[0089] Step A3, generating a perpendicular line of the first auxiliary line through the key passable point, recorded as a first perpendicular line;

[0090] Step A4, taking the key passable point as the center, selecting two endpoints on the first vertical line according to a preset length as an initial first auxiliary passable point and an initial second auxiliary passable point;

[0091] Step A5, generating a window area to be selected with the first auxiliary passable point and the initial second auxiliary passable point as the center respectively;

[0092] Step A6: Calculate the distance cost value in the candidate window area according to the obstacle distribution, and use the point with the minimum distance cost as the first auxiliary passable point and the second auxiliary passable point.

[0093] like Figure 4 As shown, the relative positions of the first auxiliary circle and the second auxiliary circle are tangent, then the tangent point is used as the key passable point P0 (x0, y0), the key passable point P0, the first circle center C1, and the second circle center C2 are connected to generate a first auxiliary line C1C2, and then a first vertical line passing through the key passable point P0 and perpendicular to C1C2 is generated, and the two end points P1' and P2' are obtained on the first vertical line according to a preset length (for example, 1.5Rmax, Rmax is the radius of the largest circle of the two auxiliary circles) as the first initial auxiliary passable point and the second initial auxiliary passable point; with P1' and P2' as the center, the window areas to be selected are generated according to the areas of the auxiliary passable points that may be selected, and then the distance cost value of each point in the area is calculated according to the distribution of obstacles, and the point with the minimum distance cost is used as the first auxiliary passable point P1 and the second auxiliary passable point P2.

[0094] In this embodiment, the second preset rule is:

[0095] Step B1, connecting the first circle center and the second circle center to generate a second auxiliary line;

[0096] Step B2, taking the midpoint of the line segment of the second auxiliary line between the two circles as the key passable point;

[0097] Step B3, generating a perpendicular line of the second auxiliary line through the key passable point, recorded as a second perpendicular line;

[0098] Step B4, taking the key passable point as the center, selecting two endpoints on the second vertical line according to a preset length as an initial first auxiliary passable point and an initial second auxiliary passable point;

[0099] Step B5, generating a window area to be selected with the first auxiliary passable point and the initial second auxiliary passable point as the center respectively;

[0100] Step B6: Calculate the distance cost value in the candidate window area according to the obstacle distribution, and use the point with the minimum distance cost as the first auxiliary passable point and the second auxiliary passable point.

[0101] like Figure 5 As shown, the relative positions of the first auxiliary circle and the second auxiliary circle are separated, and the first circle center C1 and the second circle center C2 are connected to generate the second auxiliary line C1C2, which are respectively compared with C3 and C4 of the first auxiliary circle and the second auxiliary circle. Then, the midpoint of C3C4 is used as the key passable point P0 (x0, y0), and then a first vertical line passing through the key passable point P0 and perpendicular to C1C2 is generated. On the first vertical line, according to a preset length (for example, 1.5Rmax, Rmax is the radius of the largest circle of the two auxiliary circles), the two end points P1' and P2' are obtained as the first initial auxiliary passable point and the second initial auxiliary passable point; with P1' and P2' as the center, the window areas to be selected are respectively generated according to the areas of the auxiliary passable points that may be selected, and then the distance cost value of each point in the area is calculated according to the distribution of obstacles, and the point with the minimum distance cost is used as the first auxiliary passable point P1 and the second auxiliary passable point P2.

[0102] In this embodiment, the third preset rule is:

[0103] Step C1, taking the intersection of the first auxiliary circle and the second auxiliary circle as the first initial auxiliary passable point and the second initial auxiliary passable point;

[0104] Step C2, connecting the intersection points of the first auxiliary circle and the second auxiliary circle to generate a third auxiliary line;

[0105] Step C3, taking the midpoint of the third auxiliary line as a key passable point;

[0106] Step C4, generating a window area to be selected with the first auxiliary passable point and the initial second auxiliary passable point as the center respectively;

[0107] Step C5: Calculate the distance cost value in the candidate window area according to the obstacle distribution, and use the point with the minimum distance cost as the first auxiliary passable point and the second auxiliary passable point.

[0108] like Figure 6 As shown, the relative positions of the first auxiliary circle and the second auxiliary circle are intersecting, and the intersection points are C5 and C6. C5 and C6 are connected to generate the third auxiliary line C5C6, then the midpoint of C5C6 is used as the key passable point P0 (x0, y0), and the intersection points C5 and C6 are used as the first initial auxiliary passable point P1' and the second initial auxiliary passable point P2'; with P1' and P2' as the center, the selected window areas are respectively generated according to the areas of the auxiliary passable points that may be selected, and then the distance cost value of each point is calculated in the area according to the distribution of obstacles, the initial auxiliary passable points are adjusted according to the preset constraints, and the points with the minimum distance cost are used as the first auxiliary passable point P1 and the second auxiliary passable point P2.

[0109] In this embodiment, the preset constraint condition is:

[0110] (1) The initial assistance can satisfy the point that the vehicle model V has no intersection with each obstacle;

[0111] (2) Determine the shortest distance between the initial assistance point X and each obstacle:

[0112]

[0113] Where p0 is the point on the obstacle closest to the initial auxiliary passable point X.

[0114] (3) Determine the distance D(X) from the initial assistance point X to its nearest obstacle:

[0115] D(X)=in i∈1...N d i ()

[0116] (4) The initial assistance can be achieved by matching point X with the first auxiliary circle, the second auxiliary circle, or an obstacle equivalent to O. i and O j The shortest distance is equal, that is, D(X)= i (X)= j (X).

[0117] Step 204, determine the target arc according to the key passable point and the first auxiliary passable point and the second auxiliary passable point, the target arc is the target reference line, the point on the target arc is the target point, and at the same time obtain the tangent of the key passable point, and use the angle of the tangent as the yaw angle of the target point.

[0118] Step 3: Perform hybrid A* trajectory planning based on the target point and the passable area, calculate and record the trajectory of the vehicle during driving, and determine whether the trajectory planning fails. If yes, proceed to the next step, otherwise repeat this step.

[0119] In this embodiment, the hybrid A* trajectory planning based on the traversable area includes:

[0120] Step D1: Get the current position of the vehicle as the current node, and expand the surrounding nodes to obtain several nodes to be selected.

[0121] In this embodiment, the node represents information such as coordinate position, cost value, and path of parent node extension.

[0122] In this embodiment, the expanding surrounding nodes includes: performing node expansion according to a first preset formula, where the first preset formula is:

[0123]

[0124] Among them, x(t), y(t) represent the coordinates of the midpoint of the rear axle of the vehicle, θ(t) represents the yaw angle of the vehicle, v(t) represents the instantaneous speed, and L w Indicates wheelbase, Indicates the front wheel turning angle.

[0125] In this embodiment, the expanding the surrounding nodes includes: performing node expansion according to a second preset formula, where the second preset formula is:

[0126]

[0127] Among them, (x A ,y A ) represents the vector before rotation, (x B ,y B ) represents the rotated vector, and θ represents the rotation angle.

[0128] Step D2: Calculate the cost value of the candidate nodes and select the node with the smallest cost value as the next node.

[0129] In this embodiment, in order to improve planning efficiency, the node with the smallest cost value is selected for expansion. The cost value formula is:

[0130] F(n)=g(n)+h(n)

[0131] Among them, g(n) represents the distance between the starting point and the current node, and h(n) represents the distance between the current node and the end point.

[0132] Considering the narrow road scenario, it is also necessary to avoid frequent gear switching, large and fast turns, and collision risks. Therefore, the following objective function is constructed:

[0133]

[0134] Where ω is the weight, X i is the position vector of the ith node, O i is the position vector of the nearest obstacle corresponding to the i-th node, σ is the collision penalty function, is the steering wheel angle of the current node, d n is the direction of the current node.

[0135] h(n) is divided into the cases of considering kinematic constraints and obstacles: for considering obstacles but not kinematic constraints, the Djikstra algorithm is generally used to calculate the distance value of each grid node first, and then the distance value of the node is found when the node is searched; for considering kinematic constraints but not obstacles, the end points are first connected with the Dubins curve or RS curve, and the arc lengths of all arcs and straight line lengths are calculated to obtain the distance.

[0136] Therefore, the h(n) function is constructed as follows:

[0137] h(n)=ω1h1(n)+2h2(n)

[0138] Among them, h1(n) means considering kinematic constraints but not obstacles, h2(n) means considering obstacles but not kinematic constraints, and ω1 and ω2 represent weights.

[0139] Step D3: According to all the nodes and target points, the path is extended by using the Dubins curve through the hybrid A* algorithm to obtain the driving trajectory.

[0140] The Dubins curve is the shortest path connecting two points under the conditions of satisfying the curvature constraint and the specified tangent direction of the start and end. Since the Dubins curve only allows the vehicle to move in the forward direction, it is suitable for obstacle avoidance in scenarios where frequent gear changes are not required. Without considering the vehicle's reverse movement, the vehicle's movement can be divided into straight lines, left turns, and right turns; the shortest distance between any two points can always be expressed as the three basic motion combinations mentioned above, and the turning radius is the minimum turning radius. The shortest path can be selected from the following combinations: {LSL, LSR, RSL, RSR}.

[0141] Using the Dubins curve to perform path expansion is an existing technology and will not be described in detail here.

[0142] In another embodiment of the present invention, after step D3, the method further includes:

[0143] Step D4: smoothing the driving trajectory.

[0144] Since the Dubins curve is composed of arcs and straight lines, the curvature of the straight line is 0 and the curvature of the arc is Therefore, there is a curvature jump problem in the transition from arc to straight line. The solution is to add a clothoid transition. The curvature of the clothoid changes linearly from zero, which can well connect between arcs and arcs, and between arcs and straight lines to achieve a smooth transition.

[0145] The curvature of the clothoid curve is continuous, and the curvature is proportional to the length of the trajectory:

[0146]

[0147] In the formula, s is the length of the curve, R is the radius of curvature of the corresponding length, and Q is the rate of change of curvature with the length of the curve, which is a constant.

[0148] In this embodiment, calculating and recording the trajectory of the vehicle during driving includes:

[0149] Figure 7 This is a schematic diagram of the vehicle motion plane model of this embodiment. The parking condition is a low-speed condition, the vehicle has no side slip, and the center point of the vehicle's rear axle is used as the reference point. In the figure, Δθ is the yaw angle change, R is the turning radius, and L axies is the wheelbase, L wheels is the wheelbase, θ(i) and θ(i+1) are the yaw angles at the previous moment and the current moment respectively, ΔC is the distance traveled by the vehicle, λ is the direction of travel of the vehicle, λ=1 means forward, λ=-1 means backward. It is easy to understand that when the vehicle moves in a curve, ΔC is the length of an arc, and the arc length traveled by the whole vehicle can be obtained from the average arc length of the left and right rear wheels, and ΔL is the chord length corresponding to the arc trajectory when the vehicle moves in a curve.

[0150] There is a difference in the change of yaw angle during straight driving and curved driving: when driving in a straight line, the change of yaw angle is 0, that is, |Δθ|=0; when driving in a curve, the change of yaw angle is not 0, that is, |Δθ|≠0.

[0151] Step E1, calculating the yaw angle change Δθ according to a first preset formula.

[0152] In this embodiment, the first preset formula is:

[0153]

[0154] Among them, ΔC represents the vehicle's travel distance, Δδ represents the change in the vehicle's front wheel angle, and L axies Indicates wheelbase.

[0155] Step E2, judging the driving type of the vehicle according to the yaw angle change Δθ, if it is straight driving, calculating the coordinates and yaw angle of the rear axle of the vehicle at the current moment according to the second preset formula, if it is curved driving, calculating the coordinates and yaw angle of the rear axle of the vehicle at the current moment according to the third formula.

[0156] In this embodiment, the second preset formula is:

[0157] X(i+1)=X(i)+λΔCcos(θ(i))

[0158] Y(i+1)=Y(i)+λΔCsin(θ(i))

[0159] θ(i+1)=θ(i)

[0160] The third preset formula is:

[0161]

[0162]

[0163] θ(i+1)=θ(i)+Δθ

[0164] Where x(i) and y(i) represent the coordinates of the rear axle of the vehicle at the previous moment, and x(i+1) and y(i+1) represent the coordinates of the rear axle of the vehicle at the current moment; θ(i) represents the yaw angle at the previous moment, and θ(i+1) represents the yaw angle at the current moment; Δθ represents the change in yaw angle; ΔC represents the distance traveled by the vehicle; Δδ represents the change in the front wheel steering angle of the vehicle; λ represents the direction of travel of the vehicle, λ=1 represents forward movement, and λ=-1 represents backward movement; L axies Indicates wheelbase.

[0165] Step E3: record the coordinates and yaw angle of the rear axle of the vehicle at all times.

[0166] Step 4: Read the recorded historical driving trajectory, perform tracking and fallback, and return to step 2.

[0167] Based on the real-time recorded trajectory in step 3, the trajectory is from the vehicle's starting position to the current position. To avoid returning to the starting point, a trajectory segment of a preset length (e.g., 1m) from the current position is intercepted, and the vehicle is controlled to track to the specified position using a tracking algorithm, and then returns to step 2.

[0168] The tracking algorithm can adopt PP algorithm, Stanley and other geometric algorithms and PID algorithm, etc. These classic algorithms are existing technologies and will not be described in detail here.

[0169] This embodiment records the path information of the vehicle as it moves forward and adopts the tracking technology. The vehicle can perform a reverse operation along the recorded path and retreat to a specified position, thereby maintaining sufficient obstacle avoidance space with obstacles, so that the hybrid A* algorithm can successfully plan the driving trajectory.

[0170] The above disclosure is only a preferred embodiment of the present invention, which cannot be used to limit the scope of protection of the present invention. Therefore, equivalent changes made according to the scope of the patent application of the present invention are still within the scope covered by the present invention.

Claims

1. A trajectory planning method, characterized in that: include: Step 1: Determine the boundaries, obstacles and vehicle postures in the narrow road scene, establish a narrow road traffic model, and establish a local coordinate system for trajectory planning according to the vehicle posture; Step 2: determining a target reference line and a target point in the narrow passage model; Step 3: Perform hybrid A* trajectory planning based on the target point and the passable area, calculate and record the trajectory of the vehicle during driving, and determine whether the trajectory planning fails. If yes, proceed to the next step, otherwise repeat this step; Step 4: Read the recorded historical driving trajectory, perform tracking and fallback, and return to step 2.

2. The trajectory planning method according to claim 1, characterized in that: The step 2 comprises: Step 201: Establish a front view of the vehicle and determine the narrow road boundary and obstacle distribution; Step 202: determine whether there is an obstacle in the narrow passage. If yes, establish a circumscribed circle of the obstacle with the center point of the obstacle as the center as a first auxiliary circle, and take the corner point of the narrow passage opposite to the obstacle as an equivalent obstacle, and establish a second auxiliary circle with the center point of the equivalent obstacle and a preset radius. Otherwise, take the corner points on both sides of the narrow passage as equivalent obstacles, and establish a first auxiliary circle and a second auxiliary circle with the center point of the equivalent obstacle and a preset radius, respectively. Step 203: determining a key passable point and a first auxiliary passable point and a second auxiliary passable point according to the relative positions of the first auxiliary circle and the second auxiliary circle; Step 204, determine the target arc according to the key passable point and the first auxiliary passable point and the second auxiliary passable point, the target arc is the target reference line, the point on the target arc is the target point, and at the same time obtain the tangent of the key passable point, and use the angle of the tangent as the yaw angle of the target point.

3. The trajectory planning method according to claim 2, characterized in that: The step 203 includes: if the relative positions of the first auxiliary circle and the second auxiliary circle are tangent, determining the key passable point and the first auxiliary passable point and the second auxiliary passable point according to a first preset rule, wherein the first preset rule is: Step A1: Use the tangent point as the key passable point; Step A2, connecting the key passable point, the first circle center, and the second circle center to generate a first auxiliary line; The first circle center is the center of the first auxiliary circle, and the second circle center is the center of the second auxiliary circle; Step A3, generating a perpendicular line of the first auxiliary line through the key passable point, recorded as a first perpendicular line; Step A4, taking the key passable point as the center, selecting two endpoints on the first vertical line according to a preset length as an initial first auxiliary passable point and an initial second auxiliary passable point; Step A5, generating a window area to be selected with the first auxiliary passable point and the initial second auxiliary passable point as the center respectively; Step A6: Calculate the distance cost value in the candidate window area according to the obstacle distribution, and use the point with the minimum distance cost as the first auxiliary passable point and the second auxiliary passable point.

4. The trajectory planning method according to claim 2, characterized in that: The step 203 includes: if the relative positions of the first auxiliary circle and the second auxiliary circle are separated, determining the key passable point and the first auxiliary passable point and the second auxiliary passable point according to a second preset rule, wherein the second preset rule is: Step B1, connecting the first circle center and the second circle center to generate a second auxiliary line; Step B2, taking the midpoint of the line segment of the second auxiliary line between the two circles as the key passable point; Step B3, generating a perpendicular line of the second auxiliary line through the key passable point, recorded as a second perpendicular line; Step B4, taking the key passable point as the center, selecting two endpoints on the second vertical line according to a preset length as an initial first auxiliary passable point and an initial second auxiliary passable point; Step B5, generating a window area to be selected with the first auxiliary passable point and the initial second auxiliary passable point as the center respectively; Step B6: Calculate the distance cost value in the candidate window area according to the obstacle distribution, and use the point with the minimum distance cost as the first auxiliary passable point and the second auxiliary passable point.

5. The trajectory planning method according to claim 2, characterized in that: The step 203 includes: if the relative positions of the first auxiliary circle and the second auxiliary circle intersect, determining the key passable point and the first auxiliary passable point and the second auxiliary passable point according to a third preset rule, wherein the third preset rule is: Step C1, taking the intersection of the first auxiliary circle and the second auxiliary circle as the first initial auxiliary passable point and the second initial auxiliary passable point; Step C2, connecting the intersection points of the first auxiliary circle and the second auxiliary circle to generate a third auxiliary line; Step C3, taking the midpoint of the third auxiliary line as a key passable point; Step C4, generating a window area to be selected with the first auxiliary passable point and the initial second auxiliary passable point as the center respectively; Step C5: Calculate the distance cost value in the candidate window area according to the obstacle distribution, and use the point with the minimum distance cost as the first auxiliary passable point and the second auxiliary passable point.

6. The trajectory planning method according to claim 1, characterized in that: The hybrid A* trajectory planning based on the traversable area includes: Step D1, obtaining the current position of the vehicle as the current node, and expanding the surrounding nodes to obtain several nodes to be selected; Step D2: Calculate the cost value of the candidate nodes and select the node with the smallest cost value as the next node; Step D3: According to all the nodes and target points, the path is extended by using the Dubins curve through the hybrid A* algorithm to obtain the driving trajectory.

7. The trajectory planning method according to claim 6, characterized in that: The calculating and recording of the trajectory of the vehicle during driving includes: Step E1, calculating the yaw angle change according to a first preset formula; Step E2, judging the driving type of the vehicle according to the yaw angle change, if it is straight driving, calculating the coordinates and yaw angle of the rear axle of the vehicle at the current moment according to the second preset formula, if it is curved driving, calculating the coordinates and yaw angle of the rear axle of the vehicle at the current moment according to the third preset formula; Step E3: record the coordinates and yaw angle of the rear axle of the vehicle at all times.

8. The trajectory planning method according to claim 7, characterized in that: The first preset formula is: Among them, ΔC represents the vehicle's travel distance, Δδ represents the change in the vehicle's front wheel angle, and L axies Indicates wheelbase.

9. The trajectory planning method according to claim 8, characterized in that: The second preset formula is: X(i+1)=X(i)+λΔCcos(θ(i)) Y(i+1)=Y(i)+λΔCsin(θ(i)) θ(i+1)=θ(i) Where x(i) and y(i) represent the coordinates of the rear axle of the vehicle at the previous moment, and x(i+1) and y(i+1) represent the coordinates of the rear axle of the vehicle at the current moment; θ(i) represents the yaw angle at the previous moment, and θ(i+1) represents the yaw angle at the current moment; ΔC represents the distance traveled by the vehicle; λ represents the direction of travel of the vehicle, λ=1 represents forward movement, and λ=-1 represents backward movement; L axies Indicates wheelbase.

10. The trajectory planning method according to claim 8, characterized in that: The third preset formula is: θ(i+1)=θ(i)+Δθ Where x(i) and y(i) represent the coordinates of the rear axle of the vehicle at the previous moment, and x(i+1) and y(i+1) represent the coordinates of the rear axle of the vehicle at the current moment; θ(i) represents the yaw angle at the previous moment, and θ(i+1) represents the yaw angle at the current moment; Δθ represents the change in yaw angle; Δδ represents the change in the front wheel steering angle of the vehicle; λ represents the direction of vehicle travel, λ=1 represents forward movement, and λ=-1 represents backward movement; L axies Indicates wheelbase.

Images