Narrow space trajectory planning method based on model predictive control

Abstract

The invention discloses a narrow space trajectory planning method based on model predictive control, and belongs to the technical field of intelligent automobile automatic driving trajectory planning. According to the narrow space trajectory planning method based on model predictive control, the distance between vehicles is converted into the distance between rectangles, the minimum safe distance constraint is added, and the expected trajectory is obtained by solving the optimization problem. The method comprises the steps of building a vehicle model, designing an accurate obstacle avoidance MPC trajectory planner, and designing a variable prediction time domain and variable control time domain method. According to the method, a reasonable control time domain is selected through an event triggering mechanism, and open-loop control is performed on the vehicle when certain conditions are met, so that the designed method can achieve the purpose of reducing calculation burden by reducing planning tracking times.

Description

Technical Field

[0001] The present invention belongs to the technical field of intelligent vehicle automatic driving trajectory planning. Background Art

[0002] In recent years, academic institutions and autonomous driving companies have conducted a lot of research on motion planning. Most of the methods used are derived from the field of mobile robots, and targeted improvements and optimizations are made for autonomous driving scenarios, resulting in many representative classic algorithms and planning strategies. The motion planning algorithms currently used in the field of autonomous driving mainly include graph search-based algorithms, curve interpolation algorithms, sampling-based algorithms, and numerical optimization algorithms. At the same time, methods such as reinforcement learning are increasingly being used in the fields of motion planning and trajectory planning. Among them, the trajectory planning method based on graph search can obtain a path from the starting point to the end point by traversing in space, but this method is usually used for path planning where the trajectory does not contain time information; curve interpolation refers to the process of constructing and inserting a set of new data points in a set of known reference points. The planning method based on curve interpolation requires a set of pre-generated reference points; the sampling algorithm is divided into deterministic sampling and random sampling, and it is iteratively expanded by adding new configuration nodes to finally generate the desired trajectory; the numerical optimization algorithm is based on the mechanism model to construct the optimization problem, and obtains the final desired trajectory by solving it. It has been widely studied because of its ability to handle multi-objective optimization and multi-constraint problems. The present invention studies the trajectory planning method based on the numerical optimization algorithm.

[0003] After investigation and analysis, the existing trajectory planning methods usually have the following problems: 1. In the process of trajectory planning, when considering the need for obstacle avoidance, most trajectory planning methods consider the approximate shapes of the ego vehicle and the obstacle vehicle, such as expanding the shape of the obstacle to form a safe redundant space. However, this solution ignores the need for obstacle avoidance in a small space. When the vehicle is in a small space, a more precise trajectory is required to complete the obstacle avoidance process.

[0004] 2. Traditional MPC-based trajectory planning methods usually use a fixed time domain. For a fixed prediction time domain, if the prediction time domain is too short, the dynamic prediction is insufficient; if the prediction time domain is too long, the prediction is inaccurate due to model mismatch and disturbance; for a fixed control time domain, if the selected control time domain is too short, it will cause a waste of computing resources, and if the control time domain is too long, the computing burden will increase. Therefore, it is necessary to design a reasonable prediction time domain and control time domain method. Summary of the invention

[0005] The purpose of the present invention is to convert the distance between vehicles into the distance between rectangles, add a minimum safety distance constraint, and obtain a narrow space trajectory planning method based on model predictive control by solving the optimization problem to obtain the desired trajectory.

