Intelligent vehicle motion planning and control method for traffic light intersection

Abstract

The application discloses a kind of intelligent vehicle motion planning and control method for traffic light intersection, belongs to intelligent automobile automatic driving trajectory planning and motion control technical field.The purpose of the present application is around the scene with traffic light intersection, according to vehicle, intersection relative position relationship and signal light phase, design self-vehicle speed time-varying constraint condition for intelligent vehicle motion planning and control method of traffic light signal intersection.The present application is around the scene with traffic light intersection, according to vehicle, intersection relative position relationship and signal light phase, design self-vehicle speed time-varying constraint condition, utilize model predictive control to solve constrained optimization problem, propose corresponding motion planner scheme, give the model predictive control trajectory tracking controller design connected with upper layer, realize stable tracking to the local reference information planned.The present application considers the state of preceding vehicle and real-time traffic light phase, adopts speed constraint mode, brings more optimization indexes for local trajectory planning problem, guarantees driving safety, and also improves road traffic efficiency.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of intelligent automobile automatic driving trajectory planning and motion control. BACKGROUND

[0002] The intelligent driving automobile technology develops rapidly, and has developed from the initial laboratory prototype to mature products put into the market in the second decade of the 21st century, and quickly enters ordinary people's homes. Compared with human driver operation of the automobile, our expectations for the intelligent driving automobile are often focused on the improvement of vehicle driving safety and road traffic efficiency in complex scenarios, and one of the most typical scenarios is the traffic signal intersection. Due to the complex interaction relationship of vehicles and the changeable motion condition, the traffic intersection gradually becomes a bottleneck of road traffic operation and is one of the important problems of urban road traffic. With the development of the times, the vehicle based on intelligent network technology can easily obtain various types of real-time road environment information including traffic light phase and real-time state of surrounding vehicles in the road network, and can construct a more reasonable and orderly traffic flow under limited road conditions, improve road use efficiency and traffic capacity, and also can guarantee driving safety and reduce the incidence of traffic accidents, so it is quite necessary to carry out the intelligent driving vehicle trajectory planning and motion control research for the traffic signal intersection.

[0003] Through investigation and analysis, the current intelligent driving automobile related technology in the intersection scene still has the following problems:

[0004] 1. The motion planning research considering the traffic light signal mostly focuses on realizing the ecological driving of reducing energy consumption, the upper layer realizes the global vehicle speed planning according to the traffic information, and then outputs the expected speed curve to the local planning layer, and re-plans combined with other control targets. This kind of control method has many layers, which increases the system complexity, and uses traditional methods such as dynamic programming for global vehicle speed planning, which has the problem of large amount of calculation.

[0005] 2. In actual application, the vehicle approaching the traffic light intersection will be limited by the preceding vehicle, and the motion planning method considering only the vehicle state has great limitations, and the existing research more focuses on using traffic information for global vehicle speed planning, and the research combining the preceding vehicle state with real-time traffic light information is relatively less. SUMMARY

[0006] The purpose of the application is to design an intelligent vehicle motion planning and control method for the traffic light signal intersection according to the vehicle, the relative position relationship of the intersection and the signal light phase, and the time-varying constraint condition of the vehicle speed.

[0007] The steps of the application are:

[0008] S1, design vehicle motion planning layer for traffic signal intersection S11, establish vehicle kinematics model under global coordinate system

[0009]

[0010] where v is the vehicle speed, is the vehicle heading angle, δ f is the front wheel steering angle, L is the wheelbase, x and y are the longitudinal and lateral position coordinates of the vehicle in the global coordinate system;

[0011] Simplify as:

[0012]

[0013] The vehicle state quantity is The control quantity is [v, δ f ] T ;

[0014] S12, consider the motion planning method of the ego vehicle through the traffic signal intersection

[0015] S121, speed limit:

[0016] 0≤v(k)≤v rule (3)

[0017] where v rule is the maximum vehicle driving speed limited by traffic regulations on this section;

[0018] S122, make time-varying constraints on vehicle speed v, if in the red light period, the constraint condition is as follows:

[0019]

[0020] where S left (k) is the remaining distance of the vehicle to the intersection, T redleft (k) is the remaining time of the red light, and T green is the green light cycle period;

[0021] If in the green light period, time threshold t judge , X traf is the intersection position, T greleftpre is the remaining time of the green light when the vehicle is in the initial position;

[0022] S123, when T geleftpre >t judge , the speed constraint is as follows:

[0023]

[0024] where v latest is the speed reference value at the last time step;

[0025] S124, the following constraint is made on Δv:

[0026] a min T≤Δv(k)≤a max T (6)

[0027] where a max , a min are the maximum and minimum acceleration of the vehicle, and T is the sampling period of the planning layer;

[0028] S125, the following constraint is made on the control variable δ f :

