Automatic lane changing control method based on LQR transverse control

Abstract

The invention discloses an automatic lane changing control method based on LQR transverse control, and the method comprises the following steps: S1, activating an LCC transverse lane centering function, and obtaining parameters of a vehicle, a front vehicle and a camera; s2, acquiring a lane and adjacent lane center line equation by using camera data; s3, detecting a steering lamp signal in combination with the vehicle and lane information; s4, if a signal is detected, planning a quintic polynomial lane changing path based on current and target lane information; s5, calculating a steering wheel angle control quantity by using an LQR algorithm, and changing lanes along a planned path; s6, in the lane changing period, dynamically adjusting the path according to the vehicle state and the target point position, and entering a second lane changing stage when the vehicle is close to the target point; s7, after the vehicle gets close to the target point, the preview point is cut to the center line of the current lane and kept for 1 second, and then the LCC lane center controls the vehicle to be taken over; the intelligence of the ALC longitudinal control function is improved, whether the surrounding environment meets the lane changing condition or not is judged, and the system better conforms to the operation intention of a driver.

Description

Technical Field

[0001] The present invention relates to the technical field of vehicle autonomous driving, and specifically to an automatic lane change control method based on LQR lateral control. Background Art

[0002] With the rapid development of automotive technology, the functions of intelligent driving systems have become increasingly perfect, and more and more vehicles are equipped with autonomous driving assistance functions. Among them, the lateral control of vehicles is mainly responsible for assisting the driver in operating the steering wheel, enabling the vehicle to achieve accurate path tracking in different driving scenarios, and improving the driving safety and comfort. In an intelligent driving system, lateral control can not only keep the vehicle driving stably, but also implement advanced driving functions such as automatic lane change (ALC), thereby optimizing the driving efficiency of the vehicle and enhancing the overall driving experience.

[0003] In the automatic lane change (ALC) function, when the driver triggers a lane change command or the system automatically decides to change lanes based on environmental perception, the vehicle needs to make a smooth transition from the current lane to the target lane. The ALC controller combines vehicle state, path planning results, and surrounding environment information, and uses a control method to adjust the steering wheel angle, so that the vehicle is controlled according to the planned lane change trajectory. The lane change trajectory is planned using a fifth-order polynomial to ensure the smoothness and feasibility of the trajectory.

[0004] During the ALC lateral control process, the LQR controller calculates the optimal control input in real time according to state variables such as lateral error, lateral speed, heading error, and heading angle change rate, to ensure the stability and comfort during the lane change process. When the lane change task is completed, the system needs to smoothly switch to the lane centering and keeping (LCC) mode to ensure the vehicle drives stably in the target lane. The switching strategy during LCC takeover is crucial, and it is necessary to judge the best switching timing by combining vehicle states (such as yaw rate, lateral acceleration, trajectory smoothness, etc.) to avoid instability caused by sudden changes.

[0005] To ensure the safety and reliability of automatic lane change, the ALC control logic usually adopts a state machine for management. After the ALC function is enabled, the vehicle will decide whether it can change lanes under the current working conditions based on the information of the camera, the surrounding obstacle information, the lane line quality, and the driver's state. If the conditions are met, it will select the preview point in the adjacent lane for path planning according to the preview time. During the lane change process, different control algorithms will have different impacts on the smoothness of the lane change path, and different planning methods will also affect the lane change quality. The conventional control methods and planning methods are as follows: Use a pure control method to select the center preview point in the target lane and calculate the expected steering wheel angle based on the preview point; starting from the vehicle coordinate system, use the PID control algorithm to plan the lane change path based on the quintic polynomial. The path is only planned once at the beginning of the lane change and is positioned relative to the path starting coordinate system during the lane change process until the lane change is completed.

[0006] Currently, the following problems mainly exist in these two schemes: The path is unstable and is greatly affected by the change of the preview point, and the steering wheel control may oscillate; There is a lack of global optimization and only local information is used for adjustment, which may lead to uneven lane changes; It cannot ensure the success of the lane change, may deviate from the target trajectory or the lane change time is uncertain. It has poor robustness to errors, is only planned once at the beginning, and cannot dynamically adapt to the change of the vehicle state; The path tracking error is uncontrollable, and PID is difficult to cope with complex dynamic environments, which may lead to trajectory deviation or jitter; Therefore, the present invention proposes an automatic lane change control method based on LQR lateral control. Summary of the Invention