[0006] The steps of the present invention are: S1. Design an MPC trajectory planner for accurate obstacle avoidance S11. Design of an MPC trajectory planner for accurate obstacle avoidance Among them, v x is the longitudinal velocity of the vehicle, v y is the vehicle lateral velocity, is the vehicle heading angle, X and Y are the longitudinal and lateral position coordinates of the vehicle in the global coordinate system respectively; S12. Create a rectangular expression to represent the car: P a (t) = {[X a ,Y a ] T |A a (t)[X a ,Y a ] T ≤ b a} Where ω is the width of the vehicle, l f ,l r Respectively represent the front and rear wheelbase of the vehicle, Indicates the heading angle of the vehicle; Any given vehicle position is a rectangle at a specific time, and the relative position between two rectangles is expressed as follows: The subscript a represents the ego vehicle, and k represents the kth adjacent vehicle; Using strong duality theory transformation, the above equation is transformed into the following dual problem: Among them, λ,μ,s are all dual variables; Add minimum safety distance constraint Convert the above formula into: Finally, through transformation, the collision avoidance constraints required to achieve obstacle avoidance are obtained: Where i represents the i-th prediction point in MPC, and i∈{1,2,...,N}, and k represents the adjacent k-th vehicle; Combined with the collision avoidance constraint, we get the complete constrained optimization problem: S2. Variable prediction time domain design and event trigger mechanism design S21. Virtual vehicle VT design The speed of VT is designed as: where v ref is the reference speed of the vehicle, E v is the difference between the vehicle speed and the reference speed, δ and δ' are two constants; the position of VT is designed as: in is the initial position of VT; S22. Design cost function in v k is the speed of the vehicle at time k, It represents the difference between the terminal state of the predicted trajectory of the ego vehicle and VT; S23, variable time domain process design Predict the time domain for the current k moment: Among them, l 1 represents the closest point from the vehicle to the reference trajectory, l 2 Represents the distance from the nearest point to the virtual vehicle along the reference trajectory; the prediction time domain for the next moment: Among them, δ is an adjustable parameter, sgn(m) is a sign function, and D is the distance between the terminal of the predicted trajectory of the ego vehicle and the virtual vehicle; S24, variable control time domain design When the terminal state of the predicted trajectory of the ego vehicle is within the virtual vehicle’s d min When the range is reached, the first m control commands are used from the current k The vehicle is controlled in an open-loop manner from the moment , and the control time domain m' for determining the vehicle position is selected as follows: in represents the position of the vehicle at the time of m' control commands; At time k, the cumulative error is proposed: in Indicates the actual running position of the vehicle. represents the position of the planned trajectory of the ego vehicle, and m' is the control time domain for judging the vehicle tracking error; Therefore, the selection method of the first m control commands is: m=min{m',m",N c} (15) Among them, N c It is the control time domain used in the tracking layer.

[0007] The beneficial effects of the present invention are: 1. The present invention describes the shape of the vehicle as a rectangle of a certain size, adds a minimum safety distance constraint, and can achieve the purpose of accurate obstacle avoidance in a small space by solving the optimization problem with constraints; 2. Compared with the existing MPC method using a fixed test time domain, the designed adaptive prediction time domain method can dynamically adjust the prediction time domain through calculation, ensuring the dynamic prediction effect while ensuring sufficient prediction accuracy; 3. The designed adaptive control time domain can select a reasonable control time domain through an event trigger mechanism while ensuring vehicle safety, and perform open-loop control of the vehicle when certain conditions are met. Therefore, the designed method can reduce the computational burden by reducing the number of planning and tracking times. BRIEF DESCRIPTION OF THE DRAWINGS

[0008] Figure 1 It is the overall control block diagram of the method of the present invention; Figure 2 It is a schematic diagram of the variable prediction time domain; Figure 3 It is a schematic diagram of the variable control time domain; Figure 4 It is the vehicle motion trajectory diagram in simulation scenario 1; Figure 5 It is the vehicle heading angle change diagram in simulation scenario 1; Figure 6 It is the change diagram of the vehicle lateral speed in simulation scenario 1; Figure 7 This is the time-domain effect diagram in simulation scenario 1; Figure 8 It is the vehicle motion trajectory diagram in simulation scenario 2; Fig. 9 It is the vehicle heading angle change diagram in simulation scenario 2; Fig.10 It is the change diagram of the vehicle lateral speed in simulation scenario 2; Fig.11 This is the time-domain effect diagram in simulation scenario 2; Fig.12 It is the vehicle motion trajectory diagram in simulation scenario three; Fig.13 It is the vehicle heading angle change diagram in simulation scenario three; Fig.14 It is the change diagram of the vehicle lateral speed in simulation scenario three; Fig.15 This is the time domain effect diagram of simulation scenario three. DETAILED DESCRIPTION

[0009] The present invention proposes a trajectory planning method for achieving precise obstacle avoidance based on MPC. In view of the planning requirements in a small space, the shape of the vehicle is described as a rectangle consistent with the actual size of the vehicle, the distance between vehicles is converted into the distance between rectangles, and a minimum safety distance constraint is added. The desired trajectory is obtained by solving the optimization problem. At the same time, in order to solve the problem of heavy computational burden, a virtual front vehicle running along the reference trajectory is designed. The self-vehicle tracks the virtual front vehicle while tracking the reference trajectory, and the change in the predicted time domain is adaptively adjusted according to the distance between the two vehicles. In addition, an event triggering mechanism of the trajectory planner is designed to reduce the computational burden by reducing the number of planning times.

[0010] The research method of the present invention comprises the following steps: Step 1: Build a vehicle model and use the Carsim simulation platform to provide real-time vehicle status information feedback.

[0011] Step 2: Design an MPC trajectory planner for accurate obstacle avoidance: 2.1) Establish vehicle kinematic model according to the requirements of motion trajectory planning; 2.2) The vehicle shape is represented by an inequality, and the expression of the distance between the two vehicles is obtained through the minimum problem. The distance expression is transformed into a feasible solution problem with constraints through the strong duality theorem. Then, the minimum safe distance constraint is added to obtain the final constraint required by the obstacle avoidance MPC. Solving this optimization problem can obtain the trajectory that achieves accurate obstacle avoidance.