[0029] δ fmin ≤δ f (k)≤δ fmax (7)

[0030] where δ fmax , δ fmin are the maximum and minimum front wheel steering angle of the vehicle;

[0031] S126, the objective function is designed according to the control requirements of the planning layer as follows:

[0032]

[0033] where N p represents the prediction time domain, N c represents the control time domain, Q and S are weight values, and Δu is the control increment;

[0034] S127, the objective function and the constraint conditions are integrated as follows:

[0035]

[0036] s.t.(2)(3)(4)(5)(6)(7) (9)

[0037] S13, a motion planning method considering the influence of the preceding vehicle on the ego vehicle passing through a traffic signal intersection

[0038] When the vehicle predicted state is in the red light period, the time-varying constraint condition is as follows:

[0039]

[0040] The time-varying constraint condition is as follows:

[0041]

[0042] Objective function and constraints:

[0043]

[0044] s.t.(2)(3)(6)(7)(10)(11) (12)S2, Design tracking control layer

[0045] S21, Vehicle dynamics model, a vehicle dynamics model considering vehicle yaw and slip is established in the global coordinate system

[0046] In the formula, m is the mass of the vehicle, is the yaw angular velocity of the vehicle, δ f is the front wheel steering angle of the vehicle, I z is the moment of inertia, l f , l r are the distances from the vehicle mass center to the front and rear axles respectively, F yf , F yr are the resultant forces of the tire lateral forces on the front and rear axles of the vehicle respectively, F xf , F xr are the resultant forces of the tire longitudinal forces on the front and rear axles of the vehicle respectively;

[0047] The lateral force expression is as follows:

[0048]

[0049] Where C αf and C αr represent the lateral stiffness of the front and rear tires, and β is the mass center side slip angle, which is generally considered The longitudinal force expression of the vehicle is:

[0050]

[0051] Where C lf , C lr are the longitudinal stiffness of the front and rear tires, and λf, λr are the front and rear tire slip rates;

[0052] Briefly described as:

[0053]

[0054] The state quantity is The control quantity is u = [a x , δ f ], i.e. the longitudinal acceleration;

[0055] S22, MPC tracking trajectory controller

[0056] Speed limit:

[0057] 0≤v x (k)≤v rule (17)

[0058] The size and increment of the vehicle longitudinal acceleration are limited:

[0059]

[0060] In summary, the tracking reference trajectory target function is as follows:

[0061]

[0062] s.t.(16)(17)(18) (19)

[0063] wherein, The first term of the target function is the deviation size of the ego state and the local planning reference information generated by the planning layer, and the second term indicates the change value of the control value, indicates the control amount, and Q1, S1 and S2 are weights.

[0064] The application proposes an optimization method based on model predictive control in the local trajectory planning layer, uses the relative position relationship of the vehicle and the intersection and the phase information of the traffic light to construct time-varying constraint conditions, thereby canceling the global speed planning layer, and realizing motion planning only by the local trajectory planning layer, reducing the system complexity, having better real-time performance, and being able to effectively improve the control efficiency. The application takes the state of the preceding vehicle and the real-time traffic light phase into consideration, adopts the vehicle speed constraint mode, brings more optimization indexes for the local trajectory planning problem, ensures the driving safety, and improves the road passing efficiency. BRIEF DESCRIPTION OF DRAWINGS

[0065] Figure 1 is the intelligent vehicle MPC motion planning and control method flow chart for the traffic light signal intersection described in the application;

[0066] Figure 2 is the traffic signal intersection scene schematic diagram described in the application;

[0067] Figure 3 is the motion planning method schematic diagram considering only the motion of the ego vehicle through the traffic signal intersection described in the application;

[0068] Figure 4 is the time threshold value schematic diagram for dividing the acceleration and parking conditions according to the green light remaining time in the motion planning method considering only the motion of the ego vehicle through the traffic signal intersection described in the application;

[0069] Figure 5is a schematic diagram of a motion planning method for a vehicle to pass a traffic signal intersection considering the influence of a preceding vehicle according to the present application;

[0070] Figure 6a is a simulation diagram of longitudinal acceleration of an intelligent vehicle in scenario A according to the present application;

[0071] Figure 6b is a simulation diagram of global reference longitudinal speed and actual longitudinal speed of an intelligent vehicle in scenario A according to the present application;

[0072] Figure 6c is a simulation diagram of global reference longitudinal position and actual longitudinal position of an intelligent vehicle in scenario A according to the present application;

[0073] Figure 7a is a simulation diagram of longitudinal acceleration of an intelligent vehicle in scenario B according to the present application;

[0074] Figure 7b is a simulation diagram of global reference longitudinal speed and actual longitudinal speed of an intelligent vehicle in scenario B according to the present application;

[0075] Figure 7c is a simulation diagram of global reference longitudinal position and actual longitudinal position of an intelligent vehicle in scenario B according to the present application;

