A trajectory planning method for lane keeping

Abstract

The present invention relates to a lane keeping trajectory planning method, comprising the following steps: step 1, establishing a lane line equation based on a vehicle coordinate system, sampling left and right lane lines respectively to obtain trajectory points of the left and right lane lines; step 2, translating the trajectory points of the left and right lane lines respectively to the lane center by a set distance and then merging them to obtain a lane center line, and setting the lane center line as a target trajectory for lane keeping; step 3, calculating an initial trajectory from a current position of the vehicle to an initial point of a target trajectory according to a current state of the vehicle, and splicing the initial trajectory and the target trajectory to obtain a planned trajectory for lane keeping; and providing a trajectory planning method with low computing power requirements and reliability and effectiveness.

Description

Technical Field

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

[0002] Intelligent driving vehicles use sensors, controllers and actuators to assist or even replace drivers in completing a series of driving actions, and ensure safety, stability, comfort and economy during driving. The entire implementation process generally includes several steps: environmental perception, positioning recognition, behavior prediction, decision planning and control execution. Environmental perception is to identify lane lines, traffic signs, vehicles, etc. through various sensors such as cameras and radars carried by the vehicle; positioning recognition is to determine the current position of the vehicle through map modules or visual SLAM; behavior prediction is to estimate the future position and motion state of pedestrians or vehicles based on their position and motion state over a period of time; decision control is to calculate an optimal execution target based on the current driving environment; control execution controls the throttle, brake and steering according to the execution target to complete intelligent driving.

[0003] At present, there are many trajectory planning methods for the lane keeping function of intelligent driving, which have two main disadvantages: 1) Simple trajectory planning methods cannot guarantee the continuity of the trajectory, resulting in problems such as jitter during the control process and a poor driving experience; 2) Complex trajectory planning methods ensure the continuity of the trajectory, but have high requirements for hardware and high computing power, and do not have a price advantage for general L2-level assisted driving functions. Summary of the invention

[0004] The present invention aims at the technical problems existing in the prior art and provides a trajectory planning method for lane keeping, which has low computing power requirements and is reliable and effective.

[0005] According to a first aspect of the present invention, a lane keeping trajectory planning method is provided, comprising:

[0006] Step 1: Establish the lane line equation based on the vehicle coordinate system, and sample the left and right lane lines respectively to obtain the trajectory points of the left and right lane lines;

[0007] Step 2: The trajectory points of the left and right lane lines are respectively translated toward the lane center by a set distance and then merged to obtain a lane center line, and the lane center line is set as the target trajectory for lane keeping;

[0008] Step 3: Calculate an initial trajectory from the current position of the vehicle to the initial point of the target trajectory according to the current state of the vehicle, and concatenate the initial trajectory and the target trajectory to obtain a planned trajectory for lane keeping.

[0009] Based on the above technical solution, the present invention can also make the following improvements.

[0010] Optionally, the process of obtaining the trajectory points of the left and right lane lines in step 1 includes:

[0011] The left and right lane lines are sampled at equal intervals respectively to obtain the trajectory points of the left and right lane lines in the vehicle coordinate system, and the trajectory points of the left and right lane lines are processed by Kalman filtering to obtain the left lane line trajectory points leftLanePoints and the right lane line trajectory points rightLanePoints.

[0012] Optionally, the process of merging to obtain the lane centerline in step 2 includes:

[0013] Based on the left / right lane line trajectory points, the right / left lane line trajectory points are interpolated to obtain the vehicle coordinate system lateral y value under the same vehicle coordinate system longitudinal x value coordinate, and the vehicle coordinate system lateral y value is averaged to obtain the target trajectory targetTrail(x

[30] ,y

[30] ,n).

[0014] Optionally, in step 3, the vehicle starts to trigger trajectory planning for lane keeping under set scenarios, and the set scenarios include: the lane keeping function is activated for the first time, the lane line is lost for a period of time causing the function to be activated again, and the driver completes the lane change.

[0015] Optionally, the method for calculating the initial trajectory in step 4 includes:

[0016] The vehicle initial state startState[x0,y0,heading0,yawrate0], the state of the preview point extrapolateState[x1,y1,yaw1,yawrate1] and the exit state exitState of the initial trajectory are respectively calculated using a fixed step length to obtain three adjacent points to obtain the initial trajectory initializeTrail(x[9],y[9],n);

[0017] Among them, x0 and y0 are the current coordinate values ​​of the ego vehicle in the vehicle coordinate system, heading0 is the heading angle of the ego vehicle in the vehicle coordinate system, and yawrate0 is the yaw angular velocity of the ego vehicle.

[0018] Optionally, the process of calculating the state extrapolateState of the preview point according to the initial state startState of the vehicle in step 4 includes:

[0019] The longitudinal distance of the preview point extrapolationSistance is obtained by multiplying the preview time extrapolationTime by the current vehicle speed velocity;

[0020] The preview point state is obtained by calculating the radius r1 of the circular motion and the longitudinal distance extrapolationSistance of the preview point.

[0021] Optionally, the process of calculating the exit state exitState of the initial trajectory in step 4 includes:

[0022] Calculate the heading angle exitHeading of the target trajectory endpoint according to the target state targetState obtained in step 2, and obtain the state trailEndState[x3,y3,heading3,yawrate3] of the target trajectory targetTrail by combining the coordinate value of the trajectory endpoint of the target trajectory targetTail in the vehicle coordinate system;

[0023] The exit state exitState[x4,y4,heading4,yawrate4] of the initial trajectory is calculated according to the preview point state extrapolateState and the target trajectory end state trailEndState.

[0024] Optionally, the process of calculating the exit state exitState[x4,y4,heading4,yawrate4] of the initial trajectory according to the preview point state extrapolateState and the target trajectory end state trailEndState includes:

[0025] The turning radius r2 of the vehicle is obtained according to the vehicle speed interpolation, the center position p1(px1,py1) is obtained by deflecting in the normal direction of the preview point state extrapolateState, and the parallel state alignedState(x2,y2,heading2,yawrate2) is obtained by deflecting in the normal direction of the target trajectory end state trailEndState; the exit state exitState of the initial trajectory is calculated based on the parallel state alignedState based on the geometric principle.

[0026] Optionally, step 3 further includes: when the lane keeping trajectory planning stops, limiting the change rate of the current exit state exitState according to the current exit state exitState, the exit state oldExirState calculated in the previous cycle, and the target trajectory point targetTrail, including:

[0027] When the end point exitState(1) of the initial track is greater than the end point trailEndState(trailEndN) of the target track, the exit state exitState is updated according to the restriction parameter;

[0028] When the end point exitState(1) of the initial track is less than the end point trailEndState(trailEndN) of the target track and the end point exitState(1) of the initial track is greater than the starting point trailEndState(1) of the target track, the exit state exitState is updated by calculating the slope by difference;

[0029] When the end point exitState(1) of the initial trajectory is less than the end point trailEndState(1) of the target trajectory, the exit state oldExirState calculated in the previous cycle is directly used to update the exit state exitState.

[0030] The present invention provides a lane keeping trajectory planning method, which realizes a reliable and effective method based on a hardware system with lower computing power. A reliable and effective trajectory is obtained based on a hardware system with lower computing power, thereby ensuring the continuity and guideability of the trajectory, avoiding problems such as jitter during the control process, and improving the driving experience. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] Figure 1 A flow chart of a lane keeping trajectory planning method provided by the present invention;

[0032] Figure 2 It is a schematic diagram of an embodiment of a complete planned trajectory of a lane keeping trajectory planning method provided by the present invention. DETAILED DESCRIPTION

[0033] The principles and features of the present invention are described below in conjunction with the accompanying drawings. The examples given are only used to explain the present invention and are not used to limit the scope of the present invention.

[0034] Figure 1 A flow chart of a lane keeping trajectory planning method provided by the present invention, such as Figure 1 As shown, the trajectory planning method for lane keeping includes:

[0035] Step 1: Establish the lane line equation based on the vehicle coordinate system, and sample the left and right lane lines respectively to obtain the trajectory points of the left and right lane lines.

[0036] Step 2: The trajectory points of the left and right lane lines are respectively translated toward the lane center by a set distance and then merged to obtain the lane center line, and the lane center line is set as the target trajectory for lane keeping.