[0012] Step 3: Design of variable prediction time domain and variable control time domain method 3.1) Design a virtual front vehicle VT running along the global reference trajectory, model it as a point mass model, and design its speed and position respectively; 3.2) Design a cost function so that the ego vehicle can approach the VT smoothly and effectively while tracking the reference trajectory; 3.3) Design the variable time domain process through the position relationship between the ego vehicle and VT; 3.4) To ensure safety, the deviation between the vehicle and the reference trajectory and the predicted trajectory is calculated in real time. By setting judgment conditions, the event trigger mechanism is implemented to reduce the number of planning times and thus reduce the computational burden.

[0013] The present invention proposes a trajectory planning method based on MPC to achieve accurate obstacle avoidance in a small space. Figure 1As shown in the figure, the overall framework is divided into two layers, the upper layer is the trajectory planning layer, which takes the global trajectory as the reference trajectory and introduces the minimum safety distance constraint to perform local trajectory planning; the lower layer is the trajectory tracking layer, which receives the local trajectory planned by the planning layer, tracks it as the reference trajectory, and then passes the final control amount to the vehicle for control.

[0014] The goal of the present invention is to achieve trajectory planning that can accurately avoid obstacles in a small space while reducing the computational burden.

[0015] According to the above operating principles and operating processes, a model for joint simulation was built. The building process and operating process are as follows: 1. Software selection and joint simulation settings The MPC trajectory planning controller of the control system and the simulation model of the controlled intelligent driving car in the present invention are respectively built by software MATLAB / Simulink and CarSim, and the software versions are MATLABR2021a and CarSim2019.0 respectively. MATLAB / Simulink programming is used to realize the controller function logic.

[0016] As a platform with high-fidelity vehicle dynamics simulation, CarSim acts as a controlled object in the control logic to simulate the motion process of a real vehicle.

[0017] To realize the joint simulation of the two, first you need to add the path of CarSim in the path setting of MATLAB, add the output interface module in the CarSim interface, add the path of the Simulink file to CarSim, and after running, CarSimS-function will be generated in Simulink. At this time, calling the CarSim module in Simulink can realize the joint simulation of the two. In order to realize the control of CarSim, it is also necessary to add input and output ports in CarSim. The input quantities of CarSim used in the present invention are IMP_STEER_L1, IMP_STEER_R1, IMP_STEER_L2, and IMP_STEER_R2, which are the turning angles of the four wheels. For vehicles with only front wheel steering, the rear wheel steering can be directly set to 0. The output quantity of CarSim is the state of the vehicle, which is Vy, Vx, Yaw, AV_Y, Yo, and Xo respectively. The control method of the present invention is verified by a joint simulation experiment.

[0018] The following describes the specific steps of the trajectory planning method of the present invention for achieving accurate obstacle avoidance in a small space: Step 1: Build an intelligent driving car model and simulation conditions. Use the vehicle model in the simulation software CarSim to replace the real vehicle as the controlled object. CarSim provides accurate and real-time vehicle status information for the planning and control method of the present invention. Select "C-Class" as the simulation vehicle model and modify the required parameters according to actual needs. At the same time, set the road environment and obstacle vehicle status information in CarSim according to the required simulation conditions.

[0019] Step 2: Design an MPC trajectory planner for accurate obstacle avoidance 2.1) Establish a vehicle kinematic model. The selection of the vehicle model needs to comprehensively consider factors such as performance and computational complexity. Since the planning algorithm itself has a large amount of computation, and the main purpose of introducing model predictive control is to make the planning results meet the constraints of vehicle dynamics and kinematic models, in order to reduce the amount of computation, it is not advisable to use a too complex model.

[0020] The present invention adopts the point quality model as the planning layer model, and its equation is as follows: Among them, v x is the longitudinal velocity of the vehicle, v y is the vehicle lateral velocity, is the vehicle heading angle, and X and Y are the longitudinal and lateral position coordinates of the vehicle in the global coordinate system.