[0076] Figure 8a is a simulation diagram of longitudinal acceleration of an intelligent vehicle in scenario C according to the present application;

[0077] Figure 8b is a simulation diagram of global reference longitudinal speed and actual longitudinal speed of an intelligent vehicle in scenario C according to the present application;

[0078] Figure 8c is a simulation diagram of global reference longitudinal position and actual longitudinal position of an intelligent vehicle in scenario C according to the present application;

[0079] Figure 9a is a simulation diagram of longitudinal acceleration of an intelligent vehicle in scenario D according to the present application;

[0080] Figure 9b is a simulation diagram of global reference longitudinal speed, actual longitudinal speed and longitudinal speed of a preceding vehicle of an intelligent vehicle in scenario D according to the present application;

[0081] Figure 9c is a simulation diagram of global reference longitudinal position, actual longitudinal position and longitudinal position of a preceding vehicle of an intelligent vehicle in scenario D according to the present application;

[0082] Figure 10a is a simulation diagram of longitudinal acceleration of an intelligent vehicle in scenario E according to the present application;

[0083] Figure 10bThe simulation diagram of comparison between the global reference longitudinal speed of the intelligent vehicle under the scenario E of the application, the actual longitudinal speed and the longitudinal speed of the front vehicle;

[0084] Figure 10c The simulation diagram of comparison between the global reference longitudinal position of the intelligent vehicle under the scenario E of the application, the actual longitudinal position and the longitudinal position of the front vehicle. DETAILED DESCRIPTION

[0085] The application proposes a motion planning and tracking control method for intelligent vehicles at intersections with traffic lights. Around the scenario of intersections with traffic lights, time-varying constraint conditions of the vehicle speed are designed according to the relative position relationship between the vehicle and the intersection and the phase of the signal light, the model predictive control is used to solve the constrained optimization problem, and the corresponding motion planner scheme is proposed to control the vehicle to accelerate or decelerate through the intersection or stop; during the approach of the intelligent driving vehicle to the intersection, the front vehicle will affect it, therefore, the above motion planning method is expanded and the time-varying constraint condition is improved, so as to improve the traffic efficiency under the premise of ensuring the driving safety. Finally, according to the vehicle dynamics model, the model predictive control trajectory tracking controller design connected with the upper layer is given to realize stable tracking of the planned local reference information. The application relates to a motion planning and tracking control method for intelligent vehicles at intersections with traffic lights. According to the phase timing of the signal light and the front vehicle information, time-varying constraint conditions are designed for the scenarios with or without the front vehicle, the model predictive control method is used to solve the local trajectory planning problem, and the obtained local reference information is tracked and controlled.

[0086] The research method of the application comprises the following steps:

[0087] Step one, vehicle model building and simulation scenario design, and real-time vehicle state information feedback is provided by using the CarSim simulation platform.

[0088] Step two, design of vehicle motion planning layer for traffic signal intersection:

[0089] 1) according to the demand of motion trajectory planning, the vehicle kinematics model under the global coordinate system is established;

[0090] 2) for the scenario of the vehicle passing through the traffic signal intersection, according to the traffic light cycle, the initial phase, the signal remaining time and the remaining distance of the vehicle from the intersection, the time-varying constraint of the vehicle speed, one of the planning layer control quantities, is designed to realize the acceleration and deceleration of the vehicle, and the maximum speed limit of the road is also considered, if the vehicle cannot pass through the intersection on time, it will stop and wait, combined with the model predictive control algorithm, the intelligent driving vehicle can pass through the intersection safely and comfortably;

[0091] 3) In the presence of the front vehicle, the original planning result cannot be realized due to the presence of the front vehicle, according to whether the front vehicle can pass through the intersection in the green light period, the behaviors of following the vehicle, accelerating, decelerating and stopping of the ego vehicle are determined, the real-time vehicle speed of the front vehicle is combined with the time-varying constraint in the case of only the ego vehicle, the constraint condition is redesigned, and the corresponding NMPC motion planning controller is designed;

[0092] Step three, design the tracking control layer:

[0093] 1) Considering the safety of intelligent driving vehicles in medium and high speed driving and parking, a dynamic model considering vehicle slip and yaw is established;

[0094] 2) The constraints of guaranteeing the vehicle actuator, ride comfort and driving safety are given, and the model predictive control algorithm can process the constraints, so the trajectory tracking control layer based on the cost function and constraint is designed.

[0095] The intelligent vehicle motion planning and control method designed in the application has a control block diagram as shown in the accompanying Figure 1 The control system design is mainly divided into two layers, namely the motion planning layer and the tracking control layer.

[0096] In the motion planning layer, first, according to the real-time feedback of the vehicle state information and the front feed of the obstacle vehicle information, it is judged whether there is a front vehicle affecting the process of the ego vehicle passing through the traffic light intersection, and different motion control strategies are selected accordingly.

