Multi-lane car-following control method based on front vehicle state information and safety constraint

Abstract

The invention discloses a multi-lane car-following control method based on front vehicle state information and safety constraints. The method comprises the steps that the speed and the yaw velocity of a front vehicle are obtained; identifying a lane through a visual sensor, and determining whether the car-following task is car-following on the same lane or car-following on an adjacent lane; when car following is carried out on the same lane, the track curvature of the front car is directly calculated through the obtained speed and yaw velocity of the front car; when the adjacent lane follows, projecting the front vehicle to the rear vehicle lane according to the lane width, and calculating the trajectory curvature after projection; the trajectory curvature of the front vehicle or the trajectory curvature of the front vehicle after projection is converted from the time dimension to the distance dimension to serve as the target curvature of the vehicle; designing a self-vehicle transverse controller to obtain an ideal self-vehicle wheel rotation angle, and enabling the self-vehicle wheel rotation angle to track a target path; a self-vehicle longitudinal speed controller is designed, ideal self-vehicle wheel torque is obtained, self-vehicle acceleration and deceleration motion is controlled, and the ideal distance between the self-vehicle and the front vehicle is guaranteed.

Description

Technical Field

[0001] The invention belongs to the technical field of unmanned vehicle following, and relates to a multi-lane following control method based on leading vehicle state information and safety constraints. Background Art

[0002] The car-following technology for unmanned vehicles is one of the core technologies in the research of unmanned vehicles, and plays a vital role in improving road utilization and driving safety. Among them, the estimation of the trajectory of the leading vehicle and the control of the self-vehicle are the key components of the car-following technology, which play a decisive role in the completion of the car-following task by unmanned vehicles in specific driving environments. The car-following control of unmanned vehicles refers to obtaining the status information of the leading vehicle, analyzing and processing its historical trajectory, and calculating the wheel angle and torque in combination with the control algorithm, so as to achieve the precise following of the self-vehicle to the leading vehicle and maintain the ideal vehicle-to-vehicle distance.

[0003] At present, the application of car-following technology in the field of unmanned vehicle control is becoming more and more extensive, and the requirements for its performance are gradually increasing. First of all, safety is the primary consideration for car-following control. Ensuring that the lateral and longitudinal errors are always within the safe range during the control process can not only improve the stability of the system, but also significantly enhance the safety of the car-following process. Secondly, the driving environment of unmanned vehicles is complex and changeable, and the car-following control method needs to be able to adapt to a variety of road conditions and achieve flexible cross-lane following under different circumstances. Finally, the core of car-following control lies in accurately tracking the trajectory of the vehicle in front. Quickly and accurately obtaining the trajectory information of the vehicle in front is an important factor in accelerating the response of the system and is crucial to improving the control effect.

[0004] In the technical solution described in the Chinese patent "CN 115683116 A Front Vehicle Trajectory Generation Method and Module", when constructing the front vehicle trajectory, it mainly reconstructs the front vehicle trajectory by screening each trajectory point of the front vehicle to achieve car-following control, but polynomial fitting is required when constructing the trajectory, and its calculation process is relatively complicated. In the technical solution described in the Chinese patent "CN 118289014 A MPC-based Car-following Control Method, Device, Equipment and Storage Medium", the information of the vehicle and the front vehicle are mainly used to achieve adaptive cruise control, but the adjacent lanes and error constraints are not taken into account when performing car-following control, which is somewhat conservative.

[0005] Therefore, designing a fast-responding, lane-switching-flexible method with high safety and adaptability is of great significance to the development of unmanned driving technology. Summary of the invention

[0006] In order to solve the above technical problems, an object of the present invention is to provide a multi-lane following control method based on leading vehicle status information and safety constraints.

[0007] The present invention provides a multi-lane following control method based on leading vehicle status information and safety constraints, comprising:

[0008] Step 1: Get the speed and yaw rate of the front vehicle;

[0009] Step 2: Identify the lane through the visual sensor and determine whether the following task is following the same lane or following the adjacent lane;

[0010] Step 3: When following in the same lane, directly calculate the trajectory curvature of the leading vehicle using the speed and yaw rate of the leading vehicle obtained in step 1; when following in an adjacent lane, project the leading vehicle onto the lane of the following vehicle according to the lane width, and calculate the curvature of the projected trajectory;