[0037] Step 3: Calculate the initial trajectory from the current position of the vehicle to the initial point of the target trajectory according to the current state of the vehicle, and concatenate the initial trajectory and the target trajectory to obtain the planned trajectory for lane keeping.

[0038] The present invention provides a lane keeping trajectory planning method, which realizes a reliable and effective lane keeping trajectory planning method based on a hardware system with lower computing power.

[0039] Example 1

[0040] Embodiment 1 provided by the present invention is an embodiment of a lane keeping trajectory planning provided by the present invention, such as Figure 2 FIG. 1 is a schematic diagram of an embodiment of a complete planned trajectory of a lane keeping trajectory planning method provided by the present invention, combined with FIG. Figure 1 and Figure 2 It can be seen that the embodiment of the trajectory planning method includes:

[0041] Step 1: Establish the lane line equation based on the vehicle coordinate system, and sample the left and right lane lines respectively to obtain the trajectory points of the left and right lane lines.

[0042] In a possible implementation manner, the process of obtaining the trajectory points of the left and right lane lines in step 1 includes:

[0043] The left and right lane lines are sampled discretized at equal intervals to obtain the trajectory points of the left and right lane lines in the vehicle coordinate system, and the trajectory points of the left and right lane lines are processed by Kalman filtering to obtain the left lane line trajectory points leftLanePoints and the right lane line trajectory points rightLanePoints.

[0044] In the specific implementation, the vehicle coordinate system is x in the longitudinal direction and y in the lateral direction. The lane line can be represented by a cubic polynomial y=c0+c1*x+c2*x 2 +c3*x 3 . c0, c1, c2, c3 are coefficient parameters.

[0045] Step 2: The trajectory points of the left and right lane lines are respectively translated toward the lane center by a set distance and then merged to obtain the lane center line, and the lane center line is set as the target trajectory for lane keeping.

[0046] In a possible implementation manner, the process of merging to obtain the lane center line in step 2 includes:

[0047] Based on the left / right lane line trajectory points, the right / left lane line trajectory points are interpolated to obtain the vehicle coordinate system lateral y value under the same vehicle coordinate system longitudinal x value coordinate, and the vehicle coordinate system lateral y value is averaged to obtain the target trajectory targetTrail(x

[30] ,y

[30] ,n).

[0048] Based on the leftLanePoints and rightLanePoints obtained in step 1, they are translated to the lane center by a set distance Offset, and then the two translated lane lines are merged to obtain the lane center line, that is, the target trajectory of lane keeping targetTrail(x

[30] ,y

[30] ,n). In specific implementation, the left point can be used as the basis, and the right point can be interpolated to obtain the y value under the same x value coordinate, and finally the y value is averaged, such as Figure 2 shown.

[0049] Step 3: Calculate the initial trajectory from the current position of the vehicle to the initial point of the target trajectory according to the current state of the vehicle, and concatenate the initial trajectory and the target trajectory to obtain the planned trajectory for lane keeping.

[0050] In one possible implementation manner, in step 3, the vehicle begins to trigger trajectory planning for lane keeping under set scenarios, where the set scenarios include: the lane keeping function is activated for the first time, the lane line is lost for a period of time causing the function to be activated again, and the driver completes the lane change, etc.

[0051] In the specific implementation, for the target track targetTrail of lane keeping obtained in step 2, the vehicle needs an initial track initializeTrail to splice from the current position to the target position. When the lane keeping function is activated for the first time, or the lane line is lost for a period of time and the function is activated again, or the driver completes the lane change, the calculation of the initial track is triggered (B_Reinitialize=1).

[0052] In a possible embodiment, the method of calculating the initial trajectory in step 3 includes:

[0053] The vehicle initial state startState[x0,y0,heading0,yawrate0], the state of the preview point extrapolateState[x1,y1,yaw1,yawrate1] and the exit state exitState of the initial trajectory are calculated using a fixed step size to obtain the three adjacent points and obtain the initial trajectory initializeTrail(x[9],y[9],n).