[0021] The vehicle state in the trajectory planning layer is: The control amount is a y .

[0022] 2.2) Create a rectangular expression to represent the car: P a (t) = {[X a ,Y a ] T |A a (t)[X a ,Y a ] T ≤ b a} Where ω is the width of the vehicle, l f ,l r Respectively represent the front and rear wheelbase of the vehicle, Indicates the heading angle of the vehicle.

[0023] Any given vehicle position can be represented as a rectangle at a specific time. This transformation allows the relative position of the two vehicles to be taken into account, ensuring that a safe distance is always maintained. The relative position between the two rectangles is represented by The subscript “a” represents the ego vehicle, and “k” represents the kth adjacent vehicle.

[0024] The introduction of the above formula results in a new cost function that is incompatible with the cost constraint framework of MPC. Therefore, in order to integrate formula (3) into the MPC framework, the above optimization problem can be reformulated as a feasible solution problem constrained by a set of inequalities. The above formula is transformed into the following dual problem using the strong duality theory: Among them, λ,μ,s are all dual variables.

[0025] In order to ensure collision avoidance, a minimum safe distance constraint must be added Therefore, the above formula can be transformed into the following feasible solution problem:

[0026] Finally, through transformation, the collision avoidance constraints required to achieve obstacle avoidance can be obtained: Where i represents the i-th prediction point in MPC, and i∈{1,2,...,N}, and k represents the adjacent k-th vehicle.

[0027] Combined with the collision avoidance constraint, the complete constrained optimization problem can be described as follows:

[0028] In summary, by solving the optimization problem, the desired local planning trajectory can be obtained, and then by passing the local trajectory to the tracking layer for trajectory tracking, the task of avoiding obstacles in a small space can be achieved.

[0029] Step 3: Design of variable prediction time domain and event trigger mechanism 3.1) Virtual Vehicle (VT) Design For ease of calculation, VT is modeled as a point mass located at As the time step k changes, the position of VT is determined by a specially designed virtual velocity V vt To be determined dynamically.

[0030] The speed of VT is designed as: where v ref is the reference speed of the vehicle, E v is the difference between the vehicle speed and the reference speed, δ and δ' are two constants.

[0031] The location of VT is designed as: in is the initial position of VT.

[0032] It should be noted that VT always moves in front of the ego vehicle along the global trajectory. When the ego vehicle approaches VT, VT is moved forward a certain distance along the global trajectory to keep the ego vehicle tracking VT.

[0033] 3.2) In order to improve the smoothness of the generated local trajectory and velocity profile, the cost function is designed as follows: in v k is the speed of the vehicle at time k. It represents the difference between the terminal state of the predicted trajectory of the ego vehicle and VT. This cost function can make the ego vehicle approach VT smoothly and effectively, while gradually tracking the reference path.

[0034] 3.3) Variable time domain process design The prediction time domain is designed to adapt to the relative position of the ego vehicle and the virtual vehicle. The first step is to guess the prediction time domain at the current k moment: Among them, Figure 2 As shown, 1 represents the closest point from the vehicle to the reference trajectory, l 2 It represents the distance from the nearest point to the virtual vehicle along the reference trajectory. Based on this formula, the prediction time domain required to travel from the vehicle to the virtual vehicle can be estimated.

[0035] After obtaining the prediction time domain at the current moment, it is necessary to calculate the prediction time domain at the next moment. The prediction time domain at the next moment is determined by the relationship between the predicted state of the vehicle terminal and the position of the virtual vehicle. The calculation is as follows: Among them, δ is an adjustable parameter. When the distance between the ego vehicle and the virtual vehicle and the distance between the ego vehicle's predicted trajectory terminal position and the virtual vehicle are both greater than d min When the distance between the self-vehicle and the virtual vehicle is greater than d min But the distance between the predicted trajectory terminal of the ego vehicle and the virtual vehicle is less than d min , δ is the setting parameter, sgn(m) is the sign function, when m = X-VT x When >0, the item is -1, m=X-VT x When the distance between the ego vehicle and the virtual vehicle is less than d min When , formula 12 is used to re-estimate the predicted time domain value at time k+1.

[0036] Through the above calculations, the process of predicting the time domain changes in the process of the ego-vehicle tracking the reference trajectory can be realized, and the prediction time domain used in the trajectory planning process can be reasonably estimated, thereby reducing the computational burden accordingly.