[0007] To solve the above problems and improve the stability and comfort of the host vehicle during the lane change process, a method combining LQR lateral control with dynamic quintic polynomial planning is adopted. And in the setting of the state machine, when the center of the rear axle of the vehicle enters the target lane, the preview path is switched to the center line of the current lane, and after maintaining for a period of time, it is taken over by LCC; Compared with only relying on a fixed path or preview point for control, this method can optimize the trajectory in real time during the lane change process and adapt to the dynamic state of the vehicle, making the lane change process smoother and more accurate.

[0008] To achieve the above object, the present invention provides the following technical solutions: An automatic lane change control method based on LQR lateral control, including the following steps:

[0009] S1: Activate the LCC lateral lane centering function, and obtain the host vehicle state information, the information of the target vehicle ahead, and the input parameters of the camera;

[0010] Activate the LCC (Lane Centering Control) lateral lane centering function. Through vehicle sensors and camera systems, obtain the vehicle's own state information, including lateral error, lateral speed, heading angle error, yaw rate, lateral acceleration, tire side slip angle, etc., information about the target vehicle ahead, and the input parameters of the camera.

[0011] S2: Based on the input data of the camera, obtain the lane centerline equation and the centerline equations of the left and right adjacent lanes;

[0012] Based on the input data of the camera, process and analyze the lane information, and fit to obtain the lane centerline equation and the centerline equations of the left and right adjacent lanes; these equations provide a basis for subsequent path planning and vehicle position judgment.

[0013] S3: Based on the vehicle state information and lane information, detect whether the driver toggles the turn signal;

[0014] Based on the vehicle state information and lane information, continuously monitor whether the driver toggles the turn signal in real time. This detection is a key signal to trigger the automatic lane change process.

[0015] S4: When the turn signal is detected, based on the current lane information and the target lane information, plan a lane change path of a fifth-order polynomial;

[0016] When the turn signal is detected, combine the current lane information and the target lane information. Determine the longitudinal position of the preview point for lane change based on the vehicle's own speed and lane change time. Calculate the lateral position, the first derivative of the lateral position, and the second derivative of the lateral position of the preview point through the centerline of the target lane. Take the center of the rear axle of the vehicle as the starting point and fit a fifth-order polynomial path in the vehicle coordinate system. Since the fifth-order polynomial starts from the origin of the vehicle coordinate system, C0, C1, and C2 are all 0. Based on the above determined parameters, substitute them into the fifth-order polynomial and its first and second derivative equations to find the remaining coefficients, thus planning a fifth-order polynomial lane change path that meets the requirements of smoothness and feasibility.

[0017] S5: Adopt the LQR (Linear Quadratic Regulator) lateral control algorithm, calculate the steering wheel angle control amount according to the planned path, and control the vehicle to change lanes along the planned fifth-order polynomial path;

[0018] The LQR lateral control algorithm is adopted to calculate the steering wheel angle control amount according to the planned path. First, based on the lateral motion model of the vehicle, usually a two-degree-of-freedom model is designed. Under the small-angle approximation, high-order effects such as roll and longitudinal dynamics are ignored, and the lateral dynamics of the vehicle are described in the form of state equations to determine the state variables X and control inputs. It is judged whether the system is controllable. If it is full rank, the system is controllable, and the state feedback control law is the feedback form of the LQR controller. The equation in the continuous domain is discretized to obtain the front wheel angle control law; by adjusting the parameters of the state weight matrix Q and the control amount weight matrix R in the objective function of the LQR controller, the separate control of the lateral deviation, lateral speed, heading deviation, and yaw rate is realized, as well as the adjustment of the severity of the system control. Based on this, the steering wheel angle is calculated to control the vehicle to change lanes along the planned quintic polynomial path.

[0019] S6: During a lane change cycle, the quintic polynomial path is dynamically adjusted multiple times according to the current vehicle state and the position of the target point until the vehicle travels near the target point and enters the second stage of lane change;