[0054] Among them, x0 and y0 are the current coordinate values ​​of the ego vehicle in the vehicle coordinate system, heading0 is the heading angle of the ego vehicle in the vehicle coordinate system, and yawrate0 is the yaw angular velocity of the ego vehicle.

[0055] In a possible implementation manner, the process of calculating the state extrapolateState of the preview point according to the initial state startState of the vehicle in step 3 includes:

[0056] Assume that the vehicle's speed and yaw rate are constant from the vehicle's initial state startState to the state of the preview point extrapolateState.

[0057] The longitudinal distance of the preview point extrapolationSistance is obtained by multiplying the preview time extrapolationTime by the current vehicle speed velocity.

[0058] The preview point state is calculated based on the circular motion radius r1 and the preview point longitudinal distance extrapolationSistance.

[0059] In a possible implementation manner, the process of calculating the exit state exitState of the initial trajectory in step 3 includes:

[0060] According to the target state targetState obtained in step 2, the heading angle exitHeading of the target trajectory endpoint is calculated, and the state trailEndState[x3,y3,heading3,yawrate3] of the target trajectory targetTail is obtained by combining the coordinate value of the trajectory endpoint of the target trajectory targetTail in the vehicle coordinate system.

[0061] The exit state exitState[x4,y4,heading4,yawrate4] of the initial trajectory is calculated based on the preview point state extrapolateState and the target trajectory end state trailEndState.

[0062] In a possible implementation manner, the process of calculating the exit state exitState[x4, y4, heading4, yawrate4] of the initial trajectory according to the preview point state extrapolateState and the target trajectory end state trailEndState includes:

[0063] The turning radius r2 of the vehicle is obtained by interpolating the vehicle speed, the center position p1(px1,py1) is obtained by deflecting in the normal direction of the preview point state extrapolateState, and the parallel state alignedState(x2,y2,heading2,yawrate2) is obtained by deflecting in the normal direction of the target trail end state trailEndState; the exit state exitState of the initial trajectory is calculated based on the parallel state alignedState based on the geometric principle.

[0064] In a possible implementation manner, step 3 further includes: when the lane keeping trajectory planning stops (B_Reinitialize=0), limiting the change rate of the current exit state exitState according to the current exit state exitState and the exit state oldExirState calculated in the previous cycle and the target trajectory point targetTrail, including:

[0065] When the end point exitState(1) of the initial trajectory is greater than the end point trailEndState(trailEndN) of the target trajectory, the exit state exitState is updated according to the restriction parameter.

[0066] When the end point exitState(1) of the initial trajectory is less than the end point trailEndState(trailEndN) of the target trajectory and the end point exitState(1) of the initial trajectory is greater than the starting point trailEndState(1) of the target trajectory, the slope is calculated by difference and the exit state exitState is updated.

[0067] When the end point exitState(1) of the initial trajectory is less than the end point trailEndState(1) of the target trajectory, the exit state oldExirState calculated in the previous cycle is directly used to update the exit state exitState.

[0068] A lane keeping trajectory planning method provided by an embodiment of the present invention obtains a reliable and effective trajectory based on a hardware system with lower computing power, ensures the continuity and guidability of the trajectory, avoids problems such as jitter during the control process, and improves the driving experience.

[0069] It should be noted that in the above embodiments, the description of each embodiment has its own emphasis, and for parts that are not described in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0070] Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0071] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded computer, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0072] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0073] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0074] Although the preferred embodiments of the present invention have been described, those skilled in the art may make other changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present invention.

[0075] Obviously, those skilled in the art can make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include these modifications and variations.

Claims