[0097] As shown in the accompanying Figure 3 , the accompanying Figure 5 , two different situations of only the ego vehicle passing through the intersection and the ego vehicle passing through the intersection with the influence of the front vehicle are considered, the traffic light signal condition at this time is judged, the local reference information is recalculated and solved according to the MPC multi-objective optimization problem according to the current signal light time period and the real-time state of the ego vehicle, and is transmitted to the next layer.

[0098] In the tracking control layer, the local reference trajectory information transmitted from the upper layer needs to be tracked, the objective function is designed according to the requirements of reducing tracking error and improving ride comfort and other performance indicators, MPC is solved, and the optimal control sequence (expected longitudinal acceleration and front wheel angle) is obtained. The front wheel angle directly acts on the vehicle steering mechanism, and the expected longitudinal acceleration is transmitted to the drive and brake switching PID controller as a reference value, and alternately outputs the motor torque and master cylinder brake pressure to produce the expected response.

[0099] According to the above operation principle and operation process, the model for joint simulation is built, and the building and operation process are as follows:

[0100] 1, software selection and joint simulation setting

[0101] The MPC motion planning controller of the control system of the application and the simulation model of the controlled intelligent driving car are respectively built through software MATLAB / Simulink and CarSim, and the software versions are MATLAB R2022a and CarSim 2020.0. Among them, MATLAB / Simulink is used to realize the function logic of the controller through programming, and CarSim is used as a vehicle model simulation software to provide a simulation environment and real-time state quantity information of the intelligent vehicle in the scene.

[0102] To realize the joint simulation of the above two softwares, firstly, the CarSim working path needs to be set to the Simulink model built in conjunction with the control system, then the required vehicle model parameters and road environment working condition information are selected in CarSim, the corresponding file package is sent to the corresponding Simulink, and finally the joint simulation between the two is realized by running the Simulink model.

[0103] Table 1 Vehicle parameter table

[0104]

[0105]

[0106] The control input of CarSim is set as IMP_MY_OUT_D1_L, IMP_MY_OUT_D1_R and IMP_PCON_BK, which are the total torque output of the left and right motors and the brake pressure of the master cylinder, and the output is set as the state quantity of the vehicle required for control and observation. The vehicle parameters are shown in Table 1.

[0107] 2, The intelligent vehicle motion planning and tracking control target and principle for traffic light intersection of the application The controlled object of the application is an intelligent driving car, and the control target is to plan and control the motion of the intelligent vehicle through the intersection with traffic lights under different conditions considering whether there is an obstacle in front of the car. For this scene, the application designs a set of longitudinal motion planning method and control strategy considering traffic light phase information and real-time vehicle state. The basic principle of the control method of the application is to update and correct the vehicle state by relying on the feedforward-feedback of the double-layer MPC system. The system mainly consists of two layers, the upper layer is the motion planning layer, which feeds the road information and the information of the preceding vehicle into the system, combines the feedback of the vehicle state, selects different control strategies according to the scene, and solves the local reference trajectory corresponding to the target function and constraint condition to pass to the lower layer controller; the lower layer is the tracking control layer, which tracks the local reference trajectory passed by the upper layer.

[0108] The specific steps of the intelligent driving vehicle motion planning and control method for the traffic light signal intersection of the application are introduced below, including the following steps:

[0109] Step one, build intelligent driving car model and simulation conditions, use the intelligent driving car model in the simulation software CarSim to replace the real vehicle controlled object, provide accurate real-time vehicle state information for the planning and control scheme proposed in the application through CarSim. The simulation vehicle model selects the common "C-Class" passenger car in CarSim software and modifies the vehicle parameters according to the actual needs, sets the road environment, obstacle vehicle specific parameters, etc. in the simulation conditions.

[0110] Step two, design vehicle motion planning layer for traffic signal intersection

[0111] Firstly, according to different situations with or without the influence of obstacle front vehicle, the intelligent vehicle is planned, and the NMPC motion planning method for controlling the intelligent vehicle to pass through the intersection is designed.

[0112] ①Establish the vehicle kinematics model under the global coordinate system

[0113] Due to the need of local trajectory planning, only its motion in two-dimensional plane is considered, so the kinematics model of the vehicle under the global coordinate system is established, and its equation is as follows:

[0114]

[0115] Among them, v is the vehicle speed, is the vehicle heading angle, δ f is the front wheel steering angle, L is the wheelbase, x and y are the longitudinal and lateral position coordinates of the vehicle in the global coordinate system.

[0116] The system can be simplified as:

[0117]

[0118] The vehicle state quantity is The control quantity is [v, δ f ] T .

[0119] ②Only consider the motion planning method of the ego vehicle through the traffic signal intersection