[0020] During a lane change cycle, the system sets the entire lane change time to 4 seconds, discretizes it into 40 cycles, and re-plans the quintic polynomial path multiple times according to the current vehicle state including the current lateral position, speed, acceleration, and heading angle, etc. and the position of the target point; starting from the current position of the vehicle itself, and calculating a new lane change trajectory according to the real-time state, abandoning the local positioning module, realizing real-time path planning, so that the trajectory can adapt to the vehicle state change at any time until the vehicle travels near the target point and enters the second stage of lane change.

[0021] S7: When the vehicle reaches near the target point, switch the preview point to the center line of the current lane and keep it for 1 s, and then switch the LCC lane centering control to take over the vehicle;

[0022] When the vehicle reaches near the target point, switch the preview point to the center line of the current lane and keep it for 1 s. During this period, the stability of the lateral error and heading error of the vehicle is detected to ensure a smooth switch. Then switch the LCC lane centering control to take over the vehicle. The LCC calculates the target trajectory based on the lane center line equation detected by the camera, and uses the LQR control algorithm for steering wheel angle control, and switches the preview position in the second stage of lane change to achieve a smooth takeover of the LCC.

[0023] Beneficial effects:

[0024] 1. The intelligence of the ALC longitudinal control function is improved, and it judges whether the surrounding environment meets the lane change conditions, making the system more in line with the driver's operation intention;

[0025] 2. The comfort of the ALC lateral control function is improved. The ALC controller can plan the path in advance, perform multiple plans within a lane change cycle, and switch the path to preview a certain distance forward when the vehicle approaches the target position, ensuring the accuracy of the target point, the smoothness of the path, the smoothness of function switching, and the comfort of the driver.

[0026] 3. The safety and usage efficiency of the ALC lateral function are improved. Through the LQR lateral algorithm and the method of multiple plans, the success rate of lane change is increased, and the number of times and usage efficiency of the driver to turn on the ALC function are improved. Description of the Drawings

[0027] Figure 1 It is a flowchart of the steps of an automatic lane change control method based on LQR lateral control;

[0028] Figure 2 It is the first five - degree polynomial fitting graph of the system when the ALC function is turned on;

[0029] Figure 3 It is the result graph of multiple plans according to the current position during the lane change process. Detailed Implementation Manner

[0030] The present invention will be further described in detail below in conjunction with the overall technical solution flowchart:

[0031] This algorithm first re - plans the five - degree polynomial path based on the current vehicle state, lateral error, lateral speed, heading angle error, etc. in each control cycle to ensure that the path can be adjusted in real time to adapt to different initial conditions and environmental changes; then, through LQR lateral control, the steering wheel control is optimized to make the vehicle change lanes smoothly along the optimized trajectory, avoiding overshoot or oscillation problems that may occur in the traditional PID method.

[0032] In addition, to further improve the safety of lane change and the matching degree with the driver's intention, this algorithm combines the motion state of the host vehicle during the lane change process to optimize the path planning strategy, ensuring that the lane change trajectory is within the safe range and meeting the driver's expectations at the same time; this method can ensure smooth lane change, rapid convergence to the target lane under different vehicle speeds and different lane change initial conditions, and adapt to complex working conditions, improving the reliability and safety of the automatic lane change function.

[0033] In this algorithm, first, it is necessary to obtain the information of the front lane lines, the relevant motion parameters of the host vehicle, and the status of the functional layer from the camera to determine whether the host vehicle is driving stably in the center of the lane and is in the LCC state. When the vehicle receives the turn signal, it then determines whether the surrounding obstacles are at risk of collision based on the information from the corner radar and the camera. If the current environment is safe, the lane change path is planned at this time; during the lane change process, the vehicle's state is constantly changing, such as speed, lateral position, heading angle, etc. If the trajectory is only planned once at the beginning of the lane change, it may deviate from the target due to vehicle state deviation or external disturbances; therefore, this method replans the quintic polynomial trajectory five times in each control cycle, starting from the current position of the host vehicle and calculating the new lane change trajectory according to the real-time state, so that the trajectory can adapt to the vehicle state at any time.

[0034] The lane line information obtained from the camera is established in the vehicle coordinate system, with the center of the rear axle of the vehicle as the starting point, and the quintic polynomial path is fitted; the representation form of the quintic polynomial is as follows:

[0035] y = C0 + C1.x + C2.x 2 + C3.x 3 + C4.x 4 + C5.x 5 (1)

[0036] Since all quintic polynomials start from the origin of the vehicle coordinate system, in the quintic polynomial, C0, C1, and C2 are all 0; that is, in the vehicle body coordinate system, the lateral deviation, slope, and curvature of the path starting point are 0.

[0037] Determine the longitudinal position x of the lane change preview point based on the host vehicle speed and the lane change time pre , the lateral position y of the preview point pre , the lateral position of the preview point , the first derivative of the lateral position of the preview point , the second derivative of the lateral position of the preview point, which can be calculated through the center line of the target lane; they are respectively expressed as follows

[0038] y pre = C0 tar + C1 tar .x pre + C2 tar .x pre 2 + C3 tar .x pre 3 (2)

[0039]

[0040] Among them, C0t ar , C1t ar , C2tar , C3t ar is the cubic polynomial coefficient of the target lane center line.

[0041] Determine the longitudinal position x of the preview point based on equations (2), (3), and (4) pre and the lateral position y of the preview point under pre , the first derivative of the lateral position of the preview point the second derivative of the lateral position of the preview point are respectively substituted into equation (1) and the first and second derivatives of this equation, as shown below.

[0042] y pre = C3.x pre 3 + C4.x pre 4 + C5.x pre 5 (5)

[0043]

[0044]

[0045] Based on equations (5), (6), and (7), C3, C4, and C5 can be obtained.

[0046] After the system receives a left turn request and the environment is safe and meets the lane change conditions, a quintic polynomial is planned in the above manner at this time, and control intervenes to achieve path tracking.

[0047] After calculating the lane change path, the lateral control algorithm of LQR is used to calculate the steering wheel angle for control. LQR can perform optimal control on lateral error, heading error, etc., making the lane change process smoother, avoiding overshoot or jitter, and optimizing the trade-off between lane change comfort and control stability by adjusting the weight matrix to improve the riding experience; LQR needs to be designed based on the lateral motion model of the vehicle, and usually a two-degree-of-freedom model is adopted; assuming that the vehicle is under small-angle approximation, roll, longitudinal dynamics, etc. are ignored.

[0048] Higher-order effects, the lateral dynamics of the vehicle can be described as:

[0049]

[0050] Among them, is the lateral error, is the heading error angle, r is the yaw angular velocity, v is the vehicle longitudinal velocity, δ is the front wheel steering angle, l f , l r is the distance from the front and rear axles to the center of mass, I z is the moment of inertia of the vehicle about the vertical axis, C f,C r are the cornering stiffnesses of the front and rear wheels.

[0051] Under small-angle approximation, using linearization, the system state can be described in the form of a state equation:

[0052]

[0053] where the state variable X is:

[0054]

[0055] represent the lateral error, heading angle error, lateral velocity, yaw angular velocity, control input. Expanding the formula gives

[0056]

[0057] where x is the state variable of the control system e cg and θ e are the lateral displacement error and heading angle error for vehicle lateral control respectively. δ is the proportional relationship between the front wheel steering angle and the steering wheel angle. r des is the desired yaw angular velocity of the reference trajectory line. Under the condition of a determined longitudinal velocity, this term is proportional to the curvature κ(s) of the reference trajectory, i.e., r(s) = v x κ(s).

[0058] When actually controlling the front wheel steering angle δ, we first need to determine whether the system is controllable, that is, to judge whether [B1 AB1A 2 B1 A 3 B1] is full rank. If it is full rank, the system is controllable, and the state feedback control law can be written as: This is also the feedback form of the LQR controller; based on this, the characteristic matrix of the closed-loop system is A - B1K, and in principle, arbitrary pole configuration can be achieved.

[0059] When actually implementing the controller, we first need to discretize the above equations in the continuous domain. Assuming Ad and Bd are the discretization results of A and B1 respectively, the front wheel steering angle control law is expressed as:

[0060] δ * (k) = -Kx(k)

[0061] where:

[0062]

[0063] where P satisfies the following Riccati equation

[0064]

[0065] Moreover, Q and R are respectively the state weight matrix and the control variable weight matrix in the objective function in the LQR controller design, and the objective function is:

[0066]