[0037] 3.4) Variable control time domain design Designing an event trigger mechanism to achieve adaptive changes in the control time domain can reduce the computational burden while ensuring that the vehicle stably tracks the reference trajectory. Usually, the trajectory tracking layer accepts the reference trajectory obtained by the trajectory planning layer, and obtains the control sequence at time k as follows: The first value of the control sequence is used for control, and the designed event trigger mechanism can use the trajectory obtained by the trajectory planning layer at time k as a reference for subsequent moments when certain conditions are met, and use the control sequence obtained by the tracking layer at time k as the subsequent control input, that is, the vehicle is controlled in an open-loop manner when certain conditions are met. This part of the design is based on the virtual vehicle designed in the second work content.

[0038] like Figure 3 As shown in the figure, when the terminal state of the predicted trajectory of the ego vehicle is within the virtual vehicle’s d min When the vehicle is within the range of k, the event trigger method is used, and the vehicle is controlled in an open-loop manner from the current time k by using the first m control commands. Figure 2 The predicted trajectory of the vehicle in becomes the trajectory to be tracked by the trajectory tracking layer, and the control time domain m' for determining the vehicle position is selected as follows: in represents the position of the ego vehicle at m' control commands. This formula indicates that the ego vehicle position should avoid entering the virtual vehicle's d in the open-loop tracking process. min within the range.

[0039] In the open-loop process, it is also necessary to avoid excessive cumulative errors in the tracking process that may lead to deviation from the expected trajectory. Therefore, at time k, a cumulative error calculation method is proposed: in Indicates the actual running position of the vehicle. represents the position of the planned trajectory of the ego vehicle, m' is the control time domain for judging the vehicle tracking error, and this formula represents the accumulated error of the ego vehicle when there are m' control commands. When the accumulated error When it is greater than a given value, open-loop control will no longer be used but closed-loop control will be used.