[0120] In the process of approaching the traffic signal intersection, the driver will adjust the vehicle motion state in time according to the color indication of the traffic signal at this time, decide whether to start accelerating or decelerating, so as to be able to stop in time after the stop line or use the remaining time of the green light to pass through the intersection, avoid making the behavior of violating traffic rules.

[0121] The present application is inspired by the above human driver's judgment process, and designs a set of intelligent vehicle motion planning method for passing through traffic light intersection.

[0122] The intersection scene containing traffic light signal is shown in the accompanying Figure 2 , and the traffic light is red and green signal alternately. Only considering the motion planning method of the ego vehicle through the traffic signal intersection, the implementation process is shown in the accompanying Figure 3 .

[0123] The vehicle can obtain traffic information such as signal light phase, signal light cycle time, intersection position, etc. in real time in the road network; first, the intelligent driving car must meet the driving speed requirement limited by the traffic regulations of the road section, so there are the following vehicle speed limits:

[0124] 0≤v(k)≤v rule (3)

[0125] Where v rule is the maximum driving speed of the vehicle limited by the traffic regulations in the road section.

[0126] Secondly, in order to enable the intelligent driving vehicle to pass through the intersection within the green light period, it is also necessary to predict the signal light cycle of the vehicle according to the vehicle kinematics model, and to make time-varying constraints on the vehicle speed v according to the red and green light conditions. If it is in the red light period, the constraint condition is as follows:

[0127]

[0128] Where S left (k) is the remaining distance of the vehicle to the intersection, T redleft (k) is the remaining time of the red light, T green is the green light cycle, if the vehicle drives according to the global reference information, it cannot pass through the intersection within the green light period, the significance of designing the above constraint condition is to make the vehicle gradually slow down when approaching the intersection, so as to avoid the red light remaining period and pass through the intersection in the next green light period.

[0129] If it is in the green light period, it is necessary to introduce a time threshold t judge according to the traffic light phase corresponding to the vehicle approaching the intersection, X traf is the position of the intersection, T greleftpre is the remaining time of the green light when the vehicle is in the initial position, see the accompanying Figure 4 .

[0130] When T greleftpre >t judge , the vehicle speed constraint is as follows:

[0131]