[0067] By adjusting the parameters of the Q matrix, the lateral deviation, lateral velocity, the error between the actual yaw angle and the desired heading angle of the heading deviation, and the yaw angular velocity can be respectively controlled. By adjusting the R vector, the intensity of the system control can be realized.

[0068] The system sets the entire lane change time to 4 seconds. Then, starting from when the environment judgment is satisfied, the pre-viewed longitudinal distance is calculated by multiplying the current vehicle speed v by the lane change period t, and the lateral distance is y = C0 + C1.x + C2.x 2 + C3.x 3 + C4.x 4 + C5.x 5 ,

[0069] Since the start of the planning, the entire lane change time is discretized into 40 cycles. The coordinates of the pre-viewed position are calculated repeatedly for each cycle, thus eliminating the local positioning module and enabling real-time path planning to be smoother. And when the vehicle state machine jumps to the second stage of lane change, at this time, the ALC function continues to take over, and the second-stage path planning is carried out, taking the center line of the front lane as the pre-view. When the vehicle is located at the center of the target lane, the center of the rear axle of the vehicle is near the center of the lane, and the orientation angle is near 0°. The ALC function exits and the LCC takes over, so as to ensure a smooth lane change and prevent abrupt jamming when switching between the ALC and LCC functions.

[0070] The algorithm used in this method performs excellently in terms of lane change time, smoothness, and the ability to handle complex working conditions. By precisely adjusting the steering wheel angle through LQR lateral control, it ensures that the vehicle can complete the lane change safely and smoothly; the seamless switching between the ALC and LCC functions avoids the driving discomfort that may be caused by function transition, and further improves the fluency and comfort of the entire driving process.

[0071] The above-described embodiments only represent the implementation modes of the present invention, and thus should not be construed as limiting the scope of the invention patent, nor as imposing any form of limitation on the structure of the present invention. It should be noted that for those of ordinary skill in the art, without departing from the concept of the present invention, several changes and improvements can still be made, and these all fall within the protection scope of the present invention.

Claims

1. An automatic lane change control method based on LQR lateral control, characterized in that: The following steps are involved: S1: The LCC lateral lane centering function is activated to obtain the vehicle status information, the target vehicle information in front and the input parameters of the camera; S2: Obtain the centerline equation of the lane and the centerline equations of the left and right adjacent lanes based on the input data from the camera; S3: Detecting whether the driver turns on the turn signal based on the vehicle status information and lane information; S4: when a turn signal is detected, a lane change path of quintic polynomial is planned based on the current lane information and the target lane information; S5: Using the LQR lateral control algorithm, the steering wheel angle control amount is calculated according to the planned path, and the vehicle is controlled to change lanes along the planned quintic polynomial path; S6: In a lane-changing cycle, the quintic polynomial path is dynamically adjusted multiple times according to the current vehicle state and the target point position until the vehicle drives near the target point and enters the second lane-changing phase; S7: When the vehicle reaches the vicinity of the target point, the preview point is switched to the center line of the current lane and maintained for 1 second, and then the LCC lane centering control is switched to take over the vehicle.

2. The automatic lane change control method based on LQR lateral control according to claim 1, characterized in that: The state variables of the LQR lateral control algorithm include lateral error, lateral error change rate, heading error and heading angle change rate, and the control input is the steering wheel angle.

3. The automatic lane change control method based on LQR lateral control according to claim 1, characterized in that: The fifth-order polynomial path planning takes into account vehicle dynamic constraints, including vehicle yaw rate, lateral acceleration and tire slip angle.

4. The automatic lane change control method based on LQR lateral control according to claim 1, characterized in that: The quintic polynomial path adjustment within the lane change cycle is based on the real-time vehicle state, including the current lateral position, velocity, acceleration and heading angle.

5. The automatic lane change control method based on LQR lateral control according to claim 1, characterized in that: Before switching to LCC lane centering control in step S7, the vehicle's lateral error and heading error are checked for stability to ensure smooth switching.

6. The automatic lane change control method based on LQR lateral control according to claim 1, characterized in that: The LCC lane centering control uses the lane centerline equation based on camera detection to calculate the target trajectory, and uses the LQR control algorithm to control the steering wheel angle, and switches the preview position in the second stage of lane change to achieve smooth LCC takeover.

Images