[0011] Step 4: Convert the trajectory curvature of the preceding vehicle or the trajectory curvature after projection of the preceding vehicle from the time dimension to the distance dimension as the target curvature of the ego vehicle;

[0012] Step 5: Design the lateral controller of the ego vehicle to obtain the ideal wheel angle of the ego vehicle so that it can track the target path;

[0013] Step 6: Design the longitudinal speed controller of the ego vehicle to obtain the ideal wheel torque of the ego vehicle, control the acceleration and deceleration of the ego vehicle, and ensure the ideal inter-vehicle distance between the ego vehicle and the vehicle ahead.

[0014] The multi-lane following control method based on the leading vehicle status information and safety constraints of the present invention has at least the following beneficial effects:

[0015] (1) The control method of the present invention directly calculates the trajectory curvature using the speed and angular velocity of the leading vehicle, without the need for reconstruction or fitting of the trajectory, thus reducing the amount of calculation. In addition, this method of calculating curvature can project the leading vehicle trajectory onto the rear vehicle lane, thus achieving the following control of the self-vehicle and the leading vehicle in adjacent lanes.

[0016] (2) The control method of the present invention is flexible in obtaining the curvature of the leading vehicle. It can be obtained directly by communication between the two vehicles or by designing a differentiator using a visual sensor, thereby avoiding the privacy issue in communication.

[0017] (3) Both the lateral control scheme and the longitudinal control scheme proposed in the present invention constrain the error, preventing the vehicle from deviating from the predetermined lane in the lateral direction and ensuring a safe distance between the two vehicles in the longitudinal direction and preventing them from exceeding the effective range of the sensor. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] Figure 1 It is a flow chart of a multi-lane following control method based on leading vehicle status information and safety constraints of the present invention;

[0019] Figure 2It is a flowchart of the dimension conversion of the trajectory curvature of the preceding vehicle. DETAILED DESCRIPTION

[0020] like Figure 1 As shown, a multi-lane following control method based on leading vehicle status information and safety constraints of the present invention comprises:

[0021] Step 1: Obtain the speed and yaw rate of the vehicle ahead. Step 1 is specifically as follows:

[0022] Step 1.1: First, obtain the speed and yaw rate information of the front vehicle through inter-vehicle communication; if it cannot be obtained due to privacy issues, obtain the relative distance and relative angle between the two vehicles through the visual sensor.

[0023] Step 1.2: Design a differentiator based on the relative distance and relative angle between the two vehicles to obtain the speed and yaw rate information of the leading vehicle. Step 1.2 is specifically as follows:

[0024] Step 1.2.1: According to the vehicle kinematic model, the speed of the front vehicle is calculated by the self-vehicle:

[0025]

[0026] Where d is the relative distance between the two vehicles, is the derivative of d; x P Indicates the distance the front vehicle moves in the x-axis direction, y P Indicates the distance the front vehicle moves in the y-axis direction; x H represents the distance the vehicle moves in the x-axis direction, For x H The derivative of y H Indicates the distance the vehicle moves in the y-axis direction. for y H The derivative of P is the yaw angle of the front vehicle.

[0027] Step 1.2.2: Design the following differentiator calculation

[0028]

[0029] in, for The estimated value of is the estimated value of d, sig n (x) = x n sgn(x),k d1 ,k d2 and k d3 are adjustable parameters for the designed differentiator.

[0030] Step 1.2.3: Calculate the yaw angle of the front vehicle according to the following formula:

[0031] ψ p =ψ H +ψ I

[0032] Among them, ψ H is the yaw angle of the vehicle; ψ I is the relative angle between the two vehicles, obtained by the visual sensor.

[0033] Step 1.2.4: The yaw rate of the front vehicle is obtained by designing the following differentiator:

[0034]

[0035] Among them, γ P is the yaw rate of the front vehicle, For γ P The estimated value of is the yaw angle of the front vehicle ψ p The estimated value of k γ1 , k γ2 , k γ3 is the adjustable parameter of the differentiator.

[0036] Step 2: Identify the lane through the visual sensor and determine whether the following task is following the same lane or following the adjacent lane.

[0037] Step 3: When following in the same lane, directly calculate the trajectory curvature of the leading vehicle according to the speed and yaw rate of the leading vehicle; when following in an adjacent lane, project the leading vehicle to the lane of the following vehicle according to the lane width, and calculate the curvature of the projected trajectory. Step 3 is specifically as follows:

[0038] Step 3.1: When following in the same lane, calculate the curvature of the track of the vehicle ahead according to the following formula:

[0039]

[0040] Step 3.2: When following in an adjacent lane, calculate the curvature of the trajectory after the projection of the preceding vehicle according to the following formula:

[0041]

[0042] Wherein, B is the lane width. The curvature of the lane where the vehicle is currently located is determined by the visual sensor. If the curvature of the lane of the vehicle is greater than the curvature of the lane where the front vehicle is located, the sign is "-", otherwise it is "+".

[0043] Step 4: The currently obtained trajectory curvature information of the leading vehicle is a variable in the time dimension, and the trajectory curvature needs to be converted into information that changes according to the driving distance. Convert the trajectory curvature of the leading vehicle or the trajectory curvature after the projection of the leading vehicle from the time dimension to the distance dimension as the target curvature of the host vehicle, as Figure 2 shown. The specific steps of Step 4 are as follows:

[0044] Step 4.1: Sample the trajectory curvature and store it in the array K in the order of sampling time, that is, the trajectory curvature at the i-th time sampling point is stored in the i-th position of the curvature array K.

[0045] Step 4.2: Calculate the driving trajectory length of the leading vehicle at the corresponding time point according to the sampling time of the trajectory curvature, and store the driving trajectory length of the leading vehicle in the array S in order, that is, store the driving trajectory length corresponding to the i-th time sampling point in the i-th position of the trajectory array S.

[0046] Step 4.3: Calculate the current driving trajectory length S H of the host vehicle and compare it with the initial distance d0 between the two vehicles. If S H < d0, the target curvature of the host vehicle is 0; if S H > d0, then compare S H - d0 with the driving trajectory lengths in the trajectory array S, traverse the data in S to find the position index i H closest to S d , and obtain the curvature κ(i d ) at the i d -th position in the curvature array K as the target curvature of the host vehicle, and complete the correspondence between the driving distance of the following vehicle and the target curvature.

[0047] Step 5: Design a lateral controller for the host vehicle to obtain the ideal wheel angle of the host vehicle to make it track the target path. The specific steps of Step 5 are as follows:

[0048] Step 5.1: Calculate the distance error e d between the current position of the host vehicle and the projection point and the angle error e ψ between the yaw angle of the host vehicle and the tangent direction of the projection point according to the following formula:

[0049]

[0050] where, v x represents the forward speed of the host vehicle, v y represents the lateral speed of the host vehicle; β is the sideslip angle of the host vehicle; γ is the yaw angular velocity of the host vehicle, κ(s) represents the target curvature, and v s is the speed in the tangent direction of the projection point on the ideal path.

[0051]

[0052] Step 5.2: Calculate the tracking error e using the preview error method p =e d +p L e ψ , p L To preview the distance and further ensure the safety of car-following control, the ego vehicle is constrained within the width of its target lane and the following constraint function is introduced:

[0053]

[0054] Among them, t is the driving time of the vehicle, T m is the set stabilization time; ρ(0) and ρ(∞) are the set constants.

[0055] Step 5.3: Reconstruct the error variable as follows:

[0056]

[0057] Among them, k e is a constant;

[0058] Step 5.4: The final design of the ego vehicle lateral controller is:

[0059]

[0060] Among them, δ f is the front wheel turning angle of the vehicle, k1, k2, p a1 ,p a12 ,p a2 ,p a22 ,p d ,p dd ,p b and p bb are the lateral controller parameters.

[0061] Step 6: Design the longitudinal speed controller of the vehicle, obtain the ideal wheel torque of the vehicle, control the acceleration and deceleration of the vehicle, and ensure the ideal inter-vehicle distance between the vehicle and the preceding vehicle. Step 6 is specifically as follows:

[0062] Step 6.1: When implementing car-following control, in addition to ensuring that the lateral error allows the ego vehicle to track the historical path of the preceding vehicle, it is also necessary to ensure the distance between the ego vehicle and the preceding vehicle to ensure vehicle safety. The vehicle-to-vehicle distance is calculated using the time-distance method as follows:

[0063] S d =S1v P +S0

[0064] Among them, S d is the ideal vehicle distance, S1 is the time distance coefficient, and S0 is the minimum vehicle safety distance.

[0065] Step 6.2: The error between the actual distance between the two vehicles and the ideal distance e s Expressed as

[0066] e s =S P -S H -S d +d0

[0067] Among them, S P is the length of the preceding vehicle’s travel path, S H is the length of the vehicle’s driving path, and d0 is the distance difference between the initial positions of the two vehicles.

[0068] Step 6.3: Considering both safety constraints and sensor effective distance constraints, the constraints introduced when designing the longitudinal controller are:

[0069]

[0070] e s =S1v P

[0071]

[0072] Step 6.4: The final designed vehicle longitudinal speed controller is:

[0073]

[0074]

[0075] Where Ta is the wheel torque, e s The upper and lower bounds of the distance error are determined according to the vehicle safety distance S1 and the effective distance of the sensor, k v1 ,k v2 ,k a ,p bv ,p bbv ,p dv ,p ddv is the longitudinal speed controller parameter.

[0076] The present invention discloses a multi-lane following control method based on the leading vehicle state information and safety constraints. The method directly constructs the trajectory curvature of the leading vehicle through the speed and yaw rate information of the leading vehicle, converts the following control into trajectory tracking control based on curvature, and constrains the lateral error and longitudinal error of the vehicle to ensure the safety of the following control. The method is not only simple to calculate, but also can realize following of the leading vehicle and the own vehicle in different lanes.

[0077] The above description is only a preferred embodiment of the present invention and is not intended to limit the concept of the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A multi-lane following control method based on leading vehicle status information and safety constraints, characterized in that: include: Step 1: Get the speed and yaw rate of the front vehicle; Step 2: Identify the lane through the visual sensor and determine whether the following task is following the same lane or following the adjacent lane; Step 3: When following in the same lane, directly calculate the trajectory curvature of the leading vehicle based on the speed and yaw rate of the leading vehicle; When following in adjacent lanes, the leading vehicle is projected onto the rear lane according to the lane width, and the curvature of the projected trajectory is calculated; Step 4: Convert the trajectory curvature of the preceding vehicle or the trajectory curvature after projection of the preceding vehicle from the time dimension to the distance dimension as the target curvature of the ego vehicle; Step 5: Design the lateral controller of the ego vehicle to obtain the ideal wheel angle of the ego vehicle so that it can track the target path; Step 6: Design the longitudinal speed controller of the ego vehicle to obtain the ideal wheel torque of the ego vehicle, control the acceleration and deceleration of the ego vehicle, and ensure the ideal inter-vehicle distance between the ego vehicle and the vehicle ahead.

2. The multi-lane following control method based on leading vehicle status information and safety constraints as claimed in claim 1, characterized in that: The step 1 is specifically as follows: Step 1.1: First, obtain the speed and yaw rate information of the front vehicle through inter-vehicle communication. If the information cannot be obtained due to privacy issues, obtain the relative distance and angle between the two vehicles through the visual sensor. Step 1.2: Design a differentiator based on the relative distance and angle between the two vehicles to obtain the speed and yaw rate information of the leading vehicle.

3. The multi-lane following control method based on leading vehicle status information and safety constraints as claimed in claim 1, characterized in that: The step 1.2 is specifically as follows: Step 1.2.1: According to the vehicle kinematic model, the speed of the leading vehicle is calculated by the ego vehicle: Where d is the relative distance between the two vehicles, is the derivative of d; x P Indicates the distance the front vehicle moves in the x-axis direction, y P Indicates the distance the front vehicle moves in the y-axis direction; x H represents the distance the vehicle moves in the x-axis direction, For x H The derivative of y H Indicates the distance the vehicle moves in the y-axis direction. for y H The derivative of P is the yaw angle of the front vehicle; Step 1.2.2: Design the following differentiator calculation in, for The estimated value of is the estimated value of d, sig n (x) = x n sgn(x),k d1 ,k d2 and k d3 The adjustable parameters of the designed differentiator; Step 1.2.3: Calculate the yaw angle of the front vehicle according to the following formula: ψ p =ψ H +ψ I Among them, ψ H is the yaw angle of the vehicle; ψ I is the relative angle between the two vehicles, obtained by the visual sensor; Step 1.2.4: The yaw rate of the front vehicle is obtained by designing the following differentiator: Among them, γ P is the yaw rate of the front vehicle, For γ P The estimated value of is the yaw angle of the front vehicle ψ p The estimated value of k γ1 , k γ2 , k γ3 is the adjustable parameter of the differentiator.

4. The multi-lane following control method based on leading vehicle status information and safety constraints as claimed in claim 1, characterized in that: The step 3 is specifically as follows: Step 3.1: When following in the same lane, calculate the curvature of the track of the vehicle ahead according to the following formula: Step 3.2: When following in an adjacent lane, calculate the curvature of the trajectory after the projection of the preceding vehicle according to the following formula: Wherein, B is the lane width; the curvature of the lane where the current vehicle is located is determined by the visual sensor. If the curvature of the lane of the vehicle is greater than the curvature of the lane where the front vehicle is located, the sign is "-", otherwise it is "+".

5. The multi-lane following control method based on leading vehicle status information and safety constraints as claimed in claim 1, characterized in that: The step 4 is specifically as follows: Step 4.1: Sample the trajectory curvature and store it in the curvature array K in the order of sampling time, that is, the trajectory curvature of the i-th time sampling point is stored in the i-th position of the curvature array K; Step 4.2: Calculate the driving trajectory length of the preceding vehicle at the corresponding time point according to the sampling time of the trajectory curvature, and store the driving trajectory length of the preceding vehicle in the trajectory array S in order, that is, store the driving trajectory length corresponding to the i-th time sampling point into the i-th position of the trajectory array S; Step 4.3: Calculate the length S of the current driving trajectory of the host vehicle H And compare it with the initial distance d0 between the two vehicles. If S H < d0, the target curvature of the host vehicle is 0; if S H > d0, then compare S H - d0 with the driving trajectory lengths in the trajectory array S, and traverse the data in S to find the position index i H closest to S d - d0. Correspondingly, obtain the curvature κ(i d ) at the i d -th position in the curvature array K as the target curvature of the host vehicle, thus completing the correspondence between the driving distance of the following vehicle and the target curvature.

6. The multi-lane following control method based on leading vehicle status information and safety constraints as claimed in claim 1, characterized in that: The step 5 is specifically as follows: Step 5.1: Calculate the distance error e between the current vehicle position and the projection point according to the following formula: d and the angle error e between the yaw angle of the vehicle and the tangent direction of the projection point ψ : Among them, v x represents the forward speed of the vehicle, v y represents the lateral velocity of the vehicle; β is the sideslip angle of the vehicle; γ is the yaw rate of the vehicle, κ(s) represents the target curvature, v s is the tangential velocity of the projection point on the ideal path; Step 5.2: Calculate the tracking error e using the preview error method p =e d +p L e ψ , p L To preview the distance and further ensure the safety of car-following control, the ego vehicle is constrained within the width of its target lane and the following constraint function is introduced: Among them, t is the driving time of the vehicle, T m is the set stabilization time; ρ(0) and ρ(∞) are the set constants; Step 5.3: Reconstruct the error variable as follows: Among them, k e is a constant; Step 5.4: The final design of the ego vehicle lateral controller is: Among them, δ f is the front wheel turning angle of the vehicle, k1, k2, p a1 ,p a12 ,p a2 ,p a22 ,p d ,p dd ,p b and p bb are the lateral controller parameters.

7. The multi-lane following control method based on leading vehicle status information and safety constraints as claimed in claim 1, characterized in that: The step 6 is specifically as follows: Step 6.1: The distance between the vehicle and the preceding vehicle must be maintained to ensure vehicle safety. The vehicle-to-vehicle distance is calculated using the time-to-distance method as follows: S d =S1v P +S0 Among them, S d is the ideal vehicle distance, S1 is the time distance coefficient, and S0 is the minimum vehicle safety distance; Step 6.2: The error between the actual distance between the two vehicles and the ideal distance e s Expressed as e s =S P -S H -S d +d0 Among them, S P is the length of the preceding vehicle’s travel path, S H is the length of the vehicle’s driving path, d0 is the distance difference between the initial positions of the two vehicles; Step 6.3: Considering both safety constraints and sensor effective distance constraints, the constraints introduced when designing the longitudinal controller are: this s =S1v P Step 6.4: The final designed vehicle longitudinal speed controller is: z v2 =v x -α v Where Ta is the wheel torque, e s The upper and lower bounds of the distance error are determined according to the vehicle safety distance S1 and the effective distance of the sensor, k v1 ,k v2 ,k a ,p bv ,p bbv ,p dv ,p ddv is the longitudinal speed controller parameter.

Images