A trajectory planning method

Abstract

This invention provides a trajectory planning method, comprising: Step 1, determining the boundaries, obstacles, and vehicle poses in a narrow passage scenario, establishing a narrow passage traffic model, and establishing a local coordinate system for trajectory planning based on the vehicle poses; Step 2, determining the target reference line and target point in the narrow passage traffic model; Step 3, performing hybrid A* trajectory planning based on the target point and the passable area, simultaneously calculating and recording the vehicle's trajectory during travel, and determining whether the trajectory planning has failed. If it has, proceed to the next step; otherwise, repeat this step; Step 4, reading the recorded historical driving trajectory, performing line tracking and backtracking, and returning to Step 2. This invention effectively avoids the situation where current path planning algorithms are stuck in narrow passages due to obstacles, thus improving the success rate of trajectory planning.

Description

Technical Field

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

[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 vehicle passage faces greater challenges. Path planning and local obstacle avoidance technologies are among the key technologies to ensure safe vehicle operation. Existing path planning algorithms, such as the hybrid A* algorithm, can provide effective path planning in most cases. However, when a vehicle gets stuck in narrow roads or complex environments, path planning fails, and the vehicle cannot escape the predicament. Therefore, existing trajectory planning techniques still have certain limitations, and a new trajectory planning method is urgently needed to improve the vehicle's obstacle avoidance and extrication capabilities in narrow roads and complex environments. Summary of the Invention

[0003] This invention provides a trajectory planning method that aims to overcome the shortcomings of existing technologies, effectively avoid the situation where current path planning algorithms get stuck in narrow passages due to obstacles and cannot get out, and improve the success rate of trajectory planning.

[0004] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

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

[0006] Step 1: Determine the boundaries, obstacles, and vehicle poses in the narrow passage scenario, establish a narrow passage traffic model, and establish a local coordinate system for trajectory planning based on the vehicle poses;

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

[0008] Step 3: Based on the target point, perform hybrid A* trajectory planning based on the passable area, calculate and record the trajectory of the vehicle during its journey, and determine whether the trajectory planning has failed. If it has, proceed to the next step; otherwise, repeat this step.

[0009] Step 4: Read the recorded historical driving trajectory, perform track tracing and rewind, and return to Step 2.

[0010] Specifically, step 2 includes:

[0011] Step 201: Create a front view of the vehicle to determine the narrow passage boundaries and obstacle distribution;

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

[0013] Step 203: Based on the relative positions of the first auxiliary circle and the second auxiliary circle, determine the key passable point and the first and second auxiliary passable points;

[0014] Step 204: Determine the target arc based on the key passable point, the first auxiliary passable point, and the second auxiliary passable point. The target arc is the target reference line, and the point on the target arc is the target point. 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, then determine the key passable point, 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: Connect the key points, the first center of the circle, and the second center of the circle to generate the first auxiliary line;

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

[0019] Step A3: Generate a perpendicular line to the first auxiliary line through the key passable point, and denot it as the first perpendicular line;

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

[0021] Step A5: Generate candidate window regions centered on the first auxiliary passable point and the initial second auxiliary passable point, respectively;

[0022] Step A6: Calculate the distance cost based on the obstacle distribution in the candidate window area, and select the point with the minimum distance cost as the first and second auxiliary passable points.

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

[0024] Step B1: Connect the first center and the second center to generate the second auxiliary line;

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

[0026] Step B3: Generate a perpendicular line to the second auxiliary line through the key passable point, and denote it as the second perpendicular line;

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

[0028] Step B5: Generate candidate window regions centered on the first auxiliary passable point and the initial second auxiliary passable point, respectively;

[0029] Step B6: Calculate the distance cost based on the obstacle distribution in the candidate window area, and select the point with the minimum distance cost as the first and second auxiliary passable points.

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

[0031] Step C1: Take 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: Connect the intersection points of the first auxiliary circle and the second auxiliary circle to generate the third auxiliary line;

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

[0034] Step C4: Generate candidate window regions centered on the first auxiliary passable point and the initial second auxiliary passable point, respectively;

[0035] Step C5: Calculate the distance cost based on the obstacle distribution in the candidate window area, and select the point with the minimum distance cost as the first and second auxiliary passable points.

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

[0037] Step D1: Obtain the vehicle's current location as the current node, and expand the surrounding nodes to obtain several candidate nodes;

[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: Based on all the nodes and target points, use the Dubins curve and the hybrid A* algorithm to expand the path and obtain the driving trajectory.

[0040] Specifically, calculating and recording the vehicle's trajectory during its journey includes:

[0041] Step E1: Calculate the change in yaw angle according to the first preset formula;

[0042] Step E2: Determine the vehicle's driving type based on the change in yaw angle. If the vehicle is driving in a straight line, calculate the coordinates of the rear axle and the yaw angle at the current moment according to the second preset formula. If the vehicle is driving on a curve, calculate the coordinates of the rear axle and the yaw angle at the current moment according to the third preset formula.

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

[0044] Specifically, the first preset formula is:

[0045]

[0046] Where ΔC represents the distance traveled by the vehicle, Δδ represents the change in the front wheel steering angle, and L axies This represents the wheelbase. 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 indicates forward movement, and λ = -1 indicates 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; λ represents the vehicle's direction of travel, λ = 1 for forward movement, and λ = -1 for backward movement; L axies Indicates wheelbase.

[0056] The beneficial effects of this invention are as follows: This invention obtains information on passable areas, calculates the target point for passing through narrow passages, expands trajectory nodes, calculates the Dubins curve and records the vehicle trajectory in real time. When path planning fails, it combines a tracking algorithm to track the historical trajectory and retreat a preset distance to increase the obstacle avoidance space. The above steps are repeated until the Dubins curve / target point is found and the route is successfully planned. This effectively avoids the situation where current path planning algorithms are stuck in narrow passages and cannot get out. Attached Figure Description

[0057] Figure 1 This is a flowchart illustrating the trajectory planning method of the present invention;

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

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

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

[0061] Figure 5 This is a schematic diagram of the two auxiliary circles being separated in this invention;

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

[0063] Figure 7 This is a schematic diagram of the vehicle motion planar model of the present invention. Detailed Implementation

[0064] The embodiments of the present invention are described in detail below with reference to the accompanying drawings. The drawings are for reference and illustration only and do not constitute a limitation on the scope of protection of the present invention.

[0065] In the process described in the specification, claims, or drawings of this invention, each step is numbered (e.g., step 10, 20, etc.). These numbers are used only to distinguish the steps and do not represent any execution order. It should be noted that the terms "first," "second," etc., used herein are only for distinguishing the objects being described and do not represent a chronological order, nor do they indicate that "first," "second," etc., are different types.

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

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

[0068] In practical implementation, the perception module obtains the coordinates of the boundary, obstacles, passable area, and vehicle pose in the narrow passage scene, establishes a narrow passage passage model, and establishes a local coordinate system for trajectory planning based on the vehicle pose. The horizontal axis of the local coordinate system is parallel to the entrance line of the narrow passage, the vertical axis is parallel to the side line of the narrow passage, and the origin of the coordinate system is obtained by offsetting the vehicle coordinate system. In other words, the narrow passage 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, and L is the length of the narrow lane. p Let C be the length of the obstacle and C be the center point of the rear axle.

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

[0071] In this embodiment, the target reference line is a circular driving trajectory at a preset distance from the narrow passage exit, and the target point refers to the target location that the vehicle wants to reach.

[0072] In this embodiment, step 2 includes:

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

[0074] As shown in Figure 3, the front view is 3.5m long and 6m 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 so, establish the circumcircle of the obstacle as the center point of the obstacle as the first auxiliary circle, and take the corner point of the narrow passage opposite the obstacle as the equivalent obstacle. Establish the second auxiliary circle with the center of the equivalent obstacle and a preset radius. Otherwise, take the corner points on both sides of the narrow passage as the equivalent obstacle, and establish the first auxiliary circle and the second auxiliary circle with the center of the equivalent obstacle and a preset radius, respectively.

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

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

[0078] The specific method for establishing the circumcircle of an obstacle is as follows: first, form a convex hull, i.e., a convex polygon, from the irregular obstacle, and then find the circumcircle. The radius of the circle is determined according to the size of the obstacle.

[0079] There are various algorithms for implementing convex hulls, such as the extreme point method, the extreme edge method, the convolute method, and the divide-and-conquer method. This invention selects the Graham-Scan algorithm as the algorithm for implementing convex hulls.

[0080] Step 203: Based on the relative positions of the first auxiliary circle and the second auxiliary circle, determine the key passable point and the first and second auxiliary passable points.

[0081] In this embodiment, step 203 includes:

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

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

[0084] If the relative positions of the first auxiliary circle and the second auxiliary circle intersect, then the key passable point, the first auxiliary passable point, and the second auxiliary passable point are determined according to the 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: Connect the key points, the first center of the circle, and the second center of the circle to generate the 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: Generate a perpendicular line to the first auxiliary line through the key passable point, and denot it as the first perpendicular line;

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

[0091] Step A5: Generate candidate window regions centered on the first auxiliary passable point and the initial second auxiliary passable point, respectively;

[0092] Step A6: Calculate the distance cost based on the obstacle distribution in the candidate window area, and select the point with the minimum distance cost as the first and second auxiliary passable points.

[0093] like Figure 4 As shown, the first auxiliary circle and the second auxiliary circle are tangent to each other. The point of tangency is taken 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 the first auxiliary line C1C2. Then, a first perpendicular line is generated that passes through the key passable point P0 and is perpendicular to C1C2. On the first perpendicular line, the two endpoints P1' and P2' are obtained according to a preset length (e.g., 1.5Rmax, where Rmax is the radius of the larger 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, a candidate window area is generated for each of the possible selectable auxiliary passable point areas. Then, the distance cost of each point in the area is calculated according to the distribution of obstacles. The point with the minimum distance cost is taken 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: Connect the first center and the second center to generate the second auxiliary line;

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

[0097] Step B3: Generate a perpendicular line to the second auxiliary line through the key passable point, and denote it as the second perpendicular line;

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

[0099] Step B5: Generate candidate window regions centered on the first auxiliary passable point and the initial second auxiliary passable point, respectively;

[0100] Step B6: Calculate the distance cost based on the obstacle distribution in the candidate window area, and select the point with the minimum distance cost as the first and second auxiliary passable points.

[0101] like Figure 5 As shown, the first auxiliary circle and the second auxiliary circle are separated. Connect the center C1 of the first circle and the center C2 of the second circle to generate the second auxiliary line C1C2. Compare the second auxiliary circle with C3 and C4 respectively. Then, the midpoint of C3C4 is taken as the key passable point P0(x0,y0). Then, a first perpendicular line is generated that passes through the key passable point P0 and is perpendicular to C1C2. On the first perpendicular line, according to the preset length (e.g., 1.5Rmax, where Rmax is the radius of the larger of the two auxiliary circles), the two endpoints 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, a candidate window area is generated for each of the possible selectable auxiliary passable point areas. Then, the distance cost of each point in the area is calculated according to the distribution of obstacles. The point with the minimum distance cost is taken 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: Take 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: Connect the intersection points of the first auxiliary circle and the second auxiliary circle to generate the third auxiliary line;

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

[0106] Step C4: Generate candidate window regions centered on the first auxiliary passable point and the initial second auxiliary passable point, respectively;

[0107] Step C5: Calculate the distance cost based on the obstacle distribution in the candidate window area, and select the point with the minimum distance cost as the first and second auxiliary passable points.

[0108] like Figure 6 As shown, the first auxiliary circle and the second auxiliary circle intersect at points C5 and C6. Connecting C5 and C6 generates a third auxiliary line C5C6. The midpoint of C5C6 is taken as the key passable point P0(x0,y0), and the intersection points C5 and C6 are taken as the first initial auxiliary passable point P1' and the second initial auxiliary passable point P2'. Centered on P1' and P2', candidate window regions are generated according to the regions of possible selectable auxiliary passable points. Then, the distance cost of each point in the region is calculated according to the distribution of obstacles. The initial auxiliary passable points are adjusted according to preset constraints, and the point with the minimum distance cost is taken 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 condition that the vehicle model V has no intersection with each obstacle through the point;

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

[0112]

[0113] Where p0 is the point on the obstacle that is closest to the initial assist point X.

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

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

[0116] (4) The initial assistance can be achieved by connecting point X with the first auxiliary circle and the second auxiliary circle, and then connecting them with corresponding or equivalent obstacles O. i and O j The shortest distances are equal, i.e., D(X) = i (X)= j (X).

[0117] Step 204: Determine the target arc based on the key passable point, the first auxiliary passable point, and the second auxiliary passable point. The target arc is the target reference line, and the point on the target arc is the target point. 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: Based on the target point, perform hybrid A* trajectory planning based on the passable area, calculate and record the trajectory of the vehicle during its journey, and determine whether the trajectory planning has failed. If it has, proceed to the next step; otherwise, repeat this step.

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

[0120] Step D1: Obtain the vehicle's current location as the current node, and expand the surrounding nodes to obtain several candidate nodes.

[0121] In this embodiment, the node represents information such as coordinate location, cost, and the path extended by the parent node.

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

[0123]

[0124] Where x(t) and 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 velocity, and L w Indicates wheelbase. Indicates the steering angle of the front wheels.

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

[0126]

[0127] Among them, (x A ,y A (x) represents the vector before rotation. 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, to improve planning efficiency, the node with the lowest cost value is selected for expansion. The cost value formula is:

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

[0131] Where 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 ending point.

[0132] Considering the narrow road scenario, it is also necessary to avoid frequent gear shifting, sharp and rapid turns, and the risk of collisions. Therefore, the following objective function is constructed:

[0133]

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

[0135] h(n) is divided into cases that consider kinematic constraints and those that consider obstacles: For cases that consider obstacles but not kinematic constraints, the distance value of each grid node is usually calculated first using the Djikstra algorithm, and then the distance value of the node is searched when the node is found; For cases that consider kinematic constraints but not obstacles, the endpoint is first connected using the Dubins curve or RS curve, and the distance is obtained by calculating the arc length of all circular arcs and the length of straight lines.

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

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

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

[0139] Step D3: Based on all the nodes and target points, use the Dubins curve and the hybrid A* algorithm to expand the path and obtain the driving trajectory.

[0140] The Dubins curve is the shortest path connecting two points, provided that curvature constraints and specified tangent directions at the beginning and end points are met. Since the Dubins curve only allows forward vehicle movement, it is suitable for obstacle avoidance scenarios where frequent gear shifts are not required. Without considering vehicle reversal, vehicle movement can be categorized into straight-line, left-turn, and right-turn movements; the shortest distance between any two points can always be represented by a combination of these three basic movements, with the turning radius always being the minimum turning radius. The shortest path can be selected from the following combinations: {LSL, LSR, RSL, RSR}.

[0141] Path extension using Dubins curves is an existing technique and will not be elaborated upon here.

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

[0143] Step D4: Smooth the driving trajectory.

[0144] Since the Dubins curve is composed of arcs and straight lines, the curvature of the straight lines is 0, while the curvature of the arcs is... Therefore, the transition from an arc to a straight line presents a curvature jump problem. A solution is to add a spiral curve for the transition. The curvature of a spiral curve changes linearly from zero, which can seamlessly connect arcs and straight lines, achieving a smooth transition.

[0145] The curvature of the cycloid is continuous, and the magnitude of the curvature is proportional to the length of the trajectory.

[0146]

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

[0148] In this embodiment, calculating and recording the vehicle's trajectory during its driving process includes:

[0149] Figure 7 This is a schematic diagram of the vehicle motion planar model in this embodiment. The parking condition is a low-speed condition, with no vehicle sideslip, and the center point of the rear axle is used as the reference point. In the diagram, Δθ is the change in yaw angle, R is the turning radius, and L... axies It's the wheelbase, L wheels Here, θ(i) and θ(i+1) are the yaw angles at the previous and current moments, respectively. ΔC is the distance the vehicle has traveled, and λ is the direction of travel, where λ = 1 indicates forward movement and λ = -1 indicates backward movement. It's easy to understand that when the vehicle is moving along a curve, ΔC is the length of an arc, which can be obtained from the average arc length of the left and right rear wheels. ΔL is the chord length corresponding to the arc trajectory of the vehicle during its curved movement.

[0150] The change in yaw angle differs between straight-line driving and curved-line driving: when driving straight, the change in yaw angle is 0, i.e., |Δθ|=0; when driving on a curve, the change in yaw angle is not 0, i.e., |Δθ|≠0.

[0151] Step E1: Calculate the change in yaw angle Δθ according to the first preset formula.

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

[0153]

[0154] Where ΔC represents the distance traveled by the vehicle, Δδ represents the change in the front wheel steering angle, and L axies Indicates wheelbase.

[0155] Step E2: Determine the vehicle's driving type based on the change in yaw angle Δθ. If the vehicle is driving in a straight line, calculate the coordinates of the rear axle and the yaw angle at the current moment according to the second preset formula. If the vehicle is driving on a curve, calculate the coordinates of the rear axle and the yaw angle 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 vehicle's rear axle at all times.

[0166] Step 4: Read the recorded historical driving trajectory, perform track tracing and rewind, and return to Step 2.

[0167] Based on the trajectory recorded in real time in step 3, which is the trajectory from the vehicle's starting position to its current position. To avoid returning to the starting point, a trajectory segment of a preset length (e.g., 1m) is extracted from the current position. A tracking algorithm is used to control the vehicle to follow the trajectory to the designated position, and then the vehicle returns to step 2.

[0168] Line tracking algorithms can employ PP algorithm, Stanley algorithm and other geometric algorithms, as well as PID algorithm, etc. These classic algorithms are existing technologies and will not be elaborated on here.

[0169] This embodiment records the path information of the vehicle as it moves forward, and combines it with line tracking technology. The vehicle can reverse along the recorded path to a designated position, thereby maintaining sufficient obstacle avoidance space and enabling the hybrid A* algorithm to successfully plan the driving trajectory.

[0170] The above-disclosed embodiments are merely preferred embodiments of the present invention and should not be construed as limiting the scope of protection of the present invention. Therefore, any equivalent variations made in accordance with the claims of the present invention are still within the scope of the present invention.

Claims

1. A trajectory planning method, characterized in that, include: Step 1: Determine the boundaries, obstacles, and vehicle poses in the narrow passage scenario, establish a narrow passage traffic model, and establish a local coordinate system for trajectory planning based on the vehicle poses; Step 2: Determine the target reference line and target point in the narrow passage model; Step 3: Based on the target point, perform hybrid A* trajectory planning based on the passable area, calculate and record the trajectory of the vehicle during its journey, and determine whether the trajectory planning has failed. If it has, proceed to the next step; otherwise, repeat this step. Step 4: Read the recorded historical driving trajectory, perform track tracing and rewind, and return to Step 2; Specifically, step 2 includes: Step 201: Create a front view of the vehicle to determine the narrow passage boundaries and obstacle distribution; Step 202: Determine whether there is an obstacle in the narrow passage. If so, establish the circumcircle of the obstacle as the center point of the obstacle as the first auxiliary circle, and take the corner point of the narrow passage opposite the obstacle as the equivalent obstacle. Establish the second auxiliary circle with the center of the equivalent obstacle and a preset radius. Otherwise, take the corner points on both sides of the narrow passage as the equivalent obstacle, and establish the first auxiliary circle and the second auxiliary circle with the center of the equivalent obstacle and a preset radius, respectively. Step 203: Based on the relative positions of the first auxiliary circle and the second auxiliary circle, determine the key passable point and the first and second auxiliary passable points; Step 204: Determine the target arc based on the key passable point, the first auxiliary passable point, and the second auxiliary passable point. The target arc is the target reference line, and the point on the target arc is the target point. 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. Specifically, step 203 includes: if the relative positions of the first auxiliary circle and the second auxiliary circle are tangent, then determine the key passable point, the first auxiliary passable point, and the second auxiliary passable point according to a first preset rule. The first preset rule is: Step A1: Use the tangent point as the key passable point; Step A2: Connect the key points, the first center of the circle, and the second center of the circle to generate the first auxiliary line; The first center is the center of the first auxiliary circle, and the second center is the center of the second auxiliary circle; Step A3: Generate a perpendicular line to the first auxiliary line through the key passable point, and denot it as the first perpendicular line; Step A4: Using the key passable point as the center, select two endpoints on the first vertical line according to a preset length as the initial first auxiliary passable point and the initial second auxiliary passable point; Step A5: Generate candidate window regions centered on the first auxiliary passable point and the initial second auxiliary passable point, respectively; Step A6: Calculate the distance cost based on the obstacle distribution in the candidate window area, and select the point with the minimum distance cost as the first and second auxiliary passable points.

2. The trajectory planning method according to claim 1, characterized in that, Step 203 includes: if the relative positions of the first auxiliary circle and the second auxiliary circle are disjoint, then the key passable point, the first auxiliary passable point, and the second auxiliary passable point are determined according to a second preset rule, wherein the second preset rule is: Step B1: Connect the first center and the second center to generate the second auxiliary line; Step B2: Take the midpoint of the line segment of the second auxiliary line located between the two circles as the key passable point; Step B3: Generate a perpendicular line to the second auxiliary line through the key passable point, and denote it as the second perpendicular line; Step B4: Using the key passable point as the center, select two endpoints on the second vertical line according to a preset length as the initial first auxiliary passable point and the initial second auxiliary passable point; Step B5: Generate candidate window regions centered on the first auxiliary passable point and the initial second auxiliary passable point, respectively; Step B6: Calculate the distance cost based on the obstacle distribution in the candidate window area, and select the point with the minimum distance cost as the first and second auxiliary passable points.

3. The trajectory planning method according to claim 1, characterized in that, Step 203 includes: if the relative positions of the first auxiliary circle and the second auxiliary circle intersect, then determine the key passable point, 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: Take 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: Connect the intersection points of the first auxiliary circle and the second auxiliary circle to generate the third auxiliary line; Step C3: Use the midpoint of the third auxiliary line as a key passable point; Step C4: Generate candidate window regions centered on the first auxiliary passable point and the initial second auxiliary passable point, respectively; Step C5: Calculate the distance cost based on the obstacle distribution in the candidate window area, and select the point with the minimum distance cost as the first and second auxiliary passable points.

4. The trajectory planning method according to claim 1, characterized in that, The hybrid A* trajectory planning based on traversable areas includes: Step D1: Obtain the vehicle's current location as the current node, and expand the surrounding nodes to obtain several candidate nodes; 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: Based on all the nodes and target points, use the Dubins curve and the hybrid A* algorithm to expand the path and obtain the driving trajectory.

5. The trajectory planning method according to claim 4, characterized in that, The calculation and recording of the vehicle's trajectory during its journey includes: Step E1: Calculate the change in yaw angle according to the first preset formula; Step E2: Determine the vehicle's driving type based on the change in yaw angle. If the vehicle is driving in a straight line, calculate the coordinates of the rear axle and the yaw angle at the current moment according to the second preset formula. If the vehicle is driving on a curve, calculate the coordinates of the rear axle and the yaw angle at the current moment according to the third preset formula. Step E3: Record the coordinates and yaw angle of the vehicle's rear axle at all times.

6. The trajectory planning method according to claim 5, characterized in that, The first preset formula is: Where ΔC represents the distance traveled by the vehicle, Δδ represents the change in the front wheel steering angle, and L axies Indicates wheelbase.

7. The trajectory planning method according to claim 6, 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 indicates forward movement, and λ = -1 indicates backward movement; L axies Indicates wheelbase.

8. The trajectory planning method according to claim 6, 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; λ represents the vehicle's direction of travel, λ = 1 for forward movement, and λ = -1 for backward movement; L axies Indicates wheelbase.

Images