[0040] Therefore, the selection method of the first m control commands finally adopted is: m=min{m',m",N c} (15) Among them, N c It is the control time domain used in the tracking layer.

[0041] In addition, in the open-loop process, in addition to considering the cumulative error with the planned trajectory, the deviation from the reference trajectory must also be considered. At the same time, when an obstacle vehicle appears within a certain range, in order to ensure safety, the open-loop control should be ended and closed-loop control should be adopted.

[0042] Simulation Verification In order to verify the effectiveness of the proposed obstacle avoidance method, we conducted simulation verification in three scenarios: In the first scenario, if Figure 4 As shown in the figure, we set the obstacle size to 5m long and 2m wide. The ego vehicle starts from the starting position (0,0) with a speed of 10m / s. The obstacle vehicle starts from the position (50,0) and moves forward along the right lane with a speed of 5m / s. The global reference trajectory is set to the center line of the right lane. Figure 4 It can be clearly seen that at 100m, both schemes can achieve obstacle avoidance effect, but the obstacle avoidance time of the APF-based method is longer. The proposed precise obstacle avoidance algorithm minimizes the time consumed by the obstacle avoidance process while ensuring no collision. Figure 5 , Figure 6 They respectively represent the vehicle heading angle and lateral speed during the obstacle avoidance process using the two algorithms. Figure 7 It shows the time-domain process we designed, where the red part is the open loop.

[0043] In the second scenario, Figure 8 As shown in the figure, we set the obstacle size to 9m long and 4m wide. The ego vehicle starts from the starting position (0,0) with a speed of 10m / s. The obstacle vehicle starts from the starting position (50,0) and moves forward along the right lane with a speed of 5m / s. The global reference trajectory is set to the center line of the right lane. Figure 8 It can be seen that when encountering large obstacles, both the potential field method and the precise obstacle avoidance method are more effective than Figure 4 However, in this scenario, the precise obstacle avoidance method starts to avoid obstacles at around 70m, while the potential field method starts to avoid obstacles at around 55m, which takes longer. Fig. 9 The heading angle changes and Fig.10 The lateral speed changes in the figure show that the algorithm we designed can avoid obstacles in a shorter time. Fig.11 The time-domain process for design.

[0044] In the third scenario, Fig.12As shown in the figure, in order to verify that the obstacle avoidance effect can be achieved in a small space, we first set up an obstacle vehicle with a length of 9m and a width of 4m, from the starting position (50,0) to the end position (100,0). When the obstacle vehicle reaches the end position, another obstacle vehicle with a length of 5m and a width of 2m is set at the left lane (110,4). The speed of the obstacle vehicles is 5m / s. The results are as follows Fig.12 As shown in the figure, we divide the results into three stages. In the first stage, when the obstacle car runs to (100,0), the position of the car in the two obstacle avoidance algorithms is the lightest position in the figure. Figure 8 The situation is the same as in the second stage. In the second stage, a new obstacle will suddenly appear in front of the ego vehicle. At this time, both algorithms will avoid the obstacle in advance. Among them, the ego vehicle in the precise obstacle avoidance algorithm will return to the lane where the reference trajectory is located earlier to avoid the obstacle in front. It can be seen in the figure that the obstacle avoidance can be completed in the end. However, even if the ego vehicle in the potential field method takes the obstacle vehicle into consideration, it will collide with the obstacle vehicle due to understeering. At the same time, when the ego vehicle returns to the reference trajectory, it is still affected by the potential field of the obstacle vehicle and oversteers, causing it to run out of the lane. Fig.13 In both algorithms shown in Figure 1, the vehicle moves at a larger heading at 10 seconds to avoid obstacles. In contrast, the algorithm we designed has a shorter change time and a smaller change rate. Fig.14 Displays the lateral speed of the vehicle. Fig.15 This is the time domain rendering designed for us.

[0045] The present invention relates to a method for narrow space trajectory planning based on model predictive control (MPC). In order to meet the demand for more accurate trajectory planning in narrow spaces, the vehicle shape is concretized into a rectangle of corresponding size, the minimum distance between vehicles is obtained by calculating the distance between rectangles, and then the minimum distance is converted into a minimum safety distance constraint, and a trajectory that can achieve accurate obstacle avoidance is planned using the model predictive control method.

Claims

1. A method for trajectory planning in a narrow space based on model predictive control, characterized in that: The steps are: S1. Design an MPC trajectory planner for accurate obstacle avoidance S11. Design of an MPC trajectory planner for accurate obstacle avoidance Among them, v x is the vehicle longitudinal velocity, v y is the vehicle lateral velocity, is the vehicle heading angle, X and Y are the longitudinal and lateral position coordinates of the vehicle in the global coordinate system respectively; S12. Create a rectangular expression to represent the car: P a (t)={[X a ,Y a ] T |A a (t)[X a ,Y a ] T ≤b a } Where ω is the width of the vehicle, l f ,l r Respectively represent the front and rear wheelbase of the vehicle, Indicates the heading angle of the vehicle; Any given vehicle position is a rectangle at a specific time, and the relative position between two rectangles is expressed as follows: The subscript a represents the ego vehicle, and k represents the kth adjacent vehicle; Using strong duality theory transformation, the above equation is transformed into the following dual problem: Among them, λ,μ,s are all dual variables; Add minimum safety distance constraint Convert the above formula into: Finally, through transformation, the collision avoidance constraints required to achieve obstacle avoidance are obtained: Where i represents the i-th prediction point in MPC, and i∈{1,2,...,N}, and k represents the adjacent k-th vehicle; Combined with the collision avoidance constraint, we get the complete constrained optimization problem: st(6) S2. Variable prediction time domain design and event trigger mechanism design S21. Virtual vehicle VT design The speed of VT is designed as: where v ref is the reference speed of the vehicle, E v is the difference between the vehicle speed and the reference speed, δ and δ' are two constants; The location of VT is designed as: in is the initial position of VT; S22. Design cost function in v k is the speed of the vehicle at time k, It represents the difference between the terminal state of the predicted trajectory of the ego vehicle and VT; S23, variable time domain process design Predict the time domain for the current k moment: Among them, l1 represents the closest point from the vehicle to the reference trajectory, and l2 represents the distance from the closest point to the virtual vehicle along the reference trajectory; the prediction time domain for the next moment is: Among them, δ is an adjustable parameter, sgn(m) is a sign function, and D is the distance between the terminal of the predicted trajectory of the ego vehicle and the virtual vehicle; S24, variable control time domain design When the terminal state of the predicted trajectory of the ego vehicle is within the virtual vehicle’s d min When the vehicle is within the range, the vehicle is controlled in an open-loop manner starting from the current time k by using the first m control commands. The selection method of the control time domain m′ for determining the vehicle position is: in represents the position of the ego vehicle at the time of m′ control commands; At time k, the cumulative error is proposed: in Indicates the actual running position of the vehicle. represents the position of the planned trajectory of the ego vehicle, and m″ is the control time domain for judging the vehicle tracking error; Therefore, the selection method of the first m control commands is: m=min{m',m”,N c } (15) Among them, N c It is the control time domain used in the tracking layer.

Images