1. A lane keeping trajectory planning method, characterized in that: Trajectory planning methods include: Step 1: Establish the lane line equation based on the vehicle coordinate system, and sample the left and right lane lines respectively to obtain the trajectory points of the left and right lane lines; Step 2: The trajectory points of the left and right lane lines are respectively translated toward the lane center by a set distance and then merged to obtain a lane center line, and the lane center line is set as the target trajectory for lane keeping; Step 3, calculating an initial trajectory from the current position of the vehicle to the initial point of the target trajectory according to the current state of the vehicle, and concatenating the initial trajectory and the target trajectory to obtain a planned trajectory for lane keeping; The method for calculating the initial trajectory in step 3 includes: The vehicle initial state startState[x0,y0,heading0,yawrate0], the pre-aiming point state extrapolateState[x1,y1,yaw1,yawrate1] and the exit state exitState of the initial trajectory are respectively calculated using a fixed step length to obtain three adjacent points to obtain the initial trajectory initializeTrail; Among them, x0 and y0 are the current coordinate values ​​of the ego vehicle in the vehicle coordinate system, heading0 is the heading angle of the ego vehicle in the vehicle coordinate system, and yawrate0 is the yaw rate of the ego vehicle; The process of calculating the exit state exitState of the initial trajectory in step 3 includes: Calculate the heading angle exitHeading of the target trajectory end point according to the target trajectory obtained in step 2, and obtain the end state trailEndState[x3,y3,heading3,yawrate3] of the target trajectory targetTrail by combining the coordinate value of the trajectory end point of the target trajectory targetTail in the vehicle coordinate system; Calculate the exit state exitState[x4,y4,heading4,yawrate4] of the initial trajectory according to the preview point state extrapolateState and the end state trailEndState of the target trajectory; The process of calculating the exit state exitState[x4, y4, heading4, yawrate4] of the initial trajectory according to the preview point state extrapolateState and the end state trailEndState of the target trajectory includes: The turning radius r2 of the vehicle is obtained according to the vehicle speed interpolation, the center position p1(px1,py1) is obtained by deflecting in the normal direction of the preview point state extrapolateState, and the parallel state alignedState(x2,y2,heading2,yawrate2) is obtained by deflecting in the normal direction of the end state trailEndState of the target trajectory; the exit state exitState of the initial trajectory is calculated based on the parallel state alignedState based on the geometric principle.

2. The trajectory planning method according to claim 1, characterized in that: The process of obtaining the trajectory points of the left and right lane lines in step 1 includes: The left and right lane lines are sampled at equal intervals respectively to obtain the trajectory points of the left and right lane lines in the vehicle coordinate system, and the trajectory points of the left and right lane lines are processed by Kalman filtering to obtain the left lane line trajectory points leftLanePoints and the right lane line trajectory points rightLanePoints.

3. The trajectory planning method according to claim 1, characterized in that: The process of merging and obtaining the lane centerline in step 2 includes: Based on the left / right lane line trajectory points, the right / left lane line trajectory points are interpolated to obtain the vehicle coordinate system lateral y value under the same vehicle coordinate system longitudinal x value coordinate, and the vehicle coordinate system lateral y value is averaged to obtain the target trajectory targetTrail.

4. The trajectory planning method according to claim 1, characterized in that: In step 3, the vehicle starts to trigger trajectory planning for lane keeping under set scenarios, and the set scenarios include: the lane keeping function is activated for the first time, the lane line is lost for a period of time causing the function to be activated again, and the driver completes the lane change.

5. The trajectory planning method according to claim 1, characterized in that: The process of calculating the state extrapolateState of the preview point according to the initial state startState of the vehicle in step 3 includes: The longitudinal distance of the preview point extrapolationSistance is obtained by multiplying the preview time extrapolationTime by the current vehicle speed velocity; The preview point state is obtained by calculating the radius r1 of the circular motion and the longitudinal distance extrapolationSistance of the preview point.

6. The trajectory planning method according to claim 1, characterized in that: The step 3 further includes: when the lane keeping trajectory planning stops, limiting the change rate of the current exit state exitState according to the current exit state exitState, the exit state oldExirState calculated in the previous cycle, and the target trajectory point targetTrai l, including: When the end point exitState(1) of the initial track is greater than the end point trailEndState(trailEndN) of the target track, the exit state exitState is updated according to the restriction parameter; When the end point exitState(1) of the initial track is less than the end point trailEndState(trailEndN) of the target track and the end point exitState(1) of the initial track is greater than the starting point trailEndState(1) of the target track, the exit state exitState is updated by calculating the slope by difference; When the end point exitState(1) of the initial trajectory is less than the end point trailEndState(1) of the target trajectory, the exit state oldExirState calculated in the previous cycle is directly used to update the exit state exitState.

Images