[0132] where v latest If there is still enough green light time left when the vehicle approaches the intersection, the vehicle can start accelerating with the remaining distance so as to pass through the intersection before the next red light signal period. latest The presence of v max{v latest , v

[0133] When the vehicle approaches the intersection, the green light time left is not enough, i.e., T greleftpre < t judge , i.e., even if the vehicle travels at the upper limit of the road speed limit, it cannot pass through the intersection before the red light period arrives, so the vehicle stops and waits until the next green light period.

[0134] In addition, considering the actual acceleration and deceleration capability of the vehicle and the comfort of the human body, the following constraints are needed for Δv: min T ≤ Δv(k) ≤ a max T (6)

[0135] where a max , a min are the maximum and minimum acceleration of the vehicle, and T is the sampling period of the planning layer.

[0136] In addition, because there are mechanical structure limitations when the steering system is running, the control quantity δ f is constrained as follows:

[0137] δ fmin < δ f (k) < δ fmax (7)

[0138] where δ fmax , δ fmin are the maximum and minimum front wheel steering angle of the vehicle.

[0139] According to the control requirements of the planning layer, the objective function designed by the present application is as follows:

[0140]

[0141] where N p represents the prediction time domain, N c represents the control time domain, Q and S are weight values, and Δu is the control increment. Because the motion planning layer is essentially a local re-planning on the global reference information for the vehicle to pass through the intersection in view of the traffic information, the first term in the cost function represents the deviation between the state quantity of the ego vehicle and the global reference information, and the second term The change amount of the control amount indicates the significance of making the control amount change unobtrusively, and the obtained trajectory is more smooth.

[0142] The comprehensive target function and the constraint condition are:

[0143]

[0144] The optimal control sequence u and d are calculated above f , and the local trajectory Y ref ' in the time domain is obtained by iteration through the kinematics formula. xref xref , V ref ' is calculated according to the corresponding angle . V xref , V xref , Y ref ' are finally transmitted to the tracking layer as local reference information.

[0145] The simulation experiment parameters of the planning layer are shown in Table 2.

[0146] Table 2 Simulation experiment parameters of the planning layer

[0147] Symbols Definitions Values / Units <![CDATA[N p ]]> Planning layer MPC prediction step 15 <![CDATA[N c ]]> Planning layer MPC control step 2 Q Position error weight 120 S Control weight 160

[0148] ③Motion planning method considering the influence of the preceding vehicle on the ego vehicle passing through the traffic signal intersection

[0149] In real road scenes, the motion of the vehicle itself needs to consider the potential relationship with various surrounding vehicles to determine. Due to the existence of complex environment, only considering the motion planning of the ego vehicle passing through the intersection obviously cannot meet the actual application requirements. For the problem of passing through the traffic light intersection, the most likely influence on the motion planning of the intelligent driving vehicle is the motion state of the obstacle preceding vehicle.

[0150] In this paper, the different control strategies of the ego vehicle influenced by the obstacle preceding vehicle are selected, and the judgment condition is to consider the relationship between the real-time position of the obstacle vehicle, the real-time position of the ego vehicle, and the traffic light intersection obtained by the sensor and V2X technology of the intelligent vehicle. If the distance between the two vehicles is close enough when entering the approach intersection section, it is considered that the obstacle vehicle will affect the motion planning of the ego vehicle passing through the traffic light intersection, and this part of the control strategy is adopted. Otherwise, the control strategy without the influence of the obstacle preceding vehicle is adopted, i.e. the method mentioned above.

[0151] In this paper, the consideration of the motion of the obstacle preceding vehicle is divided into two cases: stopping at the intersection when encountering a red light period and normally passing through when encountering a green light period. When not approaching the intersection, the obstacle preceding vehicle travels at a constant speed of v precar . The implementation process of the motion planning method of the ego vehicle passing through the traffic signal intersection considering the influence of the preceding vehicle is shown in the attached Figure 5 :

[0152] When the obstacle vehicle is in front of the ego vehicle and the distance between the two vehicles reaches the distance within which the front vehicle can affect the original planning result of the ego vehicle, the ego vehicle cannot constrain the speed control amount according to the method in the above, and thus cannot pass the signal light intersection on time, and therefore the control method needs to be improved.

[0153] Firstly, it is needed to judge whether the obstacle front vehicle can pass the intersection within the green light period, and since the content of the application does not include the state prediction of the obstacle front vehicle, only uniform motion is considered.

[0154] If the obstacle front vehicle will reach the intersection within the red light period at the original speed, the distance between the two vehicles reaches the limit safety distance, and then the follow-up motion is performed until the obstacle front vehicle starts to slow down and stop, and the ego vehicle also stops and waits, and then the two vehicles pass the intersection in the next green light period.

[0155] If the obstacle front vehicle will reach the intersection within the green light period, the time-varying constraint condition proposed in the above is modified due to the limitation of the speed of the front vehicle on the motion planning of the ego vehicle.

[0156] The vehicle prediction state is in the red light period, and the time-varying constraint condition is as follows:

[0157]

[0158] Since the distance between the front and rear vehicles is not large, the case that the front vehicle passes the intersection and the ego vehicle cannot pass the intersection by stopping is not considered, and the front vehicle adopts acceleration driving and can certainly pass the intersection in time.

[0159] Therefore, the prediction state is in the green light period, and the time threshold t judge is no longer set according to the size of the remaining green light time to distinguish, and the time-varying constraint condition is as follows:

[0160]

[0161] For the ego vehicle, when the vehicle starts to accelerate, the maximum driving speed must not exceed that of the front vehicle while ensuring that the ego vehicle can pass the intersection within the green light period.

[0162] The comprehensive target function and constraint condition are as follows:

[0163]

[0164] Step three, design the tracking control layer

[0165] In the present application, the tracking control layer tracks the local dynamic planning trajectory passed from the upper layer, needs to model the vehicle, considers the driving safety, ride comfort and the like to design the cost function and constraint of the corresponding control target, and uses the model predictive control algorithm to solve the problem.

[0166] Vehicle dynamics model design:

[0167] To make the vehicle motion tracking reference more accurate, a vehicle dynamics model needs to be established, and then a target function for realizing longitudinal path tracking is established according to the constraint condition.

[0168] Here, the vehicle dynamics model considering vehicle yaw and slip is established in the global coordinate system as follows:

[0169]

[0170] In the above formula, m is the mass of the vehicle, is the yaw rate of the vehicle, δ f is the front wheel steering angle of the vehicle, I z is the moment of inertia, l f , l r are the distances from the vehicle mass center to the front and rear axles, respectively, F yf , F yr are the resultant forces of the tire lateral forces on the front and rear axles of the vehicle, respectively, F xf , F xr are the resultant forces of the tire longitudinal forces on the front and rear axles of the vehicle, respectively.

[0171] During driving, the front wheel steering angle and the tire side slip angle of the vehicle are a small angle change process, so the lateral force expression is as follows:

[0172]

[0173] where C αf and C αr represent the side slip stiffness of the front and rear tires, β is the mass center side slip angle, and generally

[0174] Similarly, the longitudinal force expression of the vehicle is as follows:

[0175]

[0176] where C lf , C lr are the longitudinal stiffness of the front and rear tires, and λf, λr are the front and rear tire slip rates.

[0177] The system is simply described as:

[0178]

[0179] The state quantity is The control quantity is u = [a x , δ f ], that is, the longitudinal acceleration.

[0180] 2. MPC tracking trajectory controller design:

[0181] One of the advantages of choosing a model predictive controller is that it can efficiently handle multi-objective optimization problems with constraints. In the design of the tracking controller in the present application, the constraints considered include state quantity safety constraints and control quantity constraints.

[0182] First, the intelligent driving car must meet the speed requirements limited by the traffic regulations of the section, so there are the following vehicle speed limits:

[0183] 0 ≤ v x (k) ≤ v rule (17)

[0184] Secondly, there are also control quantity constraints. In order to avoid excessive acceleration of the vehicle and cause passengers to have a terrible ride experience or even endanger their lives, the size and increment of the longitudinal acceleration of the vehicle need to be limited:

[0185]

[0186] In summary, the objective function of the designed tracking reference trajectory is as follows:

[0187]

[0188] wherein, The first term of the objective function is the deviation between the state of the ego vehicle and the local planning reference information generated by the planning layer, and the second term indicates the change value of the control value, reflecting the need for smooth increase and decrease of the control quantity to avoid large fluctuations in the control quantity, affecting comfort and smoothness, and indicates the control quantity, which is the hope of using the smallest control quantity. Q1, S1, and S2 are weights.

[0189] Table 3 MPC tracking controller simulation experiment parameter table

[0190] Symbols Definitions Values / Units <![CDATA[N p ]]> Tracking layer MPC prediction step 15 <![CDATA[N c ]]> Tracking layer MPC control step 2 q x ]]> Position tracking weight 600 q v ]]> Velocity tracking weight 500 s du ]]> Control increment weight 100 s u ]]> Control weight 150

[0191] Simulation verification

[0192] To verify the feasibility of the algorithm, MATLAB / Simulink and CarSim are used for joint simulation analysis. Here, simulation verification is made in two cases: self-vehicle passing through the intersection and self-vehicle passing through the intersection with the influence of the front vehicle.

[0193] In the two scenarios, the initial position of the self-vehicle is set to (0, 0), and the initial position of the front vehicle is set to (40, 0) in the scenario where the front vehicle exists, and the front vehicle moves forward at a constant speed.

[0194] To reflect the approach of the vehicle to the intersection in different cycle time periods of the traffic light, the position of the traffic light is changed to construct the scenario in the simulation process.

[0195] Five different scenarios are set in the simulation verification link, namely, scenarios A, B and C considering only the self-vehicle passing through the intersection, and scenarios D and E considering the influence of the front vehicle on the self-vehicle passing through the intersection, and the relevant simulation results are Figs. 6(a)(b)(c), 7(a)(b)(c), 8(a)(b)(c), 9(a)(b)(c) and 10(a)(b)(c) in turn.

[0196] In scenario A (see Figs. 6(a)(b)(c) for details), when the self-vehicle approaches the intersection, it is in the green light period at this time, and it is assumed that the vehicle travels to the intersection at the original global reference speed of 10 m / s, which is in the red light period. Even if the vehicle maintains the original driving speed, it will still be in this red light period when it travels to the intersection, and it needs to be appropriately accelerated to pass through the intersection within the limited remaining green light time, and then restored to the original global reference speed of 10 m / s after passing through the intersection.

[0197] In scenario B (see Figs. 7(a)(b)(c) for details), when the self-vehicle approaches the intersection, it is in the red light period at this time, and it is assumed that the vehicle travels to the intersection at the original global reference speed of 10 m / s, which is still at the tail end of the red light period. To avoid unnecessary start and stop, the vehicle needs to be appropriately decelerated so that the vehicle arrives at the intersection just in the green light period, and then the vehicle is restored to the original global reference speed of 10 m / s after passing through the intersection.

[0198] In scenario C (see Figs. 8(a)(b)(c) for details), when the self-vehicle approaches the intersection, it is at the end of the green light period, and it is assumed that the vehicle travels to the intersection at the original global reference speed of 10 m / s, which is in the red light period. Since the remaining green light time is short, even if the vehicle travels at the maximum speed on the road, it still cannot pass through the intersection, so it travels to the intersection and stops waiting, and then the vehicle starts again when the next green light period arrives, and is restored to the original global reference speed of 10 m / s.

[0199] In scenario D (see Figs. 9(a)(b)(c) for details), the preceding vehicle approaches the intersection at a speed of 7.8 m / s, and cannot pass through the green light period if the speed is maintained. Since the initial distance between the two vehicles is small, the ego vehicle starts to decelerate from the global reference speed of 10 m / s to the speed of the preceding vehicle for following, and stops when the preceding vehicle stops, waiting for the next green light period. The two vehicles start and pass through the intersection, and the ego vehicle continues to maintain the following speed due to the obstruction of the preceding vehicle.

[0200] In scenario E (see Figs. 10(a)(b)(c) for details), the original global planning speed of the ego vehicle is 15 m / s, which will encounter a red light period at the intersection. In addition, the preceding vehicle approaches the intersection at a constant speed of 7.8 m / s, and can pass through the green light period, and there is enough remaining green light time. Under the influence of the two factors, the ego vehicle decelerates according to the constraint range. Since the ego vehicle experiences two different signal periods during the approach to the intersection, the constraints are different, so the planning speed adopted in the road section is different. Finally, the ego vehicle can pass through the intersection, and after passing through the intersection, the same speed following is implemented to ensure the traffic efficiency.

[0201] The simulation results of the above several scenarios fully prove the effectiveness of the intersection motion planning design in the present application. Under the influence of the preceding vehicle or not, the ego vehicle can maximize the efficiency of passing through the intersection without violating the traffic signal indication, and stop behind the stop line in time and normally drive through the intersection.

Claims

1. A method for intelligent vehicle motion planning and control at traffic light signalized intersections, characterized in that: The steps are as follows: S1. Design of vehicle motion planning layer for traffic signal intersections. S11. Establish the vehicle kinematics model in the global coordinate system. Where v is the vehicle speed. For the vehicle's heading angle, δ f is the front wheel steering angle, L is the wheelbase, and x and y are the longitudinal and lateral position coordinates of the vehicle in the global coordinate system; The simplified expression is: Vehicle status variables are The control variable is [v, δ] f ] T ; S12. Motion planning method considering a vehicle passing through a traffic signal intersection. S121, Speed ​​Limit: 0≤v(k)≤v rule (3) Where v rule This refers to the vehicle's maximum speed. S122. Apply time-varying constraints to vehicle speed v. If it is during a red light cycle, the constraints are as follows: Where S left (k) represents the remaining distance from the vehicle to the intersection, T redleft (k) represents the remaining time of the red light, T green This is the green light cycle; If it is in a green light cycle, a time threshold t needs to be introduced based on the traffic light phase when the vehicle approaches the intersection. judge , X traf For the intersection location, T greleftpre The remaining green light time when the vehicle is in its initial position; S123, when T is satisfied greleftpre >t judge At that time, the vehicle speed is constrained as follows: Where v latest The speed reference value from the previous moment; S124. The following constraints are imposed on Δv: a min T≤Δv(k)≤a max T (6) Among them, a max ,a min The maximum and minimum accelerations of the vehicle are given by T, where T is the sampling period of the planning layer. S125, Regarding the control quantity δ f The constraints are as follows: d fmin <d f (k)<δ fmax (7) Where, δ fmax ,δ fmin The maximum and minimum front wheel steering angles limited for the vehicle; S126. Based on the control requirements of the planning layer, the designed objective function is as follows: Where N p N represents the prediction time domain. c Represents the control time domain, where Q and S are the weight values ​​of each item, and Δu is the control increment; S127. Comprehensive Objective Function and Constraints: st(2)(3)(4)(5)(6)(7) (9) S13. Motion planning method for passing through a traffic signal intersection considering the impact of the vehicle in front. The vehicle is predicted to be in a red light cycle, and the time-varying constraints are as follows: The time-varying constraints are as follows: Combine objective function and constraints: st(2)(3)(6)(7)(10)(11) (12) S2, Design the tracking control layer S21. Vehicle dynamics model: Establish a vehicle dynamics model considering vehicle yaw and slip in the global coordinate system. In the formula, m is the vehicle mass. Let δ be the yaw rate of the vehicle. f I is the front wheel steering angle of the vehicle. z For the moment of inertia, l f l r The distances from the vehicle's center of gravity to the front and rear axles, respectively, are F. yf F yr These are the resultant forces of the tire lateral forces on the front and rear axles of the vehicle, F. xf F xr These are the resultant forces of the longitudinal forces on the tires on the front and rear axles of the vehicle, respectively. The expression for lateral force is as follows: Where C αf and C αr Represents the lateral stiffness of the front and rear tires, and β is the sideslip angle at the center of gravity, which is generally considered to be... The expression for the longitudinal force of the vehicle is: Where C lf C lr λf and λr are the longitudinal stiffness of the front and rear tires, respectively, and the slip ratios of the front and rear tires. In short: State variables are The control quantity is u = [a x δ f ], i.e., longitudinal acceleration; S22, MPC Tracking Controller Speed ​​limit: 0≤v x (k)≤v rule (17) Limitations are imposed on the magnitude and increment of the vehicle's longitudinal acceleration: In summary, the objective function for tracking the reference trajectory is as follows: st(16)(17)(18) (19) in, The first term of the objective function The second term represents the magnitude of the deviation between the vehicle's state and the local planning reference information generated by the planning layer. This indicates the change in the control value. This indicates the magnitude of the control quantity, with Q1, S1, and S2 representing the weights